6 #include <tsid/robots/robot-wrapper.hpp>
7 #include <tsid/utils/stop-watch.hpp>
8 #include <tsid/utils/statistics.hpp>
10 #include <dynamic-graph/factory.h>
11 #include <sot/core/debug.hh>
24 using namespace dg::sot::torque_control;
27 #define PROFILE_PWM_DESIRED_COMPUTATION "Control manager "
28 #define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period "
30 #define INPUT_SIGNALS m_i_maxSIN << m_u_maxSIN << m_tau_maxSIN << m_tauSIN << m_tau_predictedSIN << m_i_measuredSIN
31 #define OUTPUT_SIGNALS m_uSOUT << m_u_safeSOUT
43 ControlManager::ControlManager(
const std::string& name)
47 CONSTRUCT_SIGNAL_IN(tau_predicted,
dynamicgraph::Vector),
51 CONSTRUCT_SIGNAL_OUT(u,
dynamicgraph::Vector, m_i_measuredSIN),
53 m_robot_util(RefVoidRobotUtil()),
54 m_initSucceeded(false),
55 m_emergency_stop_triggered(false),
56 m_is_first_iter(true),
63 docCommandVoid3(
"Initialize the entity.",
"Time period in seconds (double)",
64 "URDF file path (string)",
"Robot reference (string)")));
68 docCommandVoid1(
"Create an input signal with name 'ctrl_x' where x is the specified name.",
72 docCommandVoid0(
"Get a list of all the available control modes.")));
75 docCommandVoid2(
"Set the control mode for a joint.",
76 "(string) joint name",
"(string) control mode")));
78 addCommand(
"getCtrlMode",
80 docCommandVoid1(
"Get the control mode of a joint.",
"(string) joint name")));
82 addCommand(
"resetProfiler",
85 docCommandVoid0(
"Reset the statistics computed by the profiler (print this entity to see them).")));
88 docCommandVoid2(
"Set map for a name to an Id",
"(string) joint name",
89 "(double) joint id")));
91 addCommand(
"setForceNameToForceId",
93 docCommandVoid2(
"Set map from a force sensor name to a force sensor Id",
94 "(string) force sensor name",
"(double) force sensor id")));
96 addCommand(
"setJointLimitsFromId",
98 docCommandVoid3(
"Set the joint limits for a given joint ID",
"(double) joint id",
99 "(double) lower limit",
"(double) uppper limit")));
102 "setForceLimitsFromId",
104 docCommandVoid3(
"Set the force limits for a given force sensor ID",
"(double) force sensor id",
105 "(double) lower limit",
"(double) uppper limit")));
107 addCommand(
"setJointsUrdfToSot",
109 docCommandVoid1(
"Map Joints From URDF to SoT.",
"Vector of integer for mapping")));
111 addCommand(
"setRightFootSoleXYZ",
113 docCommandVoid1(
"Set the right foot sole 3d position.",
"Vector of 3 doubles")));
114 addCommand(
"setRightFootForceSensorXYZ",
116 docCommandVoid1(
"Set the right foot sensor 3d position.",
"Vector of 3 doubles")));
119 docCommandVoid2(
"Set the Frame Name for the Foot Name.",
120 "(string) Foot name",
"(string) Frame name")));
122 docCommandVoid2(
"Set the Frame Name for the Hand Name.",
123 "(string) Hand name",
"(string) Frame name")));
124 addCommand(
"setImuJointName",
126 docCommandVoid1(
"Set the Joint Name wich IMU is attached to.",
"(string) Joint name")));
128 docCommandVoid0(
"Display the current robot util data set.")));
131 docCommandVoid1(
"Set the period used for printing in streaming.",
132 "Print period in seconds (double)")));
134 addCommand(
"setSleepTime",
136 docCommandVoid1(
"Set the time to sleep at every iteration (to slow down simulation).",
137 "Sleep time in seconds (double)")));
140 "addEmergencyStopSIN",
143 docCommandVoid1(
"Add emergency signal input from another entity that can stop the control if necessary.",
144 "(string) signal name : 'emergencyStop_' + name")));
148 if (dt <= 0.0)
return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
152 vector<string> package_dirs;
153 m_robot =
new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
154 std::string localName(robotRef);
155 if (!isNameInRobotUtil(localName)) {
157 SEND_MSG(
"createRobotUtil success\n", MSG_TYPE_INFO);
160 SEND_MSG(
"getRobotUtil success\n", MSG_TYPE_INFO);
164 addCommand(
"getJointsUrdfToSot", makeDirectGetter(*
this, &
m_robot_util->m_dgv_urdf_to_sot,
165 docDirectSetter(
"Display map Joints From URDF to SoT.",
166 "Vector of integer for mapping")));
179 if (!m_initSucceeded) {
180 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u before initialization!");
185 m_is_first_iter =
false;
190 if (s.size() != (Eigen::VectorXd::Index)m_robot_util->m_nbJoints) s.resize(m_robot_util->m_nbJoints);
195 for (
unsigned int i = 0; i < m_ctrlInputsSIN.size(); i++) (*m_ctrlInputsSIN[i])(iter);
197 int cm_id, cm_id_prev;
198 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
199 cm_id = m_jointCtrlModes_current[i].id;
201 SEND_MSG(
"You forgot to set the control mode of joint " + toString(i), MSG_TYPE_ERROR_STREAM);
205 const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
206 assert(ctrl.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
208 if (m_jointCtrlModesCountDown[i] == 0)
211 cm_id_prev = m_jointCtrlModes_previous[i].id;
212 const dynamicgraph::Vector& ctrl_prev = (*m_ctrlInputsSIN[cm_id_prev])(iter);
213 assert(ctrl_prev.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
218 s(i) = alpha * ctrl_prev(i) + (1 - alpha) * ctrl(i);
219 m_jointCtrlModesCountDown[i]--;
221 if (m_jointCtrlModesCountDown[i] == 0) {
223 "Joint " + toString(i) +
" changed ctrl mode from " + toString(cm_id_prev) +
" to " + toString(cm_id),
225 updateJointCtrlModesOutputSignal();
232 usleep(
static_cast<unsigned int>(1e6 * m_sleep_time));
233 if (m_sleep_time >= 0.1) {
234 for (
unsigned int i = 0; i < m_ctrlInputsSIN.size(); i++) {
235 const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[i])(iter);
236 SEND_MSG(toString(iter) +
") tau =" + toString(ctrl, 1, 4,
" ") + m_ctrlModes[i], MSG_TYPE_ERROR);
244 if (!m_initSucceeded) {
245 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u_safe before initialization!");
249 const dynamicgraph::Vector& u = m_uSOUT(iter);
250 const dynamicgraph::Vector& tau_max = m_tau_maxSIN(iter);
251 const dynamicgraph::Vector& ctrl_max = m_u_maxSIN(iter);
252 const dynamicgraph::Vector& i_max = m_i_maxSIN(iter);
253 const dynamicgraph::Vector& tau = m_tauSIN(iter);
254 const dynamicgraph::Vector& i_real = m_i_measuredSIN(iter);
255 const dynamicgraph::Vector& tau_predicted = m_tau_predictedSIN(iter);
257 for (std::size_t i = 0; i < m_emergencyStopSIN.size(); i++) {
258 if ((*m_emergencyStopSIN[i]).isPlugged() && (*m_emergencyStopSIN[i])(iter)) {
259 m_emergency_stop_triggered =
true;
260 SEND_MSG(
"Emergency Stop has been triggered by an external entity", MSG_TYPE_ERROR);
266 if (!m_emergency_stop_triggered) {
267 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
268 if (fabs(tau(i)) > tau_max(i)) {
269 m_emergency_stop_triggered =
true;
270 SEND_MSG(
"Estimated torque " + toString(tau(i)) +
" > max torque " + toString(tau_max(i)) +
" for joint " +
271 m_robot_util->get_name_from_id(i),
275 if (fabs(tau_predicted(i)) > tau_max(i)) {
276 m_emergency_stop_triggered =
true;
277 SEND_MSG(
"Predicted torque " + toString(tau_predicted(i)) +
" > max torque " + toString(tau_max(i)) +
278 " for joint " + m_robot_util->get_name_from_id(i),
282 if (fabs(i_real(i)) > i_max(i)) {
283 m_emergency_stop_triggered =
true;
284 SEND_MSG(
"Joint " + m_robot_util->get_name_from_id(i) +
285 " measured current is too large: " + toString(i_real(i)) +
"A > " + toString(i_max(i)) +
"A",
290 if (fabs(u(i)) > ctrl_max(i)) {
291 m_emergency_stop_triggered =
true;
292 SEND_MSG(
"Joint " + m_robot_util->get_name_from_id(i) +
" desired current is too large: " + toString(u(i)) +
293 "A > " + toString(ctrl_max(i)) +
"A",
300 if (m_emergency_stop_triggered) s.setZero();
309 for (
unsigned int i = 0; i <
m_ctrlModes.size(); i++)
310 if (name ==
m_ctrlModes[i])
return SEND_MSG(
"It already exists a control mode with name " + name, MSG_TYPE_ERROR);
314 NULL, getClassName() +
"(" + getName() +
")::input(dynamicgraph::Vector)::ctrl_" + name));
319 getClassName() +
"(" + getName() +
")::output(dynamicgraph::Vector)::joints_ctrl_mode_" + name));
338 if (jointName ==
"all") {
342 std::stringstream ss(jointName);
344 std::list<int> jIdList;
346 while (std::getline(ss, item,
'-')) {
347 SEND_MSG(
"parsed joint : " + item, MSG_TYPE_INFO);
350 for (std::list<int>::iterator it = jIdList.begin(); it != jIdList.end(); ++it)
setCtrlMode(*it, cm);
357 return SEND_MSG(
"Cannot change control mode of joint " +
m_robot_util->get_name_from_id(jid) +
358 " because its previous ctrl mode transition has not terminated yet: " +
363 return SEND_MSG(
"Cannot change control mode of joint " +
m_robot_util->get_name_from_id(jid) +
364 " because it has already the specified ctrl mode",
379 if (jointName ==
"all") {
381 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++)
383 SEND_MSG(ss.str(), MSG_TYPE_INFO);
393 getProfiler().reset_all();
394 getStatistics().reset_all();
400 if (seconds < 0.0)
return SEND_MSG(
"Sleep time cannot be negative!", MSG_TYPE_ERROR);
405 SEND_MSG(
"New emergency signal input emergencyStop_" + name +
" created", MSG_TYPE_INFO);
408 new SignalPtr<bool, int>(NULL, getClassName() +
"(" + getName() +
")::input(bool)::emergencyStop_" + name));
418 SEND_WARNING_STREAM_MSG(
"Cannot set joint name from joint id before initialization!");
421 m_robot_util->set_name_to_id(jointName,
static_cast<Eigen::VectorXd::Index
>(jointId));
426 SEND_WARNING_STREAM_MSG(
"Cannot set joints limits from joint id before initialization!");
430 m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq);
434 const dynamicgraph::Vector& uq) {
436 SEND_WARNING_STREAM_MSG(
"Cannot set force limits from force id before initialization!");
440 m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq);
445 SEND_WARNING_STREAM_MSG(
"Cannot set force sensor name from force sensor id before initialization!");
449 m_robot_util->m_force_util.set_name_to_force_id(forceName,
static_cast<Eigen::VectorXd::Index
>(forceId));
454 SEND_WARNING_STREAM_MSG(
"Cannot set mapping to sot before initialization!");
462 SEND_WARNING_STREAM_MSG(
"Cannot set right foot sole XYZ before initialization!");
471 SEND_WARNING_STREAM_MSG(
"Cannot set right foot force sensor XYZ before initialization!");
475 m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
480 SEND_WARNING_STREAM_MSG(
"Cannot set foot frame name!");
483 if (FootName ==
"Left")
484 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
485 else if (FootName ==
"Right")
486 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
488 SEND_WARNING_STREAM_MSG(
"Did not understand the foot name !" + FootName);
493 SEND_WARNING_STREAM_MSG(
"Cannot set hand frame name!");
496 if (HandName ==
"Left")
497 m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
498 else if (HandName ==
"Right")
499 m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
501 SEND_WARNING_STREAM_MSG(
"Did not understand the hand name !" + HandName);
506 SEND_WARNING_STREAM_MSG(
"Cannot set IMU joint name!");
518 SEND_MSG(
"You should call init first. The size of the vector is unknown.", MSG_TYPE_ERROR);
524 for (
unsigned int j = 0; j <
m_robot_util->m_nbJoints; j++) {
537 for (
unsigned int i = 0; i <
m_ctrlModes.size(); i++)
542 SEND_MSG(
"The specified control mode does not exist", MSG_TYPE_ERROR);
543 SEND_MSG(
"Possible control modes are: " + toString(
m_ctrlModes), MSG_TYPE_INFO);
551 SEND_MSG(
"The specified joint name does not exist: " + name, MSG_TYPE_ERROR);
552 std::stringstream ss;
553 for (pinocchio::Model::JointIndex it = 0; it <
m_robot_util->m_nbJoints; it++)
555 SEND_MSG(
"Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
558 id = (
unsigned int)jid;
563 const JointLimits& JL =
m_robot_util->get_joint_limits_from_id((Index)
id);
565 double jl = JL.lower;
567 SEND_MSG(
"Desired joint angle " + toString(q) +
" is smaller than lower limit: " + toString(jl), MSG_TYPE_ERROR);
570 double ju = JL.upper;
572 SEND_MSG(
"Desired joint angle " + toString(q) +
" is larger than upper limit: " + toString(ju), MSG_TYPE_ERROR);
583 os <<
"ControlManager " << getName();
585 getProfiler().report_all(3, os);
586 }
catch (ExceptionSignal e) {