Directory: | ./ |
---|---|
File: | src/tools/parameter-server.cpp |
Date: | 2024-08-13 12:13:25 |
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 |