GCC Code Coverage Report

Directory: ./
File: src/tools/parameter-server.cpp
Date: 2024-11-13 12:35:17
Exec Total Coverage
Lines: 82 187 43.9%
Branches: 147 520 28.3%
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/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer, "ParameterServer");
51
52 /* ------------------------------------------------------------------- */
53 /* --- CONSTRUCTION -------------------------------------------------- */
54 /* ------------------------------------------------------------------- */
55 1 ParameterServer::ParameterServer(const std::string &name)
56 : Entity(name),
57 1 m_robot_util(RefVoidRobotUtil()),
58 1 m_initSucceeded(false),
59 1 m_emergency_stop_triggered(false),
60 1 m_is_first_iter(true),
61 1 m_iter(0),
62
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 m_sleep_time(0.0) {
63 //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
64
65 /* Commands. */
66
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("init",
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid3(*this, &ParameterServer::init,
68
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 2 docCommandVoid3("Initialize the entity.",
69 "Time period in seconds (double)",
70 "URDF file path (string)",
71 "Robot reference (string)")));
72
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
73 "init_simple",
74
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(*this, &ParameterServer::init_simple,
75
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Initialize the entity.",
76 "Time period in seconds (double)")));
77
78
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setNameToId",
79
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
80 *this, &ParameterServer::setNameToId,
81
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set map for a name to an Id",
82 "(string) joint name", "(double) joint id")));
83
84
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
85 "setForceNameToForceId",
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
87 *this, &ParameterServer::setForceNameToForceId,
88
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 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
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setJointLimitsFromId",
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid3(
94 *this, &ParameterServer::setJointLimitsFromId,
95
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 2 docCommandVoid3("Set the joint limits for a given joint ID",
96 "(double) joint id", "(double) lower limit",
97 "(double) uppper limit")));
98
99
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
100 "setForceLimitsFromId",
101
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid3(
102 *this, &ParameterServer::setForceLimitsFromId,
103
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 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
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
108 "setJointsUrdfToSot",
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(*this, &ParameterServer::setJoints,
110
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Map Joints From URDF to SoT.",
111 "Vector of integer for mapping")));
112
113
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
114 "setRightFootSoleXYZ",
115
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
116
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Set the right foot sole 3d position.",
117 "Vector of 3 doubles")));
118
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
119 "setRightFootForceSensorXYZ",
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
121
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Set the right foot sensor 3d position.",
122 "Vector of 3 doubles")));
123
124
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setFootFrameName",
125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
126 *this, &ParameterServer::setFootFrameName,
127
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set the Frame Name for the Foot Name.",
128 "(string) Foot name", "(string) Frame name")));
129
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setHandFrameName",
130
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
131 *this, &ParameterServer::setHandFrameName,
132
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set the Frame Name for the Hand Name.",
133 "(string) Hand name", "(string) Frame name")));
134
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setImuJointName",
135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid1(
136 *this, &ParameterServer::setImuJointName,
137
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Set the Joint Name wich IMU is attached to.",
138 "(string) Joint name")));
139
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("displayRobotUtil",
140
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid0(
141 *this, &ParameterServer::displayRobotUtil,
142
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 docCommandVoid0("Display the current robot util data set.")));
143
144
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
145 "setParameterBool",
146
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
147 *this, &ParameterServer::setParameter<bool>,
148
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set a parameter named ParameterName to value "
149 "ParameterValue (string format).",
150 "(string) ParameterName", "(bool) ParameterValue")));
151
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
152 "setParameterInt",
153
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
154 *this, &ParameterServer::setParameter<int>,
155
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set a parameter named ParameterName to value "
156 "ParameterValue (string format).",
157 "(string) ParameterName", "(int) ParameterValue")));
158
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setParameterDbl",
159
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
160 *this, &ParameterServer::setParameter<double>,
161
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set a parameter named ParameterName to value "
162 "ParameterValue (string format).",
163 "(string) ParameterName",
164 "(double) ParameterValue")));
165
166
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("setParameter",
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid2(
168 *this, &ParameterServer::setParameter<std::string>,
169
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 docCommandVoid2("Set a parameter named ParameterName to value "
170 "ParameterValue (string format).",
171 "(string) ParameterName",
172 "(string) ParameterValue")));
173
174
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
175 "getParameter",
176
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandReturnType1(*this, &ParameterServer::getParameter<std::string>,
177
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandReturnType1<std::string>(
178 "Return the parameter value for parameter"
179 " named ParameterName.",
180 "(string) ParameterName")));
181
182
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
183 "getParameterInt",
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandReturnType1(
185 *this, &ParameterServer::getParameter<int>,
186
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandReturnType1<int>("Return the parameter value for parameter"
187 " named ParameterName.",
188 "(int) ParameterName")));
189
190
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
191 "getParameterDbl",
192
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandReturnType1(*this, &ParameterServer::getParameter<double>,
193
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandReturnType1<double>(
194 "Return the parameter value for parameter"
195 " named ParameterName.",
196 "(double) ParameterName")));
197
198
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand(
199 "getParameterBool",
200
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandReturnType1(
201 *this, &ParameterServer::getParameter<bool>,
202
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 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/8
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 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
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::string localName(robotRef);
250
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (!isNameInRobotUtil(localName)) {
251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 m_robot_util = createRobotUtil(localName);
252 } else {
253 ✗ m_robot_util = getRobotUtil(localName);
254 }
255
256
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 m_robot_util->m_urdf_filename = urdfFile;
257 1 }
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
429