sot-dynamic with pinocchio dynamic library.

> TODO: Removed operational frames. Add via pinocchio
> TODO: Changed API
> TODO: Expose Pinocchio model class in python

include/sot-dynamic/dynamic.h

@@ -29,12 +29,10 @@
 /* STD */
 #include <string>
 #include <map>
-
 /* Matrix */
 #include <dynamic-graph/linear-algebra.h>
 
 /*Python API*/
-#include <Python.h>
 
 /* SOT */
 #include <sot/core/flags.hh>
@@ -97,12 +95,11 @@ class SOTDYNAMIC_EXPORT Dynamic
   DYNAMIC_GRAPH_ENTITY_DECL();
 
   /*  --- MODEL ATRIBUTES --- */
-  se3::Model*  m_model;
+  se3::Model  m_model;
   se3::Data*   m_data;
   std::string  m_urdfPath;
 
   /*  --- MODEL ATRIBUTES --- */
-  PyObject* getPinocchioModel(PyObject*, PyObject* args);
 
     public:
   /* --- SIGNAL ACTIVATION --- */
@@ -182,13 +179,18 @@ class SOTDYNAMIC_EXPORT Dynamic
   /// \note creates a pinocchio model. needs urdfdom libraries to parse.
   void setUrdfFile( const std::string& path );
 
+  //TODO:
+  //  void parseAndAddFrames(se3::Model& pinocchio_model,
+  //			 const std::string& filename);
+
+
   /// \name Construction of a robot by commands
   ///@{
   ///
 
   /// \brief Create an empty device
   void createRobot();
-  void displayModel(){ std::cout<<*m_model<<std::endl; };
+  void displayModel(){ std::cout<<m_model<<std::endl; };
 
   /// \brief create a joint
   /// \param inJointName name of the joint,
@@ -259,34 +261,54 @@ class SOTDYNAMIC_EXPORT Dynamic
 			    const dg::Matrix& inertia3);
   
     
+  /// \brief Set the bounds of a joint degree of freedom
+  /// \param the name of the joint
+  /// \param  non-negative integer: the dof id in the joint
+  /// \param  the minimal value of the dof
+  /// \param: the maximal value of the dof
+  void setDofBounds(const std::string& inJointName,
+		    const unsigned int inDofId,
+		    const double inMinValue, double inMaxValue);
+    
+
+
   /// \brief Set lower bound of joint position
   ///
   /// \param inJointName name of the joint,
-  /// \param lowPos vector containing lower limit bounds for all dofs of joint.
+  /// \param vector containing lower limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
   void setLowerPositionLimit(const std::string& inJointName,
-				      const dg::Vector& lowPos);
+			     const dg::Vector& lowPos);
+  
+  void setLowerPositionLimit(const std::string& inJointName,
+			     const double lowPos);
   
   /// \brief Set upper bound of joint position
   ///
   /// \param inJointName name of the joint,
-  /// \param upPos vector containing upper limit bounds for all dofs of joint.
+  /// \param upPos vector containing upper limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
+  void setUpperPositionLimit(const std::string& inJointName,
+			     const dg::Vector& upPos);
   void setUpperPositionLimit(const std::string& inJointName,
-				      const dg::Vector& lowPos);
+			     const double upPos);
 
   /// \brief Set upper bound of joint velocities
   ///
   /// \param inJointName name of the joint,
-  /// \param maxVel vector containing upper limit bounds for all dofs of joint.
+  /// \param maxVel vector containing upper limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
   void setMaxVelocityLimit(const std::string& inJointName,
 			   const dg::Vector& maxVel);
+  void setMaxVelocityLimit(const std::string& inJointName,
+			   const double maxVel);
 
 
   /// \brief Set upper bound of joint effort
   ///
   /// \param inJointName name of the joint,
-  /// \param maxEffort vector containing upper limit bounds for all dofs of joint.
+  /// \param maxEffort vector containing upper limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
   void setMaxEffortLimit(const std::string& inJointName,
-				  const dg::Vector& maxEffort);
+			 const dg::Vector& maxEffort);
+  void setMaxEffortLimit(const std::string& inJointName,
+			 const double maxEffort);
   
   /* --- GETTERS --- */
 
@@ -353,6 +375,9 @@ class SOTDYNAMIC_EXPORT Dynamic
   std::map<std::string, JointDetails> jointMap_;
   std::vector<std::string> jointTypes;
 
+
+  std::map<std::string,std::string> specificitiesMap;
+
 };
 
   // std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);

include/sot-dynamic/matrix-inertia.h

@@ -29,13 +29,6 @@
 
 #include <sot/core/matrix-geometry.hh>
 
-namespace dynamicsJRLJapan
-{
-  class HumanoidDynamicMultiBody;
-}
-
-class CjrlHumanoidDynamicRobot;
-class CjrlJoint;
 
 /* --------------------------------------------------------------------- */
 /* --- API ------------------------------------------------------------- */

src/angle-estimator-command.h

