diff --git a/include/mc_control/ControllerClient.h b/include/mc_control/ControllerClient.h index 6128cd5cde..ac969a23dd 100644 --- a/include/mc_control/ControllerClient.h +++ b/include/mc_control/ControllerClient.h @@ -461,6 +461,11 @@ struct MC_CONTROL_CLIENT_DLLAPI ControllerClient default_impl("Robot", id); } + virtual void robot_msg(const ElementId & id, const mc_rtc::gui::RobotMsgData & /*msg*/) + { + default_impl("RobotMsg", id); + } + /** Should display the visual element \p visual at the position \p pose */ virtual void visual(const ElementId & id, [[maybe_unused]] const rbd::parsers::Visual & visual, diff --git a/include/mc_rtc/gui/RobotMsg.h b/include/mc_rtc/gui/RobotMsg.h new file mode 100644 index 0000000000..36643b2d7d --- /dev/null +++ b/include/mc_rtc/gui/RobotMsg.h @@ -0,0 +1,79 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include +#include + +#include + +namespace mc_rtc::gui +{ + +namespace details +{ + +/** Robot should display a robot model in the environment + * + * The element provides the following data to the client: + * - the parameters passed to RobotLoader to get the RobotModule (vector) + * - the current robot configuration (vector>) + * + * \tparam GetT Should return an mc_rbdyn::Robot + */ +template +struct RobotMsgImpl : public Element +{ + static constexpr auto type = Elements::RobotMsg; + + RobotMsgImpl(const std::string & name, GetT get_fn) : Element(name), get_fn_(get_fn) + { + static_assert(CheckReturnType::value, "Robot element must return an mc_rbdyn::Robot"); + } + + static constexpr size_t write_size() { return Element::write_size() + 7; } + + void write(mc_rtc::MessagePackBuilder & builder) + { + const mc_rbdyn::Robot & robot = get_fn_(); + update(robot); + Element::write(builder); + builder.write(robot.module().parameters()); + builder.write(msg_.q); + builder.write(msg_.alpha); + builder.write(msg_.alphaD); + builder.write(msg_.tau); + builder.write(robot.mbc().force); + builder.write(robot.posW()); + } + +private: + GetT get_fn_; + RobotMsgData msg_; + + void update(const mc_rbdyn::Robot & robot) + { + msg_.q.resize(robot.mb().nrParams()); + rbd::paramToVector(robot.mbc().q, msg_.q); + msg_.alpha.resize(robot.mb().nrDof()); + rbd::paramToVector(robot.mbc().alpha, msg_.alpha); + msg_.alphaD.resize(msg_.alpha.size()); + rbd::paramToVector(robot.mbc().alphaD, msg_.alphaD); + msg_.tau.resize(msg_.alpha.size()); + rbd::paramToVector(robot.mbc().jointTorque, msg_.tau); + } +}; + +} // namespace details + +/** Helper function to create a RobotImpl */ +template +auto RobotMsg(const std::string & name, GetT get_fn) +{ + return details::RobotMsgImpl(name, get_fn); +} + +} // namespace mc_rtc::gui diff --git a/include/mc_rtc/gui/elements.h b/include/mc_rtc/gui/elements.h index d3895ca921..d7307b74f9 100644 --- a/include/mc_rtc/gui/elements.h +++ b/include/mc_rtc/gui/elements.h @@ -48,7 +48,8 @@ enum class Elements PolyhedronTrianglesList, PolyhedronVerticesTriangles, GenericArray, - OneOf + OneOf, + RobotMsg }; /** Element is the common class for every element's type available in the diff --git a/include/mc_rtc/gui/types.h b/include/mc_rtc/gui/types.h index a2468bb63b..975ae43074 100644 --- a/include/mc_rtc/gui/types.h +++ b/include/mc_rtc/gui/types.h @@ -476,6 +476,31 @@ struct MC_RTC_GUI_DLLAPI PolyhedronConfig ///< passed along with the triangles bool fixed_vertices_color = true; }; + +/** Provides full information about a robot kinematic and dynamic state */ +struct MC_RTC_GUI_DLLAPI RobotMsgData +{ + RobotMsgData() = default; + RobotMsgData(const std::vector & params, + const Eigen::VectorXd & q, + const Eigen::VectorXd & alpha, + const Eigen::VectorXd & alphaD, + const Eigen::VectorXd & tau, + const std::vector & forces, + const sva::PTransformd & posW) + : parameters(params), q(q), alpha(alpha), alphaD(alphaD), tau(tau), forces(forces), posW(posW) + { + } + + std::vector parameters; + Eigen::VectorXd q; + Eigen::VectorXd alpha; + Eigen::VectorXd alphaD; + Eigen::VectorXd tau; + std::vector forces; + sva::PTransformd posW; +}; + } // namespace gui template<> diff --git a/src/mc_control/ControllerClient.cpp b/src/mc_control/ControllerClient.cpp index 90542d42f2..8fb0269c63 100644 --- a/src/mc_control/ControllerClient.cpp +++ b/src/mc_control/ControllerClient.cpp @@ -375,6 +375,12 @@ void ControllerClient::handle_widget(const ElementId & id, const mc_rtc::Configu case Elements::Robot: robot(id, data[3], data[4], data[5]); break; + case Elements::RobotMsg: + { + mc_rtc::gui::RobotMsgData msg(data[3], data[4], data[5], data[6], data[7], data[8], data[9]); + robot_msg(id, msg); + break; + } case Elements::Visual: if(data[4].size() == 3) {