| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
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> |
||
9 |
#include <sot/torque_control/commands-helper.hh> |
||
10 |
#include <sot/torque_control/control-manager.hh> |
||
11 |
#include <tsid/robots/robot-wrapper.hpp> |
||
12 |
#include <tsid/utils/statistics.hpp> |
||
13 |
#include <tsid/utils/stop-watch.hpp> |
||
14 |
|||
15 |
using namespace tsid; |
||
16 |
|||
17 |
namespace dynamicgraph { |
||
18 |
namespace sot { |
||
19 |
namespace torque_control { |
||
20 |
namespace dynamicgraph = ::dynamicgraph; |
||
21 |
using namespace dynamicgraph; |
||
22 |
using namespace dynamicgraph::command; |
||
23 |
using namespace std; |
||
24 |
using 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 |
|||
37 |
/// Define EntityClassName here rather than in the header file |
||
38 |
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
||
39 |
typedef ControlManager EntityClassName; |
||
40 |
|||
41 |
/* --- DG FACTORY ---------------------------------------------------- */ |
||
42 |
✓✗ | 1 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlManager, "ControlManager"); |
43 |
|||
44 |
/* ------------------------------------------------------------------- */ |
||
45 |
/* --- CONSTRUCTION -------------------------------------------------- */ |
||
46 |
/* ------------------------------------------------------------------- */ |
||
47 |
1 |
ControlManager::ControlManager(const std::string& name) |
|
48 |
: Entity(name), |
||
49 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(i_measured, dynamicgraph::Vector), |
50 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(tau, dynamicgraph::Vector), |
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(tau_predicted, dynamicgraph::Vector), |
52 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(i_max, dynamicgraph::Vector), |
53 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(u_max, dynamicgraph::Vector), |
54 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(tau_max, dynamicgraph::Vector), |
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, m_i_measuredSIN), |
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
17 |
m_sleep_time(0.0) { |
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
65 |
|||
66 |
/* Commands. */ |
||
67 |
✓✗✓✗ |
1 |
addCommand("init", |
68 |
✓✗ | 1 |
makeCommandVoid3(*this, &ControlManager::init, |
69 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
docCommandVoid3("Initialize the entity.", |
70 |
"Time period in seconds (double)", |
||
71 |
"URDF file path (string)", |
||
72 |
"Robot reference (string)"))); |
||
73 |
✓✗✓✗ |
1 |
addCommand("addCtrlMode", |
74 |
✓✗ | 1 |
makeCommandVoid1( |
75 |
*this, &ControlManager::addCtrlMode, |
||
76 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Create an input signal with name 'ctrl_x' " |
77 |
"where x is the specified name.", |
||
78 |
"Name (string)"))); |
||
79 |
|||
80 |
✓✗✓✗ |
1 |
addCommand( |
81 |
"ctrlModes", |
||
82 |
✓✗ | 1 |
makeCommandVoid0( |
83 |
*this, &ControlManager::ctrlModes, |
||
84 |
✓✗✓✗ |
2 |
docCommandVoid0("Get a list of all the available control modes."))); |
85 |
|||
86 |
✓✗✓✗ |
1 |
addCommand( |
87 |
"setCtrlMode", |
||
88 |
✓✗ | 1 |
makeCommandVoid2( |
89 |
*this, &ControlManager::setCtrlMode, |
||
90 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set the control mode for a joint.", |
91 |
"(string) joint name", "(string) control mode"))); |
||
92 |
|||
93 |
✓✗✓✗ |
1 |
addCommand( |
94 |
"getCtrlMode", |
||
95 |
✓✗ | 1 |
makeCommandVoid1(*this, &ControlManager::getCtrlMode, |
96 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Get the control mode of a joint.", |
97 |
"(string) joint name"))); |
||
98 |
|||
99 |
✓✗✓✗ |
1 |
addCommand("resetProfiler", |
100 |
✓✗ | 1 |
makeCommandVoid0( |
101 |
*this, &ControlManager::resetProfiler, |
||
102 |
✓✗✓✗ |
2 |
docCommandVoid0("Reset the statistics computed by the " |
103 |
"profiler (print this entity to see them)."))); |
||
104 |
|||
105 |
✓✗✓✗ |
1 |
addCommand("setNameToId", |
106 |
✓✗ | 1 |
makeCommandVoid2( |
107 |
*this, &ControlManager::setNameToId, |
||
108 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set map for a name to an Id", |
109 |
"(string) joint name", "(double) joint id"))); |
||
110 |
|||
111 |
✓✗✓✗ |
1 |
addCommand( |
112 |
"setForceNameToForceId", |
||
113 |
✓✗ | 1 |
makeCommandVoid2( |
114 |
*this, &ControlManager::setForceNameToForceId, |
||
115 |
✓✗✓✗ ✓✗✓✗ |
2 |
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 |
✓✗✓✗ |
1 |
addCommand("setJointLimitsFromId", |
120 |
✓✗ | 1 |
makeCommandVoid3( |
121 |
*this, &ControlManager::setJointLimitsFromId, |
||
122 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
docCommandVoid3("Set the joint limits for a given joint ID", |
123 |
"(double) joint id", "(double) lower limit", |
||
124 |
"(double) uppper limit"))); |
||
125 |
|||
126 |
✓✗✓✗ |
1 |
addCommand( |
127 |
"setForceLimitsFromId", |
||
128 |
✓✗ | 1 |
makeCommandVoid3( |
129 |
*this, &ControlManager::setForceLimitsFromId, |
||
130 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
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 |
✓✗✓✗ |
1 |
addCommand( |
135 |
"setJointsUrdfToSot", |
||
136 |
✓✗ | 1 |
makeCommandVoid1(*this, &ControlManager::setJoints, |
137 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Map Joints From URDF to SoT.", |
138 |
"Vector of integer for mapping"))); |
||
139 |
|||
140 |
✓✗✓✗ |
1 |
addCommand( |
141 |
"setRightFootSoleXYZ", |
||
142 |
✓✗ | 1 |
makeCommandVoid1(*this, &ControlManager::setRightFootSoleXYZ, |
143 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the right foot sole 3d position.", |
144 |
"Vector of 3 doubles"))); |
||
145 |
✓✗✓✗ |
1 |
addCommand( |
146 |
"setRightFootForceSensorXYZ", |
||
147 |
✓✗ | 1 |
makeCommandVoid1(*this, &ControlManager::setRightFootForceSensorXYZ, |
148 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the right foot sensor 3d position.", |
149 |
"Vector of 3 doubles"))); |
||
150 |
|||
151 |
✓✗✓✗ |
1 |
addCommand("setFootFrameName", |
152 |
✓✗ | 1 |
makeCommandVoid2( |
153 |
*this, &ControlManager::setFootFrameName, |
||
154 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set the Frame Name for the Foot Name.", |
155 |
"(string) Foot name", "(string) Frame name"))); |
||
156 |
✓✗✓✗ |
1 |
addCommand("setHandFrameName", |
157 |
✓✗ | 1 |
makeCommandVoid2( |
158 |
*this, &ControlManager::setHandFrameName, |
||
159 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set the Frame Name for the Hand Name.", |
160 |
"(string) Hand name", "(string) Frame name"))); |
||
161 |
✓✗✓✗ |
1 |
addCommand("setImuJointName", |
162 |
✓✗ | 1 |
makeCommandVoid1( |
163 |
*this, &ControlManager::setImuJointName, |
||
164 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the Joint Name wich IMU is attached to.", |
165 |
"(string) Joint name"))); |
||
166 |
✓✗✓✗ |
1 |
addCommand("displayRobotUtil", |
167 |
✓✗ | 1 |
makeCommandVoid0( |
168 |
*this, &ControlManager::displayRobotUtil, |
||
169 |
✓✗✓✗ |
2 |
docCommandVoid0("Display the current robot util data set."))); |
170 |
|||
171 |
✓✗✓✗ |
1 |
addCommand( |
172 |
"setStreamPrintPeriod", |
||
173 |
✓✗ | 1 |
makeCommandVoid1( |
174 |
*this, &ControlManager::setStreamPrintPeriod, |
||
175 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the period used for printing in streaming.", |
176 |
"Print period in seconds (double)"))); |
||
177 |
|||
178 |
✓✗✓✗ |
1 |
addCommand( |
179 |
"setSleepTime", |
||
180 |
✓✗ | 1 |
makeCommandVoid1(*this, &ControlManager::setSleepTime, |
181 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the time to sleep at every " |
182 |
"iteration (to slow down simulation).", |
||
183 |
"Sleep time in seconds (double)"))); |
||
184 |
|||
185 |
✓✗✓✗ |
1 |
addCommand( |
186 |
"addEmergencyStopSIN", |
||
187 |
✓✗ | 1 |
makeCommandVoid1( |
188 |
*this, &ControlManager::addEmergencyStopSIN, |
||
189 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Add emergency signal input from another entity that " |
190 |
"can stop the control if necessary.", |
||
191 |
"(string) signal name : 'emergencyStop_' + name"))); |
||
192 |
1 |
} |
|
193 |
|||
194 |
1 |
void ControlManager::init(const double& dt, const std::string& urdfFile, |
|
195 |
const std::string& robotRef) { |
||
196 |
✗✓✗✗ ✗✗ |
1 |
if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); |
197 |
1 |
m_dt = dt; |
|
198 |
1 |
m_emergency_stop_triggered = false; |
|
199 |
1 |
m_initSucceeded = true; |
|
200 |
2 |
vector<string> package_dirs; |
|
201 |
✓✗ | 2 |
m_robot = new robots::RobotWrapper(urdfFile, package_dirs, |
202 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::JointModelFreeFlyer()); |
203 |
✓✗ | 1 |
std::string localName(robotRef); |
204 |
✓✗✓✗ |
1 |
if (!isNameInRobotUtil(localName)) { |
205 |
✓✗ | 1 |
m_robot_util = createRobotUtil(localName); |
206 |
✓✗✓✗ |
1 |
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 |
✓✗ | 1 |
SEND_MSG(m_robot_util->m_urdf_filename, MSG_TYPE_INFO); |
212 |
✓✗ | 1 |
m_robot_util->m_urdf_filename = urdfFile; |
213 |
✓✗✓✗ |
1 |
addCommand( |
214 |
"getJointsUrdfToSot", |
||
215 |
✓✗ | 1 |
makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot, |
216 |
✓✗✓✗ ✓✗ |
2 |
docDirectSetter("Display map Joints From URDF to SoT.", |
217 |
"Vector of integer for mapping"))); |
||
218 |
|||
219 |
✓✗ | 1 |
m_robot_util->m_nbJoints = m_robot->nv() - 6; |
220 |
✓✗ | 1 |
m_jointCtrlModes_current.resize(m_robot_util->m_nbJoints); |
221 |
✓✗ | 1 |
m_jointCtrlModes_previous.resize(m_robot_util->m_nbJoints); |
222 |
✓✗ | 1 |
m_jointCtrlModesCountDown.resize(m_robot_util->m_nbJoints, 0); |
223 |
} |
||
224 |
|||
225 |
/* ------------------------------------------------------------------- */ |
||
226 |
/* --- SIGNALS ------------------------------------------------------- */ |
||
227 |
/* ------------------------------------------------------------------- */ |
||
228 |
|||
229 |
DEFINE_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 |
|||
305 |
DEFINE_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 |
|||
376 |
void 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]); |
||
402 |
updateJointCtrlModesOutputSignal(); |
||
403 |
} |
||
404 |
|||
405 |
void ControlManager::ctrlModes() { |
||
406 |
SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO); |
||
407 |
} |
||
408 |
|||
409 |
void 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 |
} |
||
431 |
updateJointCtrlModesOutputSignal(); |
||
432 |
} |
||
433 |
|||
434 |
void 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 |
||
451 |
m_jointCtrlModes_previous[jid] = cm; |
||
452 |
m_jointCtrlModes_current[jid] = cm; |
||
453 |
} else { |
||
454 |
m_jointCtrlModesCountDown[jid] = CTRL_MODE_TRANSITION_TIME_STEP; |
||
455 |
m_jointCtrlModes_previous[jid] = m_jointCtrlModes_current[jid]; |
||
456 |
m_jointCtrlModes_current[jid] = cm; |
||
457 |
} |
||
458 |
} |
||
459 |
|||
460 |
void 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 " + |
||
473 |
m_jointCtrlModes_current[i].name, |
||
474 |
MSG_TYPE_INFO); |
||
475 |
} |
||
476 |
|||
477 |
void ControlManager::resetProfiler() { |
||
478 |
getProfiler().reset_all(); |
||
479 |
getStatistics().reset_all(); |
||
480 |
} |
||
481 |
|||
482 |
void ControlManager::setStreamPrintPeriod(const double& s) { |
||
483 |
setStreamPrintPeriod(s); |
||
484 |
} |
||
485 |
|||
486 |
void 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 |
|||
492 |
void 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 |
|||
506 |
void 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 |
|||
517 |
void 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 |
|||
528 |
void 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 |
|||
540 |
void 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 |
|||
553 |
void 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 |
|||
561 |
void 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 |
|||
571 |
void ControlManager::setRightFootForceSensorXYZ( |
||
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 |
|||
582 |
void 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 |
|||
596 |
void 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 |
|||
610 |
void 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 |
|||
618 |
void ControlManager::displayRobotUtil() { m_robot_util->display(std::cout); } |
||
619 |
|||
620 |
/* --- PROTECTED MEMBER METHODS |
||
621 |
* ---------------------------------------------------------- */ |
||
622 |
|||
623 |
void ControlManager::updateJointCtrlModesOutputSignal() { |
||
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 |
|||
645 |
bool 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 |
|||
659 |
bool 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 |
|||
677 |
bool 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 |
|||
701 |
void 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 |
| Generated by: GCOVR (Version 4.2) |