@@ -35,7 +35,10 @@ namespace dynamicgraph { namespace sot {
     class FromSensor : public Command
     {
     public:
-      virtual ~FromSensor() {}
+      virtual ~FromSensor() {
+	sotDEBUGIN(15);
+	sotDEBUGOUT(15);
+      }
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command

src/dynamic-command.h

@@ -36,7 +36,8 @@ namespace dynamicgraph { namespace sot {
     class SetFile : public Command
     {
     public:
-      virtual ~SetFile() {}
+      virtual ~SetFile() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -58,7 +59,8 @@ namespace dynamicgraph { namespace sot {
     class CreateRobot : public Command
     {
     public:
-      virtual ~CreateRobot() {}
+      virtual ~CreateRobot() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -79,7 +81,8 @@ namespace dynamicgraph { namespace sot {
     class DisplayModel : public Command
     {
     public:
-      virtual ~DisplayModel() {}
+      virtual ~DisplayModel() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -100,7 +103,8 @@ namespace dynamicgraph { namespace sot {
     class CreateJoint : public Command
     {
     public:
-      virtual ~CreateJoint() {}
+      virtual ~CreateJoint() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -125,7 +129,8 @@ namespace dynamicgraph { namespace sot {
     class AddBody : public Command
     {
     public:
-      virtual ~AddBody() {}
+      virtual ~AddBody() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -150,7 +155,8 @@ namespace dynamicgraph { namespace sot {
     class SetMass : public Command
     {
     public:
-      virtual ~SetMass() {}
+      virtual ~SetMass() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -174,7 +180,8 @@ namespace dynamicgraph { namespace sot {
     class SetLocalCenterOfMass : public Command
     {
     public:
-      virtual ~SetLocalCenterOfMass() {}
+      virtual ~SetLocalCenterOfMass() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -198,7 +205,8 @@ namespace dynamicgraph { namespace sot {
     class SetInertiaMatrix : public Command
     {
     public:
-      virtual ~SetInertiaMatrix() {}
+      virtual ~SetInertiaMatrix() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -222,7 +230,8 @@ namespace dynamicgraph { namespace sot {
     class SetInertiaProperties : public Command
     {
     public:
-      virtual ~SetInertiaProperties() {}
+      virtual ~SetInertiaProperties() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -245,12 +254,39 @@ namespace dynamicgraph { namespace sot {
       }
     }; // class SetInertiaMatrix
 
+    // Command SetDofBounds
+    class SetDofBounds : public Command
+    {
+    public:
+      virtual ~SetDofBounds() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetDofBounds(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::STRING)(Value::UNSIGNED)
+		(Value::DOUBLE)(Value::DOUBLE), docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string jointName = values[0].value();
+	unsigned int dofId = values[1].value();
+	double minValue = values[2].value();
+	double maxValue = values[3].value();
+	robot.setDofBounds(jointName, dofId, minValue, maxValue);
+	return Value();
+      }
+    }; // class SetDofBounds
 
     // Command SetLowerPositionLimit
     class SetLowerPositionLimit : public Command
     {
     public:
-      virtual ~SetLowerPositionLimit() {}
+      virtual ~SetLowerPositionLimit() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -270,11 +306,14 @@ namespace dynamicgraph { namespace sot {
       }
     }; // class SetLowerPositionLimit
 
+
+
     // Command SetUpperPositionLimit
     class SetUpperPositionLimit : public Command
     {
     public:
-      virtual ~SetUpperPositionLimit() {}
+      virtual ~SetUpperPositionLimit() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -298,7 +337,8 @@ namespace dynamicgraph { namespace sot {
     class SetMaxVelocityLimit : public Command
     {
     public:
-      virtual ~SetMaxVelocityLimit() {}
+      virtual ~SetMaxVelocityLimit() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -323,7 +363,8 @@ namespace dynamicgraph { namespace sot {
     class SetMaxEffortLimit : public Command
     {
     public:
-      virtual ~SetMaxEffortLimit() {}
+      virtual ~SetMaxEffortLimit() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
@@ -344,6 +385,54 @@ namespace dynamicgraph { namespace sot {
     }; // class SetMaxEffortLimit
 
 
+    // Command GetDimension
+    class GetDimension : public Command
+    {
+    public:
+      virtual ~GetDimension() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      GetDimension(Dynamic& entity, const std::string& docstring) :
+	Command(entity, std::vector<Value::Type>(),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	unsigned int dimension = robot.m_model.nv;
+	return Value(dimension);
+      }
+    }; // class GetDimension
+
+    // Command Write
+    class Write : public Command
+    {
+    public:
+      virtual ~Write() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      Write(Dynamic& entity, const std::string& docstring) :
+      Command(entity, boost::assign::list_of(Value::STRING),
+	      docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string filename = values[0].value();
+	std::ofstream file(filename.c_str(), std::ios_base::out);
+	file << (robot.m_model);
+	file.close();
+	return Value();
+      }
+    }; // class Write
+
 
   } // namespace command
 } /* namespace sot */} /* namespace dynamicgraph */

src/dynamic.cpp

@@ -21,7 +21,17 @@
 #include <sot-dynamic/dynamic.h>
 #include <dynamic-graph/factory.h>
 
-using namespace dynamicgraph::sot;
 using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Dynamic,"Dynamic");
+extern "C" {
+  ::dynamicgraph::Entity*						
+  EntityMaker_Dynamic(const std::string& objname)			
+  {									
+    return new Dynamic (objname);					
+  }									
+  ::dynamicgraph::EntityRegisterer					
+  reg_Dynamic ("Dynamic",						
+	       &EntityMaker_Dynamic);				
+}
+//DYNAMICGRAPH_FACTORY_DYNAMIC_PLUGIN(Dynamic,"Dynamic");

unitTesting/CMakeLists.txt

@@ -16,7 +16,8 @@
 ADD_DEFINITIONS(-DDEBUG=2)
 
 SET(tests
-dev_test_all_compute
+test_constructor
+test_config
 #  dummy
 #  test_djj
 #  test_dyn