Skip to content

Commit 61a23bf

Browse files
olivier-stasseolivier-stasse
committed
[tools] Modifies parameter-server and robot utils to handle tree of parameters.
1 parent 4c33382 commit 61a23bf

File tree

5 files changed

+279
-53
lines changed

5 files changed

+279
-53
lines changed

‎include/sot/core/parameter-server.hh

Lines changed: 30 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
/* --- INCLUDE --------------------------------------------------------- */
3636
/* --------------------------------------------------------------------- */
3737

38+
#include <sstream>
3839
#include "boost/assign.hpp"
3940
#include <dynamic-graph/signal-helper.h>
4041
#include <map>
@@ -99,9 +100,36 @@ public:
99100
/// @}
100101
/// \name Commands related to the model
101102
/// @{
103+
template <typename Type>
102104
void setParameter(const std::string &ParameterName,
103-
const std::string &ParameterValue);
104-
std::string getParameter(const std::string &ParameterName);
105+
const Type &ParameterValue)
106+
{
107+
if (!m_initSucceeded) {
108+
std::ostringstream oss;
109+
oss << ParameterValue;
110+
SEND_WARNING_STREAM_MSG("Cannot set parameter " + ParameterName + " to " +
111+
oss.str().c_str() +
112+
" before initialization!");
113+
return;
114+
}
115+
116+
m_robot_util->set_parameter<Type>(ParameterName, ParameterValue);
117+
}
118+
119+
template <typename Type>
120+
Type getParameter(const std::string &ParameterName)
121+
{
122+
123+
if (!m_initSucceeded) {
124+
SEND_WARNING_STREAM_MSG("Cannot get parameter " + ParameterName +
125+
" before initialization!");
126+
Type ParameterValue;
127+
return ParameterValue;
128+
}
129+
return m_robot_util->get_parameter<Type>(ParameterName);
130+
131+
}
132+
105133
/// @}
106134
/// Set the mapping between urdf and sot.
107135
void setJoints(const dynamicgraph::Vector &);

‎include/sot/core/robot-utils.hh

