diff --git a/include/mc_control/ControllerClient.h b/include/mc_control/ControllerClient.h index 3908ee2070..a124810fc6 100644 --- a/include/mc_control/ControllerClient.h +++ b/include/mc_control/ControllerClient.h @@ -50,20 +50,20 @@ struct MC_CONTROL_CLIENT_DLLAPI RobotMsg RobotMsg(RobotMsg &&) = default; RobotMsg(const std::vector & param, - const std::vector> joints, - std::vector> jointsVel, - std::vector> jointsAcc, - std::vector> jointsTorque, + const Eigen::VectorXd joints, + Eigen::VectorXd jointsVel, + Eigen::VectorXd jointsAcc, + Eigen::VectorXd jointsTorque, std::vector extForces, sva::PTransformd X_0_root) : q(joints), alpha(jointsVel), alphaD(jointsAcc), parameters(param), forces(extForces), posW(X_0_root) { } - std::vector> q; - std::vector> alpha; - std::vector> alphaD; - std::vector> tau; + Eigen::VectorXd q; + Eigen::VectorXd alpha; + Eigen::VectorXd alphaD; + Eigen::VectorXd tau; std::vector parameters; std::vector forces; sva::PTransformd posW; @@ -480,7 +480,19 @@ struct MC_CONTROL_CLIENT_DLLAPI ControllerClient /** Should display a robot model, the RobotModule can be created using \p parameters, its configuration is in \q and * its world position is in \p posW */ - virtual void robot(const ElementId & id, const RobotMsg & msg) { default_impl("Robot", id); } + virtual void robot(const ElementId & id, + const std::vector & /*parameters*/, + const std::vector> & /*q*/, + const sva::PTransformd & /*posW*/) + { + default_impl("Robot", id); + } + + virtual void robot_msg(const ElementId & id, + const RobotMsg & /*msg*/) + { + default_impl("RobotMsg", id); + } /** Should display the visual element \p visual at the position \p pose */ virtual void visual(const ElementId & id, diff --git a/include/mc_rtc/gui/Robot.h b/include/mc_rtc/gui/Robot.h index 02b6f04246..bc44b07655 100644 --- a/include/mc_rtc/gui/Robot.h +++ b/include/mc_rtc/gui/Robot.h @@ -34,7 +34,7 @@ struct RobotImpl : public Element static_assert(CheckReturnType::value, "Robot element must return an mc_rbdyn::Robot"); } - static constexpr size_t write_size() { return Element::write_size() + 7; } + static constexpr size_t write_size() { return Element::write_size() + 3; } void write(mc_rtc::MessagePackBuilder & builder) { @@ -42,10 +42,6 @@ struct RobotImpl : public Element Element::write(builder); builder.write(robot.module().parameters()); builder.write(robot.mbc().q); - builder.write(robot.mbc().alpha); - builder.write(robot.mbc().alphaD); - builder.write(robot.mbc().jointTorque); - builder.write(robot.mbc().force); builder.write(robot.posW()); } diff --git a/include/mc_rtc/gui/RobotMsg.h b/include/mc_rtc/gui/RobotMsg.h new file mode 100644 index 0000000000..39a01b5d12 --- /dev/null +++ b/include/mc_rtc/gui/RobotMsg.h @@ -0,0 +1,65 @@ +/* + * 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_(); + Element::write(builder); + builder.write(robot.module().parameters()); + builder.write(rbd::paramToVector(robot.mb(),robot.mbc().q)); + builder.write(rbd::paramToVector(robot.mb(),robot.mbc().alpha)); + builder.write(rbd::paramToVector(robot.mb(),robot.mbc().alphaD)); + builder.write(rbd::paramToVector(robot.mb(),robot.mbc().jointTorque)); + builder.write(robot.mbc().force); + builder.write(robot.posW()); + } + +private: + GetT get_fn_; +}; + +} // 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..85d0d2d216 100644 --- a/include/mc_rtc/gui/elements.h +++ b/include/mc_rtc/gui/elements.h @@ -44,6 +44,7 @@ enum class Elements XYTheta, Table, Robot, + RobotMsg, Visual, PolyhedronTrianglesList, PolyhedronVerticesTriangles, diff --git a/src/mc_control/ControllerClient.cpp b/src/mc_control/ControllerClient.cpp index 68a1734c1c..066583bcac 100644 --- a/src/mc_control/ControllerClient.cpp +++ b/src/mc_control/ControllerClient.cpp @@ -373,9 +373,12 @@ void ControllerClient::handle_widget(const ElementId & id, const mc_rtc::Configu data.at(4, std::vector{})); break; case Elements::Robot: + robot(id, data[3], data[4], data[5]); + break; + case Elements::RobotMsg: { - RobotMsg msg(data[3], data[4], data[5], data[6], data[7], data[8], data[9]); - robot(id, msg); + RobotMsg msg(data[3],data[4],data[5],data[6],data[7],data[8],data[9]); + robot_msg(id, msg); break; } case Elements::Visual: