sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
control-manager.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#include <dynamic-graph/factory.h>
7
8#include <sot/core/debug.hh>
11#include <tsid/robots/robot-wrapper.hpp>
12#include <tsid/utils/statistics.hpp>
13#include <tsid/utils/stop-watch.hpp>
14
15using namespace tsid;
16
17namespace dynamicgraph {
18namespace sot {
19namespace torque_control {
20namespace dynamicgraph = ::dynamicgraph;
21using namespace dynamicgraph;
22using namespace dynamicgraph::command;
23using namespace std;
24using namespace dg::sot::torque_control;
25
26// Size to be aligned "-------------------------------------------------------"
27#define PROFILE_PWM_DESIRED_COMPUTATION \
28 "Control manager "
29#define PROFILE_DYNAMIC_GRAPH_PERIOD \
30 "Control period "
31
32#define INPUT_SIGNALS \
33 m_i_maxSIN << m_u_maxSIN << m_tau_maxSIN << m_tauSIN << m_tau_predictedSIN \
34 << m_i_measuredSIN
35#define OUTPUT_SIGNALS m_uSOUT << m_u_safeSOUT
36
39typedef ControlManager EntityClassName;
40
41/* --- DG FACTORY ---------------------------------------------------- */
43
44/* ------------------------------------------------------------------- */
45/* --- CONSTRUCTION -------------------------------------------------- */
46/* ------------------------------------------------------------------- */
47ControlManager::ControlManager(const std::string& name)
48 : Entity(name),
49 CONSTRUCT_SIGNAL_IN(i_measured, dynamicgraph::Vector),
50 CONSTRUCT_SIGNAL_IN(tau, dynamicgraph::Vector),
51 CONSTRUCT_SIGNAL_IN(tau_predicted, dynamicgraph::Vector),
52 CONSTRUCT_SIGNAL_IN(i_max, dynamicgraph::Vector),
53 CONSTRUCT_SIGNAL_IN(u_max, dynamicgraph::Vector),
54 CONSTRUCT_SIGNAL_IN(tau_max, dynamicgraph::Vector),
55 CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, m_i_measuredSIN),
56 CONSTRUCT_SIGNAL_OUT(u_safe, dynamicgraph::Vector,
57 INPUT_SIGNALS << m_uSOUT),
58 m_robot_util(RefVoidRobotUtil()),
59 m_initSucceeded(false),
60 m_emergency_stop_triggered(false),
61 m_is_first_iter(true),
62 m_iter(0),
63 m_sleep_time(0.0) {
64 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
65
66 /* Commands. */
67 addCommand("init",
68 makeCommandVoid3(*this, &ControlManager::init,
69 docCommandVoid3("Initialize the entity.",
70 "Time period in seconds (double)",
71 "URDF file path (string)",
72 "Robot reference (string)")));
73 addCommand("addCtrlMode",
74 makeCommandVoid1(
76 docCommandVoid1("Create an input signal with name 'ctrl_x' "
77 "where x is the specified name.",
78 "Name (string)")));
79
80 addCommand(
81 "ctrlModes",
82 makeCommandVoid0(
84 docCommandVoid0("Get a list of all the available control modes.")));
85
86 addCommand(
87 "setCtrlMode",
88 makeCommandVoid2(
90 docCommandVoid2("Set the control mode for a joint.",
91 "(string) joint name", "(string) control mode")));
92
93 addCommand(
94 "getCtrlMode",
95 makeCommandVoid1(*this, &ControlManager::getCtrlMode,
96 docCommandVoid1("Get the control mode of a joint.",
97 "(string) joint name")));
98
99 addCommand("resetProfiler",
100 makeCommandVoid0(
102 docCommandVoid0("Reset the statistics computed by the "
103 "profiler (print this entity to see them).")));
104
105 addCommand("setNameToId",
106 makeCommandVoid2(
108 docCommandVoid2("Set map for a name to an Id",
109 "(string) joint name", "(double) joint id")));
110
111 addCommand(
112 "setForceNameToForceId",
113 makeCommandVoid2(
115 docCommandVoid2(
116 "Set map from a force sensor name to a force sensor Id",
117 "(string) force sensor name", "(double) force sensor id")));
118
119 addCommand("setJointLimitsFromId",
120 makeCommandVoid3(
122 docCommandVoid3("Set the joint limits for a given joint ID",
123 "(double) joint id", "(double) lower limit",
124 "(double) uppper limit")));
125
126 addCommand(
127 "setForceLimitsFromId",
128 makeCommandVoid3(
130 docCommandVoid3("Set the force limits for a given force sensor ID",
131 "(double) force sensor id", "(double) lower limit",
132 "(double) uppper limit")));
133
134 addCommand(
135 "setJointsUrdfToSot",
136 makeCommandVoid1(*this, &ControlManager::setJoints,
137 docCommandVoid1("Map Joints From URDF to SoT.",
138 "Vector of integer for mapping")));
139
140 addCommand(
141 "setRightFootSoleXYZ",
142 makeCommandVoid1(*this, &ControlManager::setRightFootSoleXYZ,
143 docCommandVoid1("Set the right foot sole 3d position.",
144 "Vector of 3 doubles")));
145 addCommand(
146 "setRightFootForceSensorXYZ",
147 makeCommandVoid1(*this, &ControlManager::setRightFootForceSensorXYZ,
148 docCommandVoid1("Set the right foot sensor 3d position.",
149 "Vector of 3 doubles")));
150
151 addCommand("setFootFrameName",
152 makeCommandVoid2(
154 docCommandVoid2("Set the Frame Name for the Foot Name.",
155 "(string) Foot name", "(string) Frame name")));
156 addCommand("setHandFrameName",
157 makeCommandVoid2(
159 docCommandVoid2("Set the Frame Name for the Hand Name.",
160 "(string) Hand name", "(string) Frame name")));
161 addCommand("setImuJointName",
162 makeCommandVoid1(
164 docCommandVoid1("Set the Joint Name wich IMU is attached to.",
165 "(string) Joint name")));
166 addCommand("displayRobotUtil",
167 makeCommandVoid0(
169 docCommandVoid0("Display the current robot util data set.")));
170
171 addCommand(
172 "setStreamPrintPeriod",
173 makeCommandVoid1(
175 docCommandVoid1("Set the period used for printing in streaming.",
176 "Print period in seconds (double)")));
177
178 addCommand(
179 "setSleepTime",
180 makeCommandVoid1(*this, &ControlManager::setSleepTime,
181 docCommandVoid1("Set the time to sleep at every "
182 "iteration (to slow down simulation).",
183 "Sleep time in seconds (double)")));
184
185 addCommand(
186 "addEmergencyStopSIN",
187 makeCommandVoid1(
189 docCommandVoid1("Add emergency signal input from another entity that "
190 "can stop the control if necessary.",
191 "(string) signal name : 'emergencyStop_' + name")));
192}
193
194void ControlManager::init(const double& dt, const std::string& urdfFile,
195 const std::string& robotRef) {
196 if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
197 m_dt = dt;
199 m_initSucceeded = true;
200 vector<string> package_dirs;
201 m_robot = new robots::RobotWrapper(urdfFile, package_dirs,
202 pinocchio::JointModelFreeFlyer());
203 std::string localName(robotRef);
204 if (!isNameInRobotUtil(localName)) {
205 m_robot_util = createRobotUtil(localName);
206 SEND_MSG("createRobotUtil success\n", MSG_TYPE_INFO);
207 } else {
208 m_robot_util = getRobotUtil(localName);
209 SEND_MSG("getRobotUtil success\n", MSG_TYPE_INFO);
210 }
211 SEND_MSG(m_robot_util->m_urdf_filename, MSG_TYPE_INFO);
212 m_robot_util->m_urdf_filename = urdfFile;
213 addCommand(
214 "getJointsUrdfToSot",
215 makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
216 docDirectSetter("Display map Joints From URDF to SoT.",
217 "Vector of integer for mapping")));
218
219 m_robot_util->m_nbJoints = m_robot->nv() - 6;
220 m_jointCtrlModes_current.resize(m_robot_util->m_nbJoints);
221 m_jointCtrlModes_previous.resize(m_robot_util->m_nbJoints);
222 m_jointCtrlModesCountDown.resize(m_robot_util->m_nbJoints, 0);
223}
224
225/* ------------------------------------------------------------------- */
226/* --- SIGNALS ------------------------------------------------------- */
227/* ------------------------------------------------------------------- */
228
229DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
230 if (!m_initSucceeded) {
231 SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
232 return s;
233 }
234
235 if (m_is_first_iter)
236 m_is_first_iter = false;
237 else
238 getProfiler().stop(PROFILE_DYNAMIC_GRAPH_PERIOD);
239 getProfiler().start(PROFILE_DYNAMIC_GRAPH_PERIOD);
240
241 if (s.size() != (Eigen::VectorXd::Index)m_robot_util->m_nbJoints)
242 s.resize(m_robot_util->m_nbJoints);
243
244 getProfiler().start(PROFILE_PWM_DESIRED_COMPUTATION);
245 {
246 // trigger computation of all ctrl inputs
247 for (unsigned int i = 0; i < m_ctrlInputsSIN.size(); i++)
248 (*m_ctrlInputsSIN[i])(iter);
249
250 int cm_id, cm_id_prev;
251 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
252 cm_id = m_jointCtrlModes_current[i].id;
253 if (cm_id < 0) {
254 SEND_MSG("You forgot to set the control mode of joint " + toString(i),
255 MSG_TYPE_ERROR_STREAM);
256 continue;
257 }
258
259 const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
260 assert(ctrl.size() ==
261 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
262
263 if (m_jointCtrlModesCountDown[i] == 0)
264 s(i) = ctrl(i);
265 else {
266 cm_id_prev = m_jointCtrlModes_previous[i].id;
267 const dynamicgraph::Vector& ctrl_prev =
268 (*m_ctrlInputsSIN[cm_id_prev])(iter);
269 assert(ctrl_prev.size() ==
270 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
271
272 double alpha =
273 m_jointCtrlModesCountDown[i] / CTRL_MODE_TRANSITION_TIME_STEP;
274 // SEND_MSG("Joint "+toString(i)+" changing ctrl mode from
275 // "+toString(cm_id_prev)+
276 // " to "+toString(cm_id)+"
277 // alpha="+toString(alpha),MSG_TYPE_DEBUG);
278 s(i) = alpha * ctrl_prev(i) + (1 - alpha) * ctrl(i);
279 m_jointCtrlModesCountDown[i]--;
280
281 if (m_jointCtrlModesCountDown[i] == 0) {
282 SEND_MSG("Joint " + toString(i) + " changed ctrl mode from " +
283 toString(cm_id_prev) + " to " + toString(cm_id),
284 MSG_TYPE_INFO);
285 updateJointCtrlModesOutputSignal();
286 }
287 }
288 }
289 }
290 getProfiler().stop(PROFILE_PWM_DESIRED_COMPUTATION);
291
292 usleep(static_cast<unsigned int>(1e6 * m_sleep_time));
293 if (m_sleep_time >= 0.1) {
294 for (unsigned int i = 0; i < m_ctrlInputsSIN.size(); i++) {
295 const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[i])(iter);
296 SEND_MSG(toString(iter) + ") tau =" + toString(ctrl, 1, 4, " ") +
297 m_ctrlModes[i],
298 MSG_TYPE_ERROR);
299 }
300 }
301
302 return s;
303}
304
305DEFINE_SIGNAL_OUT_FUNCTION(u_safe, dynamicgraph::Vector) {
306 if (!m_initSucceeded) {
307 SEND_WARNING_STREAM_MSG(
308 "Cannot compute signal u_safe before initialization!");
309 return s;
310 }
311
312 const dynamicgraph::Vector& u = m_uSOUT(iter);
313 const dynamicgraph::Vector& tau_max = m_tau_maxSIN(iter);
314 const dynamicgraph::Vector& ctrl_max = m_u_maxSIN(iter);
315 const dynamicgraph::Vector& i_max = m_i_maxSIN(iter);
316 const dynamicgraph::Vector& tau = m_tauSIN(iter);
317 const dynamicgraph::Vector& i_real = m_i_measuredSIN(iter);
318 const dynamicgraph::Vector& tau_predicted = m_tau_predictedSIN(iter);
319
320 for (std::size_t i = 0; i < m_emergencyStopSIN.size(); i++) {
321 if ((*m_emergencyStopSIN[i]).isPlugged() &&
322 (*m_emergencyStopSIN[i])(iter)) {
323 m_emergency_stop_triggered = true;
324 SEND_MSG("Emergency Stop has been triggered by an external entity",
325 MSG_TYPE_ERROR);
326 }
327 }
328
329 s = u;
330
331 if (!m_emergency_stop_triggered) {
332 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
333 if (fabs(tau(i)) > tau_max(i)) {
334 m_emergency_stop_triggered = true;
335 SEND_MSG("Estimated torque " + toString(tau(i)) + " > max torque " +
336 toString(tau_max(i)) + " for joint " +
337 m_robot_util->get_name_from_id(i),
338 MSG_TYPE_ERROR);
339 }
340
341 if (fabs(tau_predicted(i)) > tau_max(i)) {
342 m_emergency_stop_triggered = true;
343 SEND_MSG("Predicted torque " + toString(tau_predicted(i)) +
344 " > max torque " + toString(tau_max(i)) + " for joint " +
345 m_robot_util->get_name_from_id(i),
346 MSG_TYPE_ERROR);
347 }
348
349 if (fabs(i_real(i)) > i_max(i)) {
350 m_emergency_stop_triggered = true;
351 SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) +
352 " measured current is too large: " + toString(i_real(i)) +
353 "A > " + toString(i_max(i)) + "A",
354 MSG_TYPE_ERROR);
355 break;
356 }
357
358 if (fabs(u(i)) > ctrl_max(i)) {
359 m_emergency_stop_triggered = true;
360 SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) +
361 " desired current is too large: " + toString(u(i)) +
362 "A > " + toString(ctrl_max(i)) + "A",
363 MSG_TYPE_ERROR);
364 break;
365 }
366 }
367 }
368
369 if (m_emergency_stop_triggered) s.setZero();
370
371 return s;
372}
373
374/* --- COMMANDS ---------------------------------------------------------- */
375
376void ControlManager::addCtrlMode(const string& name) {
377 // check there is no other control mode with the same name
378 for (unsigned int i = 0; i < m_ctrlModes.size(); i++)
379 if (name == m_ctrlModes[i])
380 return SEND_MSG("It already exists a control mode with name " + name,
381 MSG_TYPE_ERROR);
382
383 // create a new input signal to read the new control
384 m_ctrlInputsSIN.push_back(new SignalPtr<dynamicgraph::Vector, int>(
385 NULL, getClassName() + "(" + getName() +
386 ")::input(dynamicgraph::Vector)::ctrl_" + name));
387
388 // create a new output signal to specify which joints are controlled with the
389 // new control mode
390 m_jointsCtrlModesSOUT.push_back(new Signal<dynamicgraph::Vector, int>(
391 getClassName() + "(" + getName() +
392 ")::output(dynamicgraph::Vector)::joints_ctrl_mode_" + name));
393
394 // add the new control mode to the list of available control modes
395 m_ctrlModes.push_back(name);
396
397 // register the new signals and add the new signal dependecy
398 Eigen::VectorXd::Index i = m_ctrlModes.size() - 1;
399 m_uSOUT.addDependency(*m_ctrlInputsSIN[i]);
400 Entity::signalRegistration(*m_ctrlInputsSIN[i]);
401 Entity::signalRegistration(*m_jointsCtrlModesSOUT[i]);
403}
404
406 SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO);
407}
408
409void ControlManager::setCtrlMode(const string& jointName,
410 const string& ctrlMode) {
411 CtrlMode cm;
412 if (convertStringToCtrlMode(ctrlMode, cm) == false) return;
413
414 if (jointName == "all") {
415 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
416 setCtrlMode(i, cm);
417 } else {
418 // decompose strings like "rk-rhp-lhp-..."
419 std::stringstream ss(jointName);
420 std::string item;
421 std::list<int> jIdList;
422 unsigned int i;
423 while (std::getline(ss, item, '-')) {
424 SEND_MSG("parsed joint : " + item, MSG_TYPE_INFO);
425 if (convertJointNameToJointId(item, i)) jIdList.push_back(i);
426 }
427 for (std::list<int>::iterator it = jIdList.begin(); it != jIdList.end();
428 ++it)
429 setCtrlMode(*it, cm);
430 }
432}
433
434void ControlManager::setCtrlMode(const int jid, const CtrlMode& cm) {
435 if (m_jointCtrlModesCountDown[jid] != 0)
436 return SEND_MSG("Cannot change control mode of joint " +
437 m_robot_util->get_name_from_id(jid) +
438 " because its previous ctrl mode transition has not "
439 "terminated yet: " +
440 toString(m_jointCtrlModesCountDown[jid]),
441 MSG_TYPE_ERROR);
442
443 if (cm.id == m_jointCtrlModes_current[jid].id)
444 return SEND_MSG("Cannot change control mode of joint " +
445 m_robot_util->get_name_from_id(jid) +
446 " because it has already the specified ctrl mode",
447 MSG_TYPE_ERROR);
448
449 if (m_jointCtrlModes_current[jid].id < 0) {
450 // first setting of the control mode
452 m_jointCtrlModes_current[jid] = cm;
453 } else {
456 m_jointCtrlModes_current[jid] = cm;
457 }
458}
459
460void ControlManager::getCtrlMode(const std::string& jointName) {
461 if (jointName == "all") {
462 stringstream ss;
463 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
464 ss << m_robot_util->get_name_from_id(i) << " "
465 << m_jointCtrlModes_current[i] << "; ";
466 SEND_MSG(ss.str(), MSG_TYPE_INFO);
467 return;
468 }
469
470 unsigned int i;
471 if (convertJointNameToJointId(jointName, i) == false) return;
472 SEND_MSG("The control mode of joint " + jointName + " is " +
474 MSG_TYPE_INFO);
475}
476
478 getProfiler().reset_all();
479 getStatistics().reset_all();
480}
481
484}
485
486void ControlManager::setSleepTime(const double& seconds) {
487 if (seconds < 0.0)
488 return SEND_MSG("Sleep time cannot be negative!", MSG_TYPE_ERROR);
489 m_sleep_time = seconds;
490}
491
492void ControlManager::addEmergencyStopSIN(const string& name) {
493 SEND_MSG("New emergency signal input emergencyStop_" + name + " created",
494 MSG_TYPE_INFO);
495 // create a new input signal
496 m_emergencyStopSIN.push_back(new SignalPtr<bool, int>(
497 NULL, getClassName() + "(" + getName() +
498 ")::input(bool)::emergencyStop_" + name));
499
500 // register the new signals and add the new signal dependecy
501 Eigen::VectorXd::Index i = m_emergencyStopSIN.size() - 1;
502 m_u_safeSOUT.addDependency(*m_emergencyStopSIN[i]);
503 Entity::signalRegistration(*m_emergencyStopSIN[i]);
504}
505
506void ControlManager::setNameToId(const std::string& jointName,
507 const double& jointId) {
508 if (!m_initSucceeded) {
509 SEND_WARNING_STREAM_MSG(
510 "Cannot set joint name from joint id before initialization!");
511 return;
512 }
513 m_robot_util->set_name_to_id(jointName,
514 static_cast<Eigen::VectorXd::Index>(jointId));
515}
516
517void ControlManager::setJointLimitsFromId(const double& jointId,
518 const double& lq, const double& uq) {
519 if (!m_initSucceeded) {
520 SEND_WARNING_STREAM_MSG(
521 "Cannot set joints limits from joint id before initialization!");
522 return;
523 }
524
525 m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq);
526}
527
528void ControlManager::setForceLimitsFromId(const double& jointId,
529 const dynamicgraph::Vector& lq,
530 const dynamicgraph::Vector& uq) {
531 if (!m_initSucceeded) {
532 SEND_WARNING_STREAM_MSG(
533 "Cannot set force limits from force id before initialization!");
534 return;
535 }
536
537 m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq);
538}
539
540void ControlManager::setForceNameToForceId(const std::string& forceName,
541 const double& forceId) {
542 if (!m_initSucceeded) {
543 SEND_WARNING_STREAM_MSG(
544 "Cannot set force sensor name from force sensor id before "
545 "initialization!");
546 return;
547 }
548
549 m_robot_util->m_force_util.set_name_to_force_id(
550 forceName, static_cast<Eigen::VectorXd::Index>(forceId));
551}
552
553void ControlManager::setJoints(const dg::Vector& urdf_to_sot) {
554 if (!m_initSucceeded) {
555 SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
556 return;
557 }
558 m_robot_util->set_urdf_to_sot(urdf_to_sot);
559}
560
561void ControlManager::setRightFootSoleXYZ(const dynamicgraph::Vector& xyz) {
562 if (!m_initSucceeded) {
563 SEND_WARNING_STREAM_MSG(
564 "Cannot set right foot sole XYZ before initialization!");
565 return;
566 }
567
568 m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
569}
570
572 const dynamicgraph::Vector& xyz) {
573 if (!m_initSucceeded) {
574 SEND_WARNING_STREAM_MSG(
575 "Cannot set right foot force sensor XYZ before initialization!");
576 return;
577 }
578
579 m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
580}
581
582void ControlManager::setFootFrameName(const std::string& FootName,
583 const std::string& FrameName) {
584 if (!m_initSucceeded) {
585 SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
586 return;
587 }
588 if (FootName == "Left")
589 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
590 else if (FootName == "Right")
591 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
592 else
593 SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
594}
595
596void ControlManager::setHandFrameName(const std::string& HandName,
597 const std::string& FrameName) {
598 if (!m_initSucceeded) {
599 SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
600 return;
601 }
602 if (HandName == "Left")
603 m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
604 else if (HandName == "Right")
605 m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
606 else
607 SEND_WARNING_STREAM_MSG("Did not understand the hand name !" + HandName);
608}
609
610void ControlManager::setImuJointName(const std::string& JointName) {
611 if (!m_initSucceeded) {
612 SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
613 return;
614 }
615 m_robot_util->m_imu_joint_name = JointName;
616}
617
618void ControlManager::displayRobotUtil() { m_robot_util->display(std::cout); }
619
620/* --- PROTECTED MEMBER METHODS
621 * ---------------------------------------------------------- */
622
624 if (m_robot_util->m_nbJoints == 0) {
625 SEND_MSG("You should call init first. The size of the vector is unknown.",
626 MSG_TYPE_ERROR);
627 return;
628 }
629
630 dynamicgraph::Vector cm(m_robot_util->m_nbJoints);
631 for (unsigned int i = 0; i < m_jointsCtrlModesSOUT.size(); i++) {
632 for (unsigned int j = 0; j < m_robot_util->m_nbJoints; j++) {
633 cm(j) = 0;
634 if ((unsigned int)m_jointCtrlModes_current[j].id == i) cm(j) = 1;
635
636 // during the transition between two ctrl modes they both result active
637 if (m_jointCtrlModesCountDown[j] > 0 &&
638 (unsigned int)m_jointCtrlModes_previous[j].id == i)
639 cm(j) = 1;
640 }
641 m_jointsCtrlModesSOUT[i]->setConstant(cm);
642 }
643}
644
645bool ControlManager::convertStringToCtrlMode(const std::string& name,
646 CtrlMode& cm) {
647 // Check if the ctrl mode name exists
648 for (unsigned int i = 0; i < m_ctrlModes.size(); i++)
649 if (name == m_ctrlModes[i]) {
650 cm = CtrlMode(i, name);
651 return true;
652 }
653 SEND_MSG("The specified control mode does not exist", MSG_TYPE_ERROR);
654 SEND_MSG("Possible control modes are: " + toString(m_ctrlModes),
655 MSG_TYPE_INFO);
656 return false;
657}
658
659bool ControlManager::convertJointNameToJointId(const std::string& name,
660 unsigned int& id) {
661 // Check if the joint name exists
662 sot::Index jid = m_robot_util->get_id_from_name(name);
663 if (jid < 0) {
664 SEND_MSG("The specified joint name does not exist: " + name,
665 MSG_TYPE_ERROR);
666 std::stringstream ss;
667 for (pinocchio::Model::JointIndex it = 0; it < m_robot_util->m_nbJoints;
668 it++)
669 ss << m_robot_util->get_name_from_id(it) << ", ";
670 SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
671 return false;
672 }
673 id = (unsigned int)jid;
674 return true;
675}
676
677bool ControlManager::isJointInRange(unsigned int id, double q) {
678 const JointLimits& JL = m_robot_util->get_joint_limits_from_id((Index)id);
679
680 double jl = JL.lower;
681 if (q < jl) {
682 SEND_MSG("Desired joint angle " + toString(q) +
683 " is smaller than lower limit: " + toString(jl),
684 MSG_TYPE_ERROR);
685 return false;
686 }
687 double ju = JL.upper;
688 if (q > ju) {
689 SEND_MSG("Desired joint angle " + toString(q) +
690 " is larger than upper limit: " + toString(ju),
691 MSG_TYPE_ERROR);
692 return false;
693 }
694 return true;
695}
696
697/* ------------------------------------------------------------------- */
698/* --- ENTITY -------------------------------------------------------- */
699/* ------------------------------------------------------------------- */
700
701void ControlManager::display(std::ostream& os) const {
702 os << "ControlManager " << getName();
703 try {
704 getProfiler().report_all(3, os);
705 } catch (ExceptionSignal e) {
706 }
707}
708
709} // namespace torque_control
710} // namespace sot
711} // namespace dynamicgraph
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN
bool convertStringToCtrlMode(const std::string &name, CtrlMode &cm)
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &)
double m_dt
true if the entity has been successfully initialized
void setCtrlMode(const std::string &jointName, const std::string &ctrlMode)
virtual void display(std::ostream &os) const
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
void setForceLimitsFromId(const double &jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq)
Command related to ForceUtil.
std::vector< dynamicgraph::SignalPtr< bool, int > * > m_emergencyStopSIN
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
void init(const double &dt, const std::string &urdfFile, const std::string &robotRef)
void setJointLimitsFromId(const double &jointId, const double &lq, const double &uq)
void setForceNameToForceId(const std::string &forceName, const double &forceId)
bool convertJointNameToJointId(const std::string &name, unsigned int &id)
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
bool m_emergency_stop_triggered
control loop time period
void addCtrlMode(const std::string &name)
same as u when everything is fine, 0 otherwise
void setRightFootSoleXYZ(const dynamicgraph::Vector &)
Commands related to FootUtil.
void setNameToId(const std::string &jointName, const double &jointId)
Commands related to joint name and joint id.
void setJoints(const dynamicgraph::Vector &)
Set the mapping between urdf and sot.
void getCtrlMode(const std::string &jointName)
void setHandFrameName(const std::string &, const std::string &)
Commands related to HandUtil.
void setFootFrameName(const std::string &, const std::string &)
#define PROFILE_PWM_DESIRED_COMPUTATION
#define PROFILE_DYNAMIC_GRAPH_PERIOD
#define CTRL_MODE_TRANSITION_TIME_STEP
Number of time step to transition from one ctrl mode to another.
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22