Lines changed: 92 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,28 @@
1111
/* --------------------------------------------------------------------- */
1212
/* --- INCLUDE --------------------------------------------------------- */
1313
/* --------------------------------------------------------------------- */
14+
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
15+
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
16+
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
17+
#define BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
18+
#endif
19+
20+
#ifdef BOOST_MPL_LIMIT_LIST_SIZE
21+
#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
22+
#undef BOOST_MPL_LIMIT_LIST_SIZE
23+
#define BOOST_MPL_LIMIT_LIST_SIZE_PUSH
24+
#endif
25+
26+
#include <boost/property_tree/ptree.hpp>
27+
28+
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
29+
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
30+
#endif
31+
32+
#ifdef BOOST_MPL_LIMIT_LIST_SIZE_PUSH
33+
#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
34+
#endif
35+
1436
#include "boost/assign.hpp"
1537
#include <dynamic-graph/linear-algebra.h>
1638
#include <dynamic-graph/logger.h>
@@ -32,6 +54,26 @@ struct SOT_CORE_EXPORT JointLimits {
3254

3355
typedef Eigen::VectorXd::Index Index;
3456

57+
class SOT_CORE_EXPORT ExtractJointMimics {
58+
59+
public:
60+
/// Constructor
61+
ExtractJointMimics(std::string & robot_model);
62+
63+
/// Get mimic joints.
64+
const std::vector<std::string> &get_mimic_joints();
65+
66+
private:
67+
void go_through(boost::property_tree::ptree &pt,int level, int stage);
68+
69+
// Create empty property tree object
70+
boost::property_tree::ptree tree_;
71+
std::vector<std::string> mimic_joints_;
72+
std::string current_joint_name_;
73+
void go_through_full();
74+
75+
};
76+
3577
struct SOT_CORE_EXPORT ForceLimits {
3678
Eigen::VectorXd upper;
3779
Eigen::VectorXd lower;
@@ -217,28 +259,74 @@ public:
217259
If parameter_name already exists the value is overwritten.
218260
If not it is inserted.
219261
*/
262+
template < typename Type>
220263
void set_parameter(const std::string &parameter_name,
221-
const std::string &parameter_value);
264+
const Type &parameter_value)
265+
{
266+
try
267+
{
268+
typedef boost::property_tree::ptree::path_type path;
269+
path apath(parameter_name,'/');
270+
property_tree_.put<Type>(apath,parameter_value);
271+
}
272+
catch(const boost::property_tree::ptree_error &e)
273+
{
274+
std::ostringstream oss;
275+
oss << "Robot utils: parameter path is invalid " << '\n'
276+
<< " for set_parameter("
277+
<< parameter_name << ")\n"
278+
<< e.what() << std::endl;
279+
sendMsg(oss.str(),
280+
MSG_TYPE_ERROR);
281+
return;
282+
283+
}
284+
285+
}
222286

223287
/** \brief Get a parameter of type string.
224288
If parameter_name already exists the value is overwritten.
225289
If not it is inserted.
226290
@param parameter_name: Name of the parameter
227291
Return false if the parameter is not found.
228292
*/
229-
const std::string &get_parameter(const std::string &parameter_name);
230-
293+
template <typename Type>
294+
Type get_parameter(const std::string &parameter_name)
295+
{
296+
try {
297+
boost::property_tree::ptree::path_type apath(parameter_name,'/');
298+
const Type & res= property_tree_.get<Type>(apath);
299+
300+
return res;
301+
}
302+
catch(const boost::property_tree::ptree_error &e)
303+
{
304+
std::ostringstream oss;
305+
oss << "Robot utils: parameter path is invalid " << '\n'
306+
<< " for get_parameter("
307+
<< parameter_name << ")\n"
308+
<< e.what() << std::endl;
309+
sendMsg(oss.str(),
310+
MSG_TYPE_ERROR);
311+
}
312+
}
231313
/** @} */
314+
315+
/** Access to property tree directly */
316+
boost::property_tree::ptree & get_property_tree();
317+
232318
protected:
233319
Logger logger_;
234320

235321
/** \brief Map of the parameters: map of strings. */
236322
std::map<std::string, std::string> parameters_strings_;
237323

324+
/** \brief Property tree */
325+
boost::property_tree::ptree property_tree_;
238326
}; // struct RobotUtil
239327

240328
/// Accessors - This should be changed to RobotUtilPtrShared
241-
typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
329+
typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
242330

243331
RobotUtilShrPtr RefVoidRobotUtil();
244332
RobotUtilShrPtr getRobotUtil(std::string &robotName);

‎src/tools/parameter-server.cpp

Lines changed: 78 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,28 @@
1414
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
1515
*/
1616

17+
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
18+
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
19+
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
20+
#define BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
21+
#endif
22+
23+
#ifdef BOOST_MPL_LIMIT_LIST_SIZE
24+
#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
25+
#undef BOOST_MPL_LIMIT_LIST_SIZE
26+
#define BOOST_MPL_LIMIT_LIST_SIZE_PUSH
27+
#endif
28+
29+
#include <boost/property_tree/ptree.hpp>
30+
31+
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
32+
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
33+
#endif
34+
35+
#ifdef BOOST_MPL_LIMIT_LIST_SIZE_PUSH
36+
#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
37+
#endif
38+
1739
#include <dynamic-graph/all-commands.h>
1840
#include <dynamic-graph/factory.h>
1941
#include <iostream>
@@ -128,20 +150,69 @@ ParameterServer::ParameterServer(const std::string &name)
128150
*this, &ParameterServer::displayRobotUtil,
129151
docCommandVoid0("Display the current robot util data set.")));
130152

153+
addCommand("setParameterBool",
154+
makeCommandVoid2(
155+
*this, &ParameterServer::setParameter<bool>,
156+
docCommandVoid2("Set a parameter named ParameterName to value "
157+
"ParameterValue (string format).",
158+
"(string) ParameterName",
159+
"(bool) ParameterValue")));
160+
addCommand("setParameterInt",
161+
makeCommandVoid2(
162+
*this, &ParameterServer::setParameter<int>,
163+
docCommandVoid2("Set a parameter named ParameterName to value "
164+
"ParameterValue (string format).",
165+
"(string) ParameterName",
166+
"(int) ParameterValue")));
167+
addCommand("setParameterDbl",
168+
makeCommandVoid2(
169+
*this, &ParameterServer::setParameter<double>,
170+
docCommandVoid2("Set a parameter named ParameterName to value "
171+
"ParameterValue (string format).",
172+
"(string) ParameterName",
173+
"(double) ParameterValue")));
174+
131175
addCommand("setParameter",
132176
makeCommandVoid2(
133-
*this, &ParameterServer::setParameter,
177+
*this, &ParameterServer::setParameter<std::string>,
134178
docCommandVoid2("Set a parameter named ParameterName to value "
135179
"ParameterValue (string format).",
136180
"(string) ParameterName",
137181
"(string) ParameterValue")));
138182

139-
addCommand("getParameter", makeCommandReturnType1(
140-
*this, &ParameterServer::getParameter,
141-
docCommandReturnType1<std::string>(
142-
"Return the parameter value for parameter"
143-
" named ParameterName.",
144-
"(string) ParameterName")));
183+
addCommand("getParameter",
184+
makeCommandReturnType1(
185+
*this, &ParameterServer::getParameter<std::string>,
186+
docCommandReturnType1<std::string>(
187+
"Return the parameter value for parameter"
188+
" named ParameterName.",
189+
"(string) ParameterName")));
190+
191+
addCommand("getParameterInt",
192+
makeCommandReturnType1(
193+
*this, &ParameterServer::getParameter<int>,
194+
docCommandReturnType1<int>(
195+
"Return the parameter value for parameter"
196+
" named ParameterName.",
197+
"(int) ParameterName")));
198+
199+
addCommand("getParameterDbl",
200+
makeCommandReturnType1(
201+
*this, &ParameterServer::getParameter<double>,
202+
docCommandReturnType1<double>(
203+
"Return the parameter value for parameter"
204+
" named ParameterName.",
205+
"(double) ParameterName")));
206+
207+
addCommand("getParameterBool",
208+
makeCommandReturnType1(
209+
*this, &ParameterServer::getParameter<bool>,
210+
docCommandReturnType1<bool>(
211+
"Return the parameter value for parameter"
212+
" named ParameterName.",
213+
"(string) ParameterName")));
214+
215+
145216
}
146217

147218
void ParameterServer::init_simple(const double &dt) {
@@ -340,29 +411,7 @@ bool ParameterServer::isJointInRange(unsigned int id, double q) {
340411
return true;
341412
}
342413

343-
void ParameterServer::setParameter(const std::string &ParameterName,
344-
const std::string &ParameterValue) {
345414

346-
if (!m_initSucceeded) {
347-
SEND_WARNING_STREAM_MSG("Cannot set parameter " + ParameterName + " to " +
348-
ParameterValue + " before initialization!");
349-
return;
350-
}
351-
m_robot_util->set_parameter(ParameterName, ParameterValue);
352-
}
353-
354-
std::string ParameterServer::getParameter(const std::string &ParameterName) {
355-
std::string ParameterValue("");
356-
357-
if (!m_initSucceeded) {
358-
SEND_WARNING_STREAM_MSG("Cannot get parameter " + ParameterName +
359-
" before initialization!");
360-
return ParameterValue;
361-
}
362-
ParameterValue = m_robot_util->get_parameter(ParameterName);
363-
364-
return ParameterValue;
365-
}
366415

367416
/* ------------------------------------------------------------------- */
368417
/* --- ENTITY -------------------------------------------------------- */

0 commit comments

Comments
 (0)