Merge pull request #156 from olivier-stasse/topic/parameter-server-reduced-model

Topic/parameter server reduced model

Author: nim65s

cmake

@@ -167,6 +167,10 @@ template <typename T>
 Vector6d convertVelocity(const MatrixHomogeneous &M,
                          const MatrixHomogeneous &Mdes,
                          const Vector &faNufafbDes);
+
+template <> const std::string FeaturePose<SE3Representation>::CLASS_NAME;
+template <> const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
+
 #if __cplusplus >= 201103L
 extern template class SOT_CORE_DLLAPI FeaturePose<SE3Representation>;
 extern template class SOT_CORE_DLLAPI FeaturePose<R3xSO3Representation>;

include/sot/core/feature-pose.hh

@@ -99,9 +99,31 @@ public:
   /// @}
   /// \name Commands related to the model
   /// @{
+  template <typename Type>
   void setParameter(const std::string &ParameterName,
-                    const std::string &ParameterValue);
-  std::string getParameter(const std::string &ParameterName);
+                    const Type &ParameterValue) {
+    if (!m_initSucceeded) {
+      DYNAMIC_GRAPH_ENTITY_WARNING(*this)
+          << "Cannot set parameter " << ParameterName << " to "
+          << ParameterValue << " before initialization!\n";
+      return;
+    }
+
+    m_robot_util->set_parameter<Type>(ParameterName, ParameterValue);
+  }
+
+  template <typename Type> Type getParameter(const std::string &ParameterName) {
+
+    if (!m_initSucceeded) {
+      DYNAMIC_GRAPH_ENTITY_WARNING(*this)
+          << "Cannot get parameter " << ParameterName
+          << " before initialization!\n";
+      Type ParameterValue;
+      return ParameterValue;
+    }
+    return m_robot_util->get_parameter<Type>(ParameterName);
+  }
+
   /// @}
   /// Set the mapping between urdf and sot.
   void setJoints(const dynamicgraph::Vector &);

include/sot/core/parameter-server.hh

@@ -11,6 +11,39 @@
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
+
+/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
+    This happen to be not working when including the boost property_tree
+   library. For this reason if defined, the current value of
+   BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
+    Once the property_tree included the pinocchio value of this variable is
+    restored.
+ */
+
+#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
+#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#define UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
+#undef BOOST_MPL_LIMIT_VECTOR_SIZE
+#endif
+
+#ifdef BOOST_MPL_LIMIT_LIST_SIZE
+#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
+#define UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
+#undef BOOST_MPL_LIMIT_LIST_SIZE
+#endif
+
+#include <boost/property_tree/ptree.hpp>
+
+#ifdef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
+#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#undef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
+#endif
+
+#ifdef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
+#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
+#undef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
+#endif
+
 #include "boost/assign.hpp"
 #include <dynamic-graph/linear-algebra.h>
 #include <dynamic-graph/logger.h>
@@ -32,6 +65,25 @@ struct SOT_CORE_EXPORT JointLimits {
 
 typedef Eigen::VectorXd::Index Index;
 
+class SOT_CORE_EXPORT ExtractJointMimics {
+
+public:
+  /// Constructor
+  ExtractJointMimics(std::string &robot_model);
+
+  /// Get mimic joints.
+  const std::vector<std::string> &get_mimic_joints();
+
+private:
+  void go_through(boost::property_tree::ptree &pt, int level, int stage);
+
+  // Create empty property tree object
+  boost::property_tree::ptree tree_;
+  std::vector<std::string> mimic_joints_;
+  std::string current_joint_name_;
+  void go_through_full();
+};
+
 struct SOT_CORE_EXPORT ForceLimits {
   Eigen::VectorXd upper;
   Eigen::VectorXd lower;
@@ -217,28 +269,61 @@ public:
       If parameter_name already exists the value is overwritten.
       If not it is inserted.
    */
+  template <typename Type>
   void set_parameter(const std::string &parameter_name,
-                     const std::string &parameter_value);
+                     const Type &parameter_value) {
+    try {
+      typedef boost::property_tree::ptree::path_type path;
+      path apath(parameter_name, '/');
+      property_tree_.put<Type>(apath, parameter_value);
+    } catch (const boost::property_tree::ptree_error &e) {
+      std::ostringstream oss;
+      oss << "Robot utils: parameter path is invalid " << '\n'
+          << " for set_parameter(" << parameter_name << ")\n"
+          << e.what() << std::endl;
+      sendMsg(oss.str(), MSG_TYPE_ERROR);
+      return;
+    }
+  }
 
   /** \brief Get a parameter of type string.
       If parameter_name already exists the value is overwritten.
       If not it is inserted.
       @param parameter_name: Name of the parameter
       Return false if the parameter is not found.
    */
-  const std::string &get_parameter(const std::string &parameter_name);
-
+  template <typename Type>
+  Type get_parameter(const std::string &parameter_name) {
+    try {
+      boost::property_tree::ptree::path_type apath(parameter_name, '/');
+      const Type &res = property_tree_.get<Type>(apath);
+
+      return res;
+    } catch (const boost::property_tree::ptree_error &e) {
+      std::ostringstream oss;
+      oss << "Robot utils: parameter path is invalid " << '\n'
+          << " for get_parameter(" << parameter_name << ")\n"
+          << e.what() << std::endl;
+      sendMsg(oss.str(), MSG_TYPE_ERROR);
+    }
+  }
   /** @} */
+
+  /** Access to property tree directly */
+  boost::property_tree::ptree &get_property_tree();
+
 protected:
   Logger logger_;
 
   /** \brief Map of the parameters: map of strings. */
   std::map<std::string, std::string> parameters_strings_;
 
+  /** \brief Property tree */
+  boost::property_tree::ptree property_tree_;
 }; // struct RobotUtil
 
 /// Accessors - This should be changed to RobotUtilPtrShared
-typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
+typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
 
 RobotUtilShrPtr RefVoidRobotUtil();
 RobotUtilShrPtr getRobotUtil(std::string &robotName);

include/sot/core/robot-utils.hh

@@ -7,7 +7,8 @@
 from dynamic_graph import plug
 from dynamic_graph.entity import Entity
 from dynamic_graph.signal_base import SignalBase
-from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
+from dynamic_graph.sot.core.feature_point6d_relative \
+    import FeaturePoint6dRelative
 
 # Identity matrix of order 4
 I4 = reduce(lambda m, i: m + (i * (0., ) + (1., ) + (3 - i) * (0., ), ),

src/dynamic_graph/sot/core/feature_position_relative.py

@@ -10,10 +10,11 @@
     Compose_R_and_T, ConvolutionTemporal, HomoToMatrix, HomoToRotation,
     HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo,
     Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
-    MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion, PoseQuatToMatrixHomo,
-    MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo,
-    MatrixToQuaternion, MatrixToRPY, MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo,
-    MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector,
+    MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
+    PoseQuatToMatrixHomo, MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta,
+    MatrixToHomo, MatrixToQuaternion, MatrixToRPY, MatrixToUTheta,
+    MatrixHomoToSE3Vector, SE3VectorToMatrixHomo, MatrixTranspose,
+    Multiply_double_vector, Multiply_matrix_vector,
     Multiply_matrixTwist_vector, Multiply_matrixHomo_vector,
     Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo,
     Multiply_of_matrixrotation, Multiply_of_matrixtwist,

src/dynamic_graph/sot/core/math_small_entities.py

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

src/dynamic_graph/sot/core/meta_task_6d.py

@@ -34,7 +34,7 @@ def ref(self, v):
         self.featureDes.errorIN.value = v
 
 
-# --- HELPER FUNCTIONS ---------------------------------------------------------
+# --- HELPER FUNCTIONS --------------------------------------------------------
 def setGain(gain, val):
     if val is not None:
         if isinstance(val, int) or isinstance(val, float):

src/dynamic_graph/sot/core/meta_tasks.py

@@ -1,7 +1,8 @@
 from dynamic_graph import plug
 from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
 from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
-# TODO: this function is imported from meta_tasks_kine in several places, whereas it is defined in meta_tasks
+# TODO: this function is imported from meta_tasks_kine in several places,
+# whereas it is defined in meta_tasks
 from dynamic_graph.sot.core.meta_tasks import gotoNd  # noqa
 from dynamic_graph.sot.core.meta_tasks import MetaTaskCom
 from dynamic_graph.sot.core.task import Task

src/dynamic_graph/sot/core/meta_tasks_kine.py

@@ -99,7 +99,7 @@ def opmodifBase(self, m):
             self.opPointModifBase.activ = True
 
 
-# --- HELPER FUNCTIONS ---------------------------------------------------------
+# --- HELPER FUNCTIONS --------------------------------------------------------
 
 
 def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
@@ -141,20 +141,27 @@ def gotoNdRel(task,
 
 Inherited from MetaTask6d.
 
-The aim of this MetaTask is to give a simple and immediate interface to implement a relative task between two
-operational points of the robot. The new variable "opPointBase" represents in fact the second operational point (the
+The aim of this MetaTask is to give a simple and immediate interface to
+implement a relative task between two
+operational points of the robot. The new variable "opPointBase" represents
+in fact the second operational point (the
 first is inherited from the father class).
 
-It's been decided to reuse (so not to redefine) all methodes from MetaTask6d related to the opPoint to implement the
-behaviour of one of the two points (called "Other" from now on) and to reimplement in a intuitive way the same
+It's been decided to reuse (so not to redefine) all methodes from MetaTask6d
+related to the opPoint to implement the
+behaviour of one of the two points (called "Other" from now on) and to
+reimplement in a intuitive way the same
 functions for the second point ("Ref").
 
 
 Utilization
 
-It should be noticed that both feature and reference are defined as a couple of signals, while normally it would be
-enough define the reference as one signal that represents the diplacement between the two positions. Nevertheless this
-redundant approach allows to a very intuitive and safe usage of the class because the references can be set just using
+It should be noticed that both feature and reference are defined as a
+couple of signals, while normally it would be
+enough define the reference as one signal that represents the diplacement
+between the two positions. Nevertheless this
+redundant approach allows to a very intuitive and safe usage of the class
+because the references can be set just using
 the current position of the two operational points.
 For this reason all the goTo functions have been redefined.