Skip to content

Commit 99bf9db

Browse files
authored
Merge pull request #156 from olivier-stasse/topic/parameter-server-reduced-model
Topic/parameter server reduced model
2 parents b34ea9c + c90869c commit 99bf9db

20 files changed

+345
-97
lines changed

cmake

include/sot/core/feature-pose.hh

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -167,6 +167,10 @@ template <typename T>
167167
Vector6d convertVelocity(const MatrixHomogeneous &M,
168168
const MatrixHomogeneous &Mdes,
169169
const Vector &faNufafbDes);
170+
171+
template <> const std::string FeaturePose<SE3Representation>::CLASS_NAME;
172+
template <> const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
173+
170174
#if __cplusplus >= 201103L
171175
extern template class SOT_CORE_DLLAPI FeaturePose<SE3Representation>;
172176
extern template class SOT_CORE_DLLAPI FeaturePose<R3xSO3Representation>;

include/sot/core/parameter-server.hh

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -99,9 +99,31 @@ public:
9999
/// @}
100100
/// \name Commands related to the model
101101
/// @{
102+
template <typename Type>
102103
void setParameter(const std::string &ParameterName,
103-
const std::string &ParameterValue);
104-
std::string getParameter(const std::string &ParameterName);
104+
const Type &ParameterValue) {
105+
if (!m_initSucceeded) {
106+
DYNAMIC_GRAPH_ENTITY_WARNING(*this)
107+
<< "Cannot set parameter " << ParameterName << " to "
108+
<< ParameterValue << " before initialization!\n";
109+
return;
110+
}
111+
112+
m_robot_util->set_parameter<Type>(ParameterName, ParameterValue);
113+
}
114+
115+
template <typename Type> Type getParameter(const std::string &ParameterName) {
116+
117+
if (!m_initSucceeded) {
118+
DYNAMIC_GRAPH_ENTITY_WARNING(*this)
119+
<< "Cannot get parameter " << ParameterName
120+
<< " before initialization!\n";
121+
Type ParameterValue;
122+
return ParameterValue;
123+
}
124+
return m_robot_util->get_parameter<Type>(ParameterName);
125+
}
126+
105127
/// @}
106128
/// Set the mapping between urdf and sot.
107129
void setJoints(const dynamicgraph::Vector &);

include/sot/core/robot-utils.hh

Lines changed: 89 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,39 @@
1111
/* --------------------------------------------------------------------- */
1212
/* --- INCLUDE --------------------------------------------------------- */
1313
/* --------------------------------------------------------------------- */
14+
15+
/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
16+
This happen to be not working when including the boost property_tree
17+
library. For this reason if defined, the current value of
18+
BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
19+
Once the property_tree included the pinocchio value of this variable is
20+
restored.
21+
*/
22+
23+
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
24+
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
25+
#define UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
26+
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
27+
#endif
28+
29+
#ifdef BOOST_MPL_LIMIT_LIST_SIZE
30+
#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
31+
#define UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
32+
#undef BOOST_MPL_LIMIT_LIST_SIZE
33+
#endif
34+
35+
#include <boost/property_tree/ptree.hpp>
36+
37+
#ifdef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
38+
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
39+
#undef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
40+
#endif
41+
42+
#ifdef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
43+
#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
44+
#undef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
45+
#endif
46+
1447
#include "boost/assign.hpp"
1548
#include <dynamic-graph/linear-algebra.h>
1649
#include <dynamic-graph/logger.h>
@@ -32,6 +65,25 @@ struct SOT_CORE_EXPORT JointLimits {
3265

3366
typedef Eigen::VectorXd::Index Index;
3467

68+
class SOT_CORE_EXPORT ExtractJointMimics {
69+
70+
public:
71+
/// Constructor
72+
ExtractJointMimics(std::string &robot_model);
73+
74+
/// Get mimic joints.
75+
const std::vector<std::string> &get_mimic_joints();
76+
77+
private:
78+
void go_through(boost::property_tree::ptree &pt, int level, int stage);
79+
80+
// Create empty property tree object
81+
boost::property_tree::ptree tree_;
82+
std::vector<std::string> mimic_joints_;
83+
std::string current_joint_name_;
84+
void go_through_full();
85+
};
86+
3587
struct SOT_CORE_EXPORT ForceLimits {
3688
Eigen::VectorXd upper;
3789
Eigen::VectorXd lower;
@@ -217,28 +269,61 @@ public:
217269
If parameter_name already exists the value is overwritten.
218270
If not it is inserted.
219271
*/
272+
template <typename Type>
220273
void set_parameter(const std::string &parameter_name,
221-
const std::string &parameter_value);
274+
const Type &parameter_value) {
275+
try {
276+
typedef boost::property_tree::ptree::path_type path;
277+
path apath(parameter_name, '/');
278+
property_tree_.put<Type>(apath, parameter_value);
279+
} catch (const boost::property_tree::ptree_error &e) {
280+
std::ostringstream oss;
281+
oss << "Robot utils: parameter path is invalid " << '\n'
282+
<< " for set_parameter(" << parameter_name << ")\n"
283+
<< e.what() << std::endl;
284+
sendMsg(oss.str(), MSG_TYPE_ERROR);
285+
return;
286+
}
287+
}
222288

223289
/** \brief Get a parameter of type string.
224290
If parameter_name already exists the value is overwritten.
225291
If not it is inserted.
226292
@param parameter_name: Name of the parameter
227293
Return false if the parameter is not found.
228294
*/
229-
const std::string &get_parameter(const std::string &parameter_name);
230-
295+
template <typename Type>
296+
Type get_parameter(const std::string &parameter_name) {
297+
try {
298+
boost::property_tree::ptree::path_type apath(parameter_name, '/');
299+
const Type &res = property_tree_.get<Type>(apath);
300+
301+
return res;
302+
} catch (const boost::property_tree::ptree_error &e) {
303+
std::ostringstream oss;
304+
oss << "Robot utils: parameter path is invalid " << '\n'
305+
<< " for get_parameter(" << parameter_name << ")\n"
306+
<< e.what() << std::endl;
307+
sendMsg(oss.str(), MSG_TYPE_ERROR);
308+
}
309+
}
231310
/** @} */
311+
312+
/** Access to property tree directly */
313+
boost::property_tree::ptree &get_property_tree();
314+
232315
protected:
233316
Logger logger_;
234317

235318
/** \brief Map of the parameters: map of strings. */
236319
std::map<std::string, std::string> parameters_strings_;
237320

321+
/** \brief Property tree */
322+
boost::property_tree::ptree property_tree_;
238323
}; // struct RobotUtil
239324

240325
/// Accessors - This should be changed to RobotUtilPtrShared
241-
typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
326+
typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
242327

243328
RobotUtilShrPtr RefVoidRobotUtil();
244329
RobotUtilShrPtr getRobotUtil(std::string &robotName);

src/dynamic_graph/sot/core/feature_position_relative.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,8 @@
77
from dynamic_graph import plug
88
from dynamic_graph.entity import Entity
99
from dynamic_graph.signal_base import SignalBase
10-
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
10+
from dynamic_graph.sot.core.feature_point6d_relative \
11+
import FeaturePoint6dRelative
1112

1213
# Identity matrix of order 4
1314
I4 = reduce(lambda m, i: m + (i * (0., ) + (1., ) + (3 - i) * (0., ), ),

src/dynamic_graph/sot/core/math_small_entities.py

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -10,10 +10,11 @@
1010
Compose_R_and_T, ConvolutionTemporal, HomoToMatrix, HomoToRotation,
1111
HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo,
1212
Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
13-
MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion, PoseQuatToMatrixHomo,
14-
MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo,
15-
MatrixToQuaternion, MatrixToRPY, MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo,
16-
MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector,
13+
MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
14+
PoseQuatToMatrixHomo, MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta,
15+
MatrixToHomo, MatrixToQuaternion, MatrixToRPY, MatrixToUTheta,
16+
MatrixHomoToSE3Vector, SE3VectorToMatrixHomo, MatrixTranspose,
17+
Multiply_double_vector, Multiply_matrix_vector,
1718
Multiply_matrixTwist_vector, Multiply_matrixHomo_vector,
1819
Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo,
1920
Multiply_of_matrixrotation, Multiply_of_matrixtwist,

src/dynamic_graph/sot/core/meta_task_6d.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,8 @@
77

88
def toFlags(arr):
99
"""
10-
Convert an array of boolean to a /flag/ format, type 001010110, in little indian
10+
Convert an array of boolean to a /flag/ format, type 001010110,
11+
in little indian
1112
(reverse order, first bool of the list will be the [01] of extrem right).
1213
"""
1314
lres = [0] * (max(arr) + 1)

src/dynamic_graph/sot/core/meta_tasks.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ def ref(self, v):
3434
self.featureDes.errorIN.value = v
3535

3636

37-
# --- HELPER FUNCTIONS ---------------------------------------------------------
37+
# --- HELPER FUNCTIONS --------------------------------------------------------
3838
def setGain(gain, val):
3939
if val is not None:
4040
if isinstance(val, int) or isinstance(val, float):

src/dynamic_graph/sot/core/meta_tasks_kine.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
from dynamic_graph import plug
22
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
33
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
4-
# TODO: this function is imported from meta_tasks_kine in several places, whereas it is defined in meta_tasks
4+
# TODO: this function is imported from meta_tasks_kine in several places,
5+
# whereas it is defined in meta_tasks
56
from dynamic_graph.sot.core.meta_tasks import gotoNd # noqa
67
from dynamic_graph.sot.core.meta_tasks import MetaTaskCom
78
from dynamic_graph.sot.core.task import Task

src/dynamic_graph/sot/core/meta_tasks_kine_relative.py

Lines changed: 15 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -99,7 +99,7 @@ def opmodifBase(self, m):
9999
self.opPointModifBase.activ = True
100100

101101

102-
# --- HELPER FUNCTIONS ---------------------------------------------------------
102+
# --- HELPER FUNCTIONS --------------------------------------------------------
103103

104104

105105
def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
@@ -141,20 +141,27 @@ def gotoNdRel(task,
141141
142142
Inherited from MetaTask6d.
143143
144-
The aim of this MetaTask is to give a simple and immediate interface to implement a relative task between two
145-
operational points of the robot. The new variable "opPointBase" represents in fact the second operational point (the
144+
The aim of this MetaTask is to give a simple and immediate interface to
145+
implement a relative task between two
146+
operational points of the robot. The new variable "opPointBase" represents
147+
in fact the second operational point (the
146148
first is inherited from the father class).
147149
148-
It's been decided to reuse (so not to redefine) all methodes from MetaTask6d related to the opPoint to implement the
149-
behaviour of one of the two points (called "Other" from now on) and to reimplement in a intuitive way the same
150+
It's been decided to reuse (so not to redefine) all methodes from MetaTask6d
151+
related to the opPoint to implement the
152+
behaviour of one of the two points (called "Other" from now on) and to
153+
reimplement in a intuitive way the same
150154
functions for the second point ("Ref").
151155
152156
153157
Utilization
154158
155-
It should be noticed that both feature and reference are defined as a couple of signals, while normally it would be
156-
enough define the reference as one signal that represents the diplacement between the two positions. Nevertheless this
157-
redundant approach allows to a very intuitive and safe usage of the class because the references can be set just using
159+
It should be noticed that both feature and reference are defined as a
160+
couple of signals, while normally it would be
161+
enough define the reference as one signal that represents the diplacement
162+
between the two positions. Nevertheless this
163+
redundant approach allows to a very intuitive and safe usage of the class
164+
because the references can be set just using
158165
the current position of the two operational points.
159166
For this reason all the goTo functions have been redefined.
160167

0 commit comments

Comments
 (0)