| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Copyright 2014, Andrea Del Prete, LAAS-CNRS |
||
3 |
* |
||
4 |
* This file is part of sot-torque-control. |
||
5 |
* sot-torque-control is free software: you can redistribute it and/or |
||
6 |
* modify it under the terms of the GNU Lesser General Public License |
||
7 |
* as published by the Free Software Foundation, either version 3 of |
||
8 |
* the License, or (at your option) any later version. |
||
9 |
* sot-torque-control is distributed in the hope that it will be |
||
10 |
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
||
11 |
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
12 |
* GNU Lesser General Public License for more details. You should |
||
13 |
* have received a copy of the GNU Lesser General Public License along |
||
14 |
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>. |
||
15 |
*/ |
||
16 |
|||
17 |
#include <iostream> |
||
18 |
#include <pinocchio/fwd.hpp> |
||
19 |
// keep pinocchio before boost |
||
20 |
|||
21 |
#include <dynamic-graph/all-commands.h> |
||
22 |
#include <dynamic-graph/factory.h> |
||
23 |
|||
24 |
#include <boost/property_tree/ptree.hpp> |
||
25 |
#include <sot/core/debug.hh> |
||
26 |
#include <sot/core/exception-tools.hh> |
||
27 |
#include <sot/core/parameter-server.hh> |
||
28 |
|||
29 |
namespace dynamicgraph { |
||
30 |
namespace sot { |
||
31 |
namespace dynamicgraph = ::dynamicgraph; |
||
32 |
using namespace dynamicgraph; |
||
33 |
using namespace dynamicgraph::command; |
||
34 |
using namespace std; |
||
35 |
|||
36 |
// Size to be aligned "-------------------------------------------------------" |
||
37 |
#define PROFILE_PWM_DESIRED_COMPUTATION \ |
||
38 |
"Control manager " |
||
39 |
#define PROFILE_DYNAMIC_GRAPH_PERIOD \ |
||
40 |
"Control period " |
||
41 |
|||
42 |
#define INPUT_SIGNALS |
||
43 |
#define OUTPUT_SIGNALS |
||
44 |
|||
45 |
/// Define EntityClassName here rather than in the header file |
||
46 |
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
||
47 |
typedef ParameterServer EntityClassName; |
||
48 |
|||
49 |
/* --- DG FACTORY ---------------------------------------------------- */ |
||
50 |
✓✗ | 1 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer, "ParameterServer"); |
51 |
|||
52 |
/* ------------------------------------------------------------------- */ |
||
53 |
/* --- CONSTRUCTION -------------------------------------------------- */ |
||
54 |
/* ------------------------------------------------------------------- */ |
||
55 |
1 |
ParameterServer::ParameterServer(const std::string &name) |
|
56 |
: Entity(name), |
||
57 |
m_robot_util(RefVoidRobotUtil()), |
||
58 |
m_initSucceeded(false), |
||
59 |
m_emergency_stop_triggered(false), |
||
60 |
m_is_first_iter(true), |
||
61 |
m_iter(0), |
||
62 |
✓✗ | 1 |
m_sleep_time(0.0) { |
63 |
//~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS); |
||
64 |
|||
65 |
/* Commands. */ |
||
66 |
✓✗✓✗ |
1 |
addCommand("init", |
67 |
✓✗ | 1 |
makeCommandVoid3(*this, &ParameterServer::init, |
68 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
docCommandVoid3("Initialize the entity.", |
69 |
"Time period in seconds (double)", |
||
70 |
"URDF file path (string)", |
||
71 |
"Robot reference (string)"))); |
||
72 |
✓✗✓✗ |
1 |
addCommand( |
73 |
"init_simple", |
||
74 |
✓✗ | 1 |
makeCommandVoid1(*this, &ParameterServer::init_simple, |
75 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Initialize the entity.", |
76 |
"Time period in seconds (double)"))); |
||
77 |
|||
78 |
✓✗✓✗ |
1 |
addCommand("setNameToId", |
79 |
✓✗ | 1 |
makeCommandVoid2( |
80 |
*this, &ParameterServer::setNameToId, |
||
81 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set map for a name to an Id", |
82 |
"(string) joint name", "(double) joint id"))); |
||
83 |
|||
84 |
✓✗✓✗ |
1 |
addCommand( |
85 |
"setForceNameToForceId", |
||
86 |
✓✗ | 1 |
makeCommandVoid2( |
87 |
*this, &ParameterServer::setForceNameToForceId, |
||
88 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2( |
89 |
"Set map from a force sensor name to a force sensor Id", |
||
90 |
"(string) force sensor name", "(double) force sensor id"))); |
||
91 |
|||
92 |
✓✗✓✗ |
1 |
addCommand("setJointLimitsFromId", |
93 |
✓✗ | 1 |
makeCommandVoid3( |
94 |
*this, &ParameterServer::setJointLimitsFromId, |
||
95 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
docCommandVoid3("Set the joint limits for a given joint ID", |
96 |
"(double) joint id", "(double) lower limit", |
||
97 |
"(double) uppper limit"))); |
||
98 |
|||
99 |
✓✗✓✗ |
1 |
addCommand( |
100 |
"setForceLimitsFromId", |
||
101 |
✓✗ | 1 |
makeCommandVoid3( |
102 |
*this, &ParameterServer::setForceLimitsFromId, |
||
103 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
docCommandVoid3("Set the force limits for a given force sensor ID", |
104 |
"(double) force sensor id", "(double) lower limit", |
||
105 |
"(double) uppper limit"))); |
||
106 |
|||
107 |
✓✗✓✗ |
1 |
addCommand( |
108 |
"setJointsUrdfToSot", |
||
109 |
✓✗ | 1 |
makeCommandVoid1(*this, &ParameterServer::setJoints, |
110 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Map Joints From URDF to SoT.", |
111 |
"Vector of integer for mapping"))); |
||
112 |
|||
113 |
✓✗✓✗ |
1 |
addCommand( |
114 |
"setRightFootSoleXYZ", |
||
115 |
✓✗ | 1 |
makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ, |
116 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the right foot sole 3d position.", |
117 |
"Vector of 3 doubles"))); |
||
118 |
✓✗✓✗ |
1 |
addCommand( |
119 |
"setRightFootForceSensorXYZ", |
||
120 |
✓✗ | 1 |
makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ, |
121 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the right foot sensor 3d position.", |
122 |
"Vector of 3 doubles"))); |
||
123 |
|||
124 |
✓✗✓✗ |
1 |
addCommand("setFootFrameName", |
125 |
✓✗ | 1 |
makeCommandVoid2( |
126 |
*this, &ParameterServer::setFootFrameName, |
||
127 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set the Frame Name for the Foot Name.", |
128 |
"(string) Foot name", "(string) Frame name"))); |
||
129 |
✓✗✓✗ |
1 |
addCommand("setHandFrameName", |
130 |
✓✗ | 1 |
makeCommandVoid2( |
131 |
*this, &ParameterServer::setHandFrameName, |
||
132 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set the Frame Name for the Hand Name.", |
133 |
"(string) Hand name", "(string) Frame name"))); |
||
134 |
✓✗✓✗ |
1 |
addCommand("setImuJointName", |
135 |
✓✗ | 1 |
makeCommandVoid1( |
136 |
*this, &ParameterServer::setImuJointName, |
||
137 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Set the Joint Name wich IMU is attached to.", |
138 |
"(string) Joint name"))); |
||
139 |
✓✗✓✗ |
1 |
addCommand("displayRobotUtil", |
140 |
✓✗ | 1 |
makeCommandVoid0( |
141 |
*this, &ParameterServer::displayRobotUtil, |
||
142 |
✓✗✓✗ |
2 |
docCommandVoid0("Display the current robot util data set."))); |
143 |
|||
144 |
✓✗✓✗ |
1 |
addCommand( |
145 |
"setParameterBool", |
||
146 |
✓✗ | 1 |
makeCommandVoid2( |
147 |
*this, &ParameterServer::setParameter<bool>, |
||
148 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set a parameter named ParameterName to value " |
149 |
"ParameterValue (string format).", |
||
150 |
"(string) ParameterName", "(bool) ParameterValue"))); |
||
151 |
✓✗✓✗ |
1 |
addCommand( |
152 |
"setParameterInt", |
||
153 |
✓✗ | 1 |
makeCommandVoid2( |
154 |
*this, &ParameterServer::setParameter<int>, |
||
155 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set a parameter named ParameterName to value " |
156 |
"ParameterValue (string format).", |
||
157 |
"(string) ParameterName", "(int) ParameterValue"))); |
||
158 |
✓✗✓✗ |
1 |
addCommand("setParameterDbl", |
159 |
✓✗ | 1 |
makeCommandVoid2( |
160 |
*this, &ParameterServer::setParameter<double>, |
||
161 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set a parameter named ParameterName to value " |
162 |
"ParameterValue (string format).", |
||
163 |
"(string) ParameterName", |
||
164 |
"(double) ParameterValue"))); |
||
165 |
|||
166 |
✓✗✓✗ |
1 |
addCommand("setParameter", |
167 |
✓✗ | 1 |
makeCommandVoid2( |
168 |
*this, &ParameterServer::setParameter<std::string>, |
||
169 |
✓✗✓✗ ✓✗✓✗ |
2 |
docCommandVoid2("Set a parameter named ParameterName to value " |
170 |
"ParameterValue (string format).", |
||
171 |
"(string) ParameterName", |
||
172 |
"(string) ParameterValue"))); |
||
173 |
|||
174 |
✓✗✓✗ |
1 |
addCommand( |
175 |
"getParameter", |
||
176 |
✓✗ | 1 |
makeCommandReturnType1(*this, &ParameterServer::getParameter<std::string>, |
177 |
✓✗✓✗ ✓✗ |
2 |
docCommandReturnType1<std::string>( |
178 |
"Return the parameter value for parameter" |
||
179 |
" named ParameterName.", |
||
180 |
"(string) ParameterName"))); |
||
181 |
|||
182 |
✓✗✓✗ |
1 |
addCommand( |
183 |
"getParameterInt", |
||
184 |
✓✗ | 1 |
makeCommandReturnType1( |
185 |
*this, &ParameterServer::getParameter<int>, |
||
186 |
✓✗✓✗ ✓✗ |
2 |
docCommandReturnType1<int>("Return the parameter value for parameter" |
187 |
" named ParameterName.", |
||
188 |
"(int) ParameterName"))); |
||
189 |
|||
190 |
✓✗✓✗ |
1 |
addCommand( |
191 |
"getParameterDbl", |
||
192 |
✓✗ | 1 |
makeCommandReturnType1(*this, &ParameterServer::getParameter<double>, |
193 |
✓✗✓✗ ✓✗ |
2 |
docCommandReturnType1<double>( |
194 |
"Return the parameter value for parameter" |
||
195 |
" named ParameterName.", |
||
196 |
"(double) ParameterName"))); |
||
197 |
|||
198 |
✓✗✓✗ |
1 |
addCommand( |
199 |
"getParameterBool", |
||
200 |
✓✗ | 1 |
makeCommandReturnType1( |
201 |
*this, &ParameterServer::getParameter<bool>, |
||
202 |
✓✗✓✗ ✓✗ |
2 |
docCommandReturnType1<bool>("Return the parameter value for parameter" |
203 |
" named ParameterName.", |
||
204 |
"(string) ParameterName"))); |
||
205 |
1 |
} |
|
206 |
|||
207 |
void ParameterServer::init_simple(const double &dt) { |
||
208 |
if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); |
||
209 |
|||
210 |
m_dt = dt; |
||
211 |
|||
212 |
m_emergency_stop_triggered = false; |
||
213 |
m_initSucceeded = true; |
||
214 |
|||
215 |
std::string localName("robot"); |
||
216 |
std::shared_ptr<std::vector<std::string> > listOfRobots = |
||
217 |
sot::getListOfRobots(); |
||
218 |
|||
219 |
if (listOfRobots->size() == 1) |
||
220 |
localName = (*listOfRobots)[0]; |
||
221 |
else { |
||
222 |
std::ostringstream oss; |
||
223 |
oss << "No robot registered in the parameter server list"; |
||
224 |
oss << " listOfRobots->size: " << listOfRobots->size(); |
||
225 |
throw ExceptionTools(ExceptionTools::ErrorCodeEnum::PARAMETER_SERVER, |
||
226 |
oss.str()); |
||
227 |
} |
||
228 |
|||
229 |
if (!isNameInRobotUtil(localName)) { |
||
230 |
m_robot_util = createRobotUtil(localName); |
||
231 |
} else { |
||
232 |
m_robot_util = getRobotUtil(localName); |
||
233 |
} |
||
234 |
|||
235 |
addCommand( |
||
236 |
"getJointsUrdfToSot", |
||
237 |
makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot, |
||
238 |
docDirectSetter("Display map Joints From URDF to SoT.", |
||
239 |
"Vector of integer for mapping"))); |
||
240 |
} |
||
241 |
|||
242 |
1 |
void ParameterServer::init(const double &dt, const std::string &urdfFile, |
|
243 |
const std::string &robotRef) { |
||
244 |
✗✓✗✗ ✗✗✗✗ |
1 |
if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); |
245 |
1 |
m_dt = dt; |
|
246 |
1 |
m_emergency_stop_triggered = false; |
|
247 |
1 |
m_initSucceeded = true; |
|
248 |
|||
249 |
✓✗ | 2 |
std::string localName(robotRef); |
250 |
✓✗✓✗ |
1 |
if (!isNameInRobotUtil(localName)) { |
251 |
✓✗ | 1 |
m_robot_util = createRobotUtil(localName); |
252 |
} else { |
||
253 |
m_robot_util = getRobotUtil(localName); |
||
254 |
} |
||
255 |
|||
256 |
✓✗ | 1 |
m_robot_util->m_urdf_filename = urdfFile; |
257 |
} |
||
258 |
|||
259 |
/* ------------------------------------------------------------------- */ |
||
260 |
/* --- SIGNALS ------------------------------------------------------- */ |
||
261 |
/* ------------------------------------------------------------------- */ |
||
262 |
|||
263 |
/* --- COMMANDS ---------------------------------------------------------- */ |
||
264 |
|||
265 |
void ParameterServer::setNameToId(const std::string &jointName, |
||
266 |
const double &jointId) { |
||
267 |
if (!m_initSucceeded) { |
||
268 |
SEND_WARNING_STREAM_MSG( |
||
269 |
"Cannot set joint name from joint id before initialization!"); |
||
270 |
return; |
||
271 |
} |
||
272 |
m_robot_util->set_name_to_id(jointName, static_cast<Index>(jointId)); |
||
273 |
} |
||
274 |
|||
275 |
void ParameterServer::setJointLimitsFromId(const double &jointId, |
||
276 |
const double &lq, const double &uq) { |
||
277 |
if (!m_initSucceeded) { |
||
278 |
SEND_WARNING_STREAM_MSG( |
||
279 |
"Cannot set joints limits from joint id before initialization!"); |
||
280 |
return; |
||
281 |
} |
||
282 |
|||
283 |
m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq); |
||
284 |
} |
||
285 |
|||
286 |
void ParameterServer::setForceLimitsFromId(const double &jointId, |
||
287 |
const dynamicgraph::Vector &lq, |
||
288 |
const dynamicgraph::Vector &uq) { |
||
289 |
if (!m_initSucceeded) { |
||
290 |
SEND_WARNING_STREAM_MSG( |
||
291 |
"Cannot set force limits from force id before initialization!"); |
||
292 |
return; |
||
293 |
} |
||
294 |
|||
295 |
m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq); |
||
296 |
} |
||
297 |
|||
298 |
void ParameterServer::setForceNameToForceId(const std::string &forceName, |
||
299 |
const double &forceId) { |
||
300 |
if (!m_initSucceeded) { |
||
301 |
SEND_WARNING_STREAM_MSG( |
||
302 |
"Cannot set force sensor name from force sensor id " |
||
303 |
" before initialization!"); |
||
304 |
return; |
||
305 |
} |
||
306 |
|||
307 |
m_robot_util->m_force_util.set_name_to_force_id(forceName, |
||
308 |
static_cast<Index>(forceId)); |
||
309 |
} |
||
310 |
|||
311 |
void ParameterServer::setJoints(const dynamicgraph::Vector &urdf_to_sot) { |
||
312 |
if (!m_initSucceeded) { |
||
313 |
SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!"); |
||
314 |
return; |
||
315 |
} |
||
316 |
m_robot_util->set_urdf_to_sot(urdf_to_sot); |
||
317 |
} |
||
318 |
|||
319 |
void ParameterServer::setRightFootSoleXYZ(const dynamicgraph::Vector &xyz) { |
||
320 |
if (!m_initSucceeded) { |
||
321 |
SEND_WARNING_STREAM_MSG( |
||
322 |
"Cannot set right foot sole XYZ before initialization!"); |
||
323 |
return; |
||
324 |
} |
||
325 |
|||
326 |
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz; |
||
327 |
} |
||
328 |
|||
329 |
void ParameterServer::setRightFootForceSensorXYZ( |
||
330 |
const dynamicgraph::Vector &xyz) { |
||
331 |
if (!m_initSucceeded) { |
||
332 |
SEND_WARNING_STREAM_MSG( |
||
333 |
"Cannot set right foot force sensor XYZ before initialization!"); |
||
334 |
return; |
||
335 |
} |
||
336 |
|||
337 |
m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz; |
||
338 |
} |
||
339 |
|||
340 |
void ParameterServer::setFootFrameName(const std::string &FootName, |
||
341 |
const std::string &FrameName) { |
||
342 |
if (!m_initSucceeded) { |
||
343 |
SEND_WARNING_STREAM_MSG("Cannot set foot frame name!"); |
||
344 |
return; |
||
345 |
} |
||
346 |
if (FootName == "Left") |
||
347 |
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName; |
||
348 |
else if (FootName == "Right") |
||
349 |
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName; |
||
350 |
else |
||
351 |
SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName); |
||
352 |
} |
||
353 |
|||
354 |
void ParameterServer::setHandFrameName(const std::string &HandName, |
||
355 |
const std::string &FrameName) { |
||
356 |
if (!m_initSucceeded) { |
||
357 |
SEND_WARNING_STREAM_MSG("Cannot set hand frame name!"); |
||
358 |
return; |
||
359 |
} |
||
360 |
if (HandName == "Left") |
||
361 |
m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName; |
||
362 |
else if (HandName == "Right") |
||
363 |
m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName; |
||
364 |
else |
||
365 |
SEND_WARNING_STREAM_MSG( |
||
366 |
"Available hand names are 'Left' and 'Right', not '" + HandName + |
||
367 |
"' !"); |
||
368 |
} |
||
369 |
|||
370 |
void ParameterServer::setImuJointName(const std::string &JointName) { |
||
371 |
if (!m_initSucceeded) { |
||
372 |
SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!"); |
||
373 |
return; |
||
374 |
} |
||
375 |
m_robot_util->m_imu_joint_name = JointName; |
||
376 |
} |
||
377 |
|||
378 |
void ParameterServer::displayRobotUtil() { m_robot_util->display(std::cout); } |
||
379 |
|||
380 |
/* --- PROTECTED MEMBER METHODS |
||
381 |
* ---------------------------------------------------------- */ |
||
382 |
|||
383 |
bool ParameterServer::convertJointNameToJointId(const std::string &name, |
||
384 |
unsigned int &id) { |
||
385 |
// Check if the joint name exists |
||
386 |
sot::Index jid = m_robot_util->get_id_from_name(name); |
||
387 |
if (jid < 0) { |
||
388 |
SEND_MSG("The specified joint name does not exist: " + name, |
||
389 |
MSG_TYPE_ERROR); |
||
390 |
std::stringstream ss; |
||
391 |
for (long unsigned int it = 0; it < m_robot_util->m_nbJoints; it++) |
||
392 |
ss << m_robot_util->get_name_from_id(it) << ", "; |
||
393 |
SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO); |
||
394 |
return false; |
||
395 |
} |
||
396 |
id = (unsigned int)jid; |
||
397 |
return true; |
||
398 |
} |
||
399 |
|||
400 |
bool ParameterServer::isJointInRange(unsigned int id, double q) { |
||
401 |
const JointLimits &JL = m_robot_util->get_joint_limits_from_id((Index)id); |
||
402 |
|||
403 |
double jl = JL.lower; |
||
404 |
if (q < jl) { |
||
405 |
SEND_MSG("Desired joint angle " + toString(q) + |
||
406 |
" is smaller than lower limit: " + toString(jl), |
||
407 |
MSG_TYPE_ERROR); |
||
408 |
return false; |
||
409 |
} |
||
410 |
double ju = JL.upper; |
||
411 |
if (q > ju) { |
||
412 |
SEND_MSG("Desired joint angle " + toString(q) + |
||
413 |
" is larger than upper limit: " + toString(ju), |
||
414 |
MSG_TYPE_ERROR); |
||
415 |
return false; |
||
416 |
} |
||
417 |
return true; |
||
418 |
} |
||
419 |
|||
420 |
/* ------------------------------------------------------------------- */ |
||
421 |
/* --- ENTITY -------------------------------------------------------- */ |
||
422 |
/* ------------------------------------------------------------------- */ |
||
423 |
|||
424 |
void ParameterServer::display(std::ostream &os) const { |
||
425 |
os << "ParameterServer " << getName(); |
||
426 |
} |
||
427 |
} // namespace sot |
||
428 |
} // namespace dynamicgraph |
| Generated by: GCOVR (Version 4.2) |