Add commands to build a robot

    * include/sot-dynamic/dynamic.h,
    * src/dynamic-command.h,
    * src/dynamic.cpp.

include/sot-dynamic/dynamic.h

@@ -25,6 +25,10 @@
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
+/* STD */
+#include <string>
+#include <map>
+
 /* Matrix */
 #include <jrl/mal/boost.hh>
 #include "jrl/mal/matrixabstractlayer.hh"
@@ -33,6 +37,7 @@ namespace ml = maal::boost;
 /* JRL dynamic */
 #include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
 #include <jrl/dynamics/dynamicsfactory.hh>
+
 namespace djj = dynamicsJRLJapan;
 
 /* SOT */
@@ -44,9 +49,6 @@ namespace djj = dynamicsJRLJapan;
 #include <sot-core/exception-dynamic.h>
 #include <sot-core/matrix-homogeneous.h>
 
-/* STD */
-#include <string>
-
 /* --------------------------------------------------------------------- */
 /* --- API ------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
@@ -234,6 +236,103 @@ class SOTDYNAMIC_EXPORT Dynamic
 			    std::istringstream& cmdArgs,
 			    std::ostream& os );
     
+ public:
+  /// \name Construction of a robot by commands
+  ///@{
+  ///
+
+  /// \brief Create an empty device
+  void createRobot();
+
+  /// \brief create a joint
+  /// \param inJointName name of the joint,
+  /// \param inJointType type of joint in ["freeflyer","rotation","translation","anchor"],
+  /// \param inPosition position of the joint (4x4 homogeneous matrix).
+  /// 
+  /// \note joints are stored in a map with names as keys for retrieval by other
+  /// commands. An empty CjrlBody is also created and attached to the joint.
+  void createJoint(const std::string& inJointName,
+		   const std::string& inJointType,
+		   const ml::Matrix& inPosition);
+
+  /// \brief Set a joint as root joint of the robot.
+  void setRootJoint(const std::string& inJointName);
+
+  /// \brief Add a child joint to a joint.
+  /// \param inParentName name of the joint to which a child is added.
+  /// \param inChildName name of the child joint added.
+  void addJoint(const std::string& inParentName,
+		const std::string& inChildName);
+
+  /// \brief Set bound of degree of freedom
+  ///
+  /// \param inJointName name of the joint,
+  /// \param inDofId id of the degree of freedom in the joint,
+  /// \param inMinValue, inMaxValue values of degree of freedom bounds
+
+  void setDofBounds(const std::string& inJointName, unsigned int inDofId,
+		    double inMinValue, double inMaxValue);
+
+  /// \brief Set mass of a body
+  ///
+  /// \param inJointName name of the joint to which the body is attached,
+  /// \param inMass mass of the body
+  void setMass(const std::string& inJointName, double inMass);
+
+  /// \brief Set local center of mass of a body
+  ///
+  /// \param inJointName name of the joint to which the body is attached,
+  /// \param inCom local center of mass.
+  void setLocalCenterOfMass(const std::string& inJointName, ml::Vector inCom);
+
+  /// \brief Set inertia matrix of a body
+  ///
+  /// \param inJointName name of the joint to which the body is attached,
+  /// \param inMatrix inertia matrix.
+  void setInertiaMatrix(const std::string& inJointName,
+			ml::Matrix inMatrix);
+
+  /// \brief Set specific joints
+  ///
+  /// \param inJointName name of the joint,
+  /// \param inJointType type of joint in ['chest','waist','left-wrist','right-wrist','left-ankle','right-ankle','gaze'].
+  void setSpecificJoint(const std::string& inJointName,
+			const std::string& inJointType);
+
+  /// \brief Set hand parameters
+  ///
+  /// \param inRight true if right hand, false if left hand,
+  /// \param inCenter center of the hand in wrist local frame,
+  /// \param inThumbAxis thumb axis in wrist local frame,
+  /// \param inForefingerAxis forefinger axis in wrist local frame,
+  /// \param inPalmNormalAxis palm normal in wrist local frame,
+  void setHandParameters(bool inRight, const ml::Vector& inCenter,
+			 const ml::Vector& inThumbAxis,
+			 const ml::Vector& inForefingerAxis,
+			 const ml::Vector& inPalmNormal);
+
+  /// \brief Set foot parameters
+  ///
+  /// \param inRight true if right foot, false if left foot,
+  /// \param inSoleLength sole length,
+  /// \param inSoleWidth sole width,
+  /// \param inAnklePosition ankle position in foot local frame,
+  void setFootParameters(bool inRight, const double& inSoleLength,
+			 const double& inSoleWidth,
+			 const ml::Vector& inAnklePosition);
+
+  /// \brief Set gaze parameters
+  ///
+  /// \param inGazeOrigin origin of the gaze in gaze joint local frame,
+  /// \param inGazeDirection direction of the gase in gaze joint local frame.
+  void setGazeParameters(const ml::Vector& inGazeOrigin,
+			 const ml::Vector& inGazeDirection);
+  /// @}
+  ///
+ private:
+  /// \brief map of joints in construction.
+  std::map<std::string, CjrlJoint*> jointMap_;
+  djj::ObjectFactory factory_;
 
 };
 
@@ -243,4 +342,3 @@ class SOTDYNAMIC_EXPORT Dynamic
 
 
 #endif // #ifndef __SOT_DYNAMIC_H__
-

src/dynamic-command.h

@@ -182,6 +182,296 @@ namespace sot {
       }
     }; // class GetProperty
 
+    // Command CreateRobot
+    class CreateRobot : public Command
+    {
+    public:
+      virtual ~CreateRobot() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      CreateRobot(Dynamic& entity, const std::string& docstring) :
+	Command(entity, std::vector<Value::Type>(),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	robot.createRobot();
+	return Value();
+      }
+    }; // class CreateRobot
+
+    // Command CreateJoint
+    class CreateJoint : public Command
+    {
+    public:
+      virtual ~CreateJoint() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      CreateJoint(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)
+		(Value::MATRIX), docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string jointName = values[0].value();
+	std::string jointType = values[1].value();
+	maal::boost::Matrix position = values[2].value();
+	robot.createJoint(jointName, jointType, position);
+	return Value();
+      }
+    }; // class CreateJoint
+
+    // Command SetRootJoint
+    class SetRootJoint : public Command
+    {
+    public:
+      virtual ~SetRootJoint() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetRootJoint(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 jointName = values[0].value();
+	robot.setRootJoint(jointName);
+	return Value();
+      }
+    }; // class SetRootJoint
+
+    // Command AddJoint
+    class AddJoint : public Command
+    {
+    public:
+      virtual ~AddJoint() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      AddJoint(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string parentName = values[0].value();
+	std::string childName = values[1].value();
+	robot.addJoint(parentName, childName);
+	return Value();
+      }
+    }; // class AddJoint
+
+    // Command SetDofBounds
+    class SetDofBounds : public Command
+    {
+    public:
+      virtual ~SetDofBounds() {}
+      /// 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 SetMass
+    class SetMass : public Command
+    {
+    public:
+      virtual ~SetMass() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetMass(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::STRING)(Value::DOUBLE),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string jointName = values[0].value();
+	double mass = values[1].value();
+	robot.setMass(jointName, mass);
+	return Value();
+      }
+    }; // class SetMass
+
+    // Command SetLocalCenterOfMass
+    class SetLocalCenterOfMass : public Command
+    {
+    public:
+      virtual ~SetLocalCenterOfMass() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetLocalCenterOfMass(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::STRING)(Value::VECTOR),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string jointName = values[0].value();
+	ml::Vector com = values[1].value();
+	robot.setLocalCenterOfMass(jointName, com);
+	return Value();
+      }
+    }; // class SetLocalCenterOfMass
+
+    // Command SetInertiaMatrix
+    class SetInertiaMatrix : public Command
+    {
+    public:
+      virtual ~SetInertiaMatrix() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetInertiaMatrix(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::STRING)(Value::MATRIX),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string jointName = values[0].value();
+	ml::Matrix inertiaMatrix = values[1].value();
+	robot.setInertiaMatrix(jointName, inertiaMatrix);
+	return Value();
+      }
+    }; // class SetInertiaMatrix
+
+    // Command SetSpecificJoint
+    class SetSpecificJoint : public Command
+    {
+    public:
+      virtual ~SetSpecificJoint() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetSpecificJoint(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	std::string jointName = values[0].value();
+	std::string jointType = values[1].value();
+	robot.setSpecificJoint(jointName, jointType);
+	return Value();
+      }
+    }; // class SetSpecificJoint
+
+    // Command SetHandParameters
+    class SetHandParameters : public Command
+    {
+    public:
+      virtual ~SetHandParameters() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetHandParameters(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::BOOL)(Value::VECTOR)
+		(Value::VECTOR)(Value::VECTOR)(Value::VECTOR), docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	bool right = values[0].value();
+	ml::Vector center = values[1].value();
+	ml::Vector thumbAxis = values[2].value();
+	ml::Vector forefingerAxis = values[3].value();
+	ml::Vector palmNormalAxis = values[4].value();
+	robot.setHandParameters(right, center, thumbAxis, forefingerAxis,
+				palmNormalAxis);
+	return Value();
+      }
+    }; // class SetHandParameters
+    // Command SetFootParameters
+    class SetFootParameters : public Command
+    {
+    public:
+      virtual ~SetFootParameters() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetFootParameters(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::BOOL)(Value::DOUBLE)
+		(Value::DOUBLE)(Value::VECTOR), docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	bool right = values[0].value();
+	double soleLength = values[1].value();
+	double soleWidth = values[2].value();
+	ml::Vector anklePosition = values[3].value();
+	robot.setFootParameters(right, soleLength, soleWidth, anklePosition);
+	return Value();
+      }
+    }; // class Setfootparameters      
+
+    // Command SetGazeParameters
+    class SetGazeParameters : public Command
+    {
+    public:
+      virtual ~SetGazeParameters() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      SetGazeParameters(Dynamic& entity, const std::string& docstring) :
+	Command(entity, boost::assign::list_of(Value::VECTOR)(Value::VECTOR),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	std::vector<Value> values = getParameterValues();
+	ml::Vector gazeOrigin  = values[0].value();
+	ml::Vector gazeDirection = values[1].value();
+	robot.setGazeParameters(gazeOrigin, gazeDirection);
+	return Value();
+      }
+    }; // class SetGazeParameters
   } // namespace command
 } //namespace sot
 

src/dynamic.cpp

@@ -37,6 +37,37 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Dynamic,"Dynamic");
 
 using namespace std;
 
+static matrix4d maalToMatrix4d(const ml::Matrix& inMatrix)
+{
+  matrix4d homogeneous;
+  for (unsigned int r=0; r<4; r++) {
+    for (unsigned int c=0; c<4; c++) {
+      homogeneous(r,c) = inMatrix(r,c);
+    }
+  }
+  return homogeneous;
+}
+
+static vector3d maalToVector3d(const ml::Vector& inVector)
+{
+  vector3d vector;
+  vector(0) = inVector(0);
+  vector(1) = inVector(1);
+  vector(2) = inVector(2);
+  return vector;
+}
+
+static matrix3d maalToMatrix3d(const ml::Matrix& inMatrix)
+{
+  matrix3d matrix;
+  for (unsigned int r=0; r<3; r++) {
+    for (unsigned int c=0; c<3; c++) {
+      matrix(r,c) = inMatrix(r,c);
+    }
+  }
+  return matrix;
+}
+
 Dynamic::
 Dynamic( const std::string & name, bool build )
   :Entity(name)
@@ -162,8 +193,139 @@ Dynamic( const std::string & name, bool build )
       "        - a string: name of the property,\n"
       "        - a string: value of the property.\n"
       "    \n";
-      addCommand("setProperty",
-		 new command::SetProperty(*this, docstring));
+    addCommand("setProperty", new command::SetProperty(*this, docstring));
+    
+    docstring = "    \n"
+      "    Get a property\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the property,\n"
+      "      Return:\n"
+      "        - a string: value of the property\n";
+    addCommand("getProperty", new command::GetProperty(*this, docstring));
+
+    docstring = "    \n"
+      "    Create an empty robot\n"
+      "    \n";
+    addCommand("createRobot", new command::CreateRobot(*this, docstring));
+
+    docstring = "    \n"
+      "    Create a joint\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the joint,\n"
+      "        - a string: type of the joint in ['freeflyer', 'rotation',\n"
+      "'                   translation', 'anchor'],\n"
+      "        - a matrix: (homogeneous) position of the joint.\n"
+      "    \n";
+    addCommand("createJoint", new command::CreateJoint(*this, docstring));
+
+    docstring = "    \n"
+      "    Set the root joint of the robot\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the joint.\n"
+      "    \n";
+    addCommand("setRootJoint", new command::SetRootJoint(*this, docstring));
+
+    docstring = "    \n"
+      "    Add a child joint to a joint\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the parent joint,\n"
+      "        - a string: name of the child joint.\n"
+      "    \n";
+    addCommand("addJoint", new command::AddJoint(*this, docstring));
+
+    docstring = "    \n"
+      "    Set the bounds of a joint degree of freedom\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: the name of the joint,\n"
+      "        - a non-negative integer: the dof id in the joint\n"
+      "        - a floating point number: the minimal value,\n"
+      "        - a floating point number: the maximal value.\n"
+      "    \n";
+    addCommand("setDofBounds", new command::SetDofBounds(*this, docstring));
+
+    docstring = "    \n"
+      "    Set the mass of the body attached to a joint\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the joint,\n"
+      "        - a floating point number: the mass of the body.\n"
+      "    \n";
+    addCommand("setMass", new command::SetMass(*this, docstring));
+
+    docstring = "    \n"
+      "    Set the position of the center of mass of a body\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the joint,\n"
+      "        - a vector: the coordinate of the center of mass in the local\n"
+      "                    frame of the joint.\n"
+      "    \n";
+    addCommand("setLocalCenterOfMass",
+	       new command::SetLocalCenterOfMass(*this, docstring));
+
+    docstring = "    \n"
+      "    Set inertia matrix of a body attached to a joint\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the joint,\n"
+      "        - a matrix: inertia matrix.\n"
+      "    \n";
+    addCommand("setInertiaMatrix",
+	       new command::SetInertiaMatrix(*this, docstring));
+
+    docstring = "    \n"
+      "    Set specific joints of humanoid robot\n"
+      "    \n"
+      "      Input:\n"
+      "        - a string: name of the joint,\n"
+      "        - a string: type of the joint in ['waist', 'chest',"
+      " 'left-wrist',\n"
+      "                    'right-wrist', 'left-ankle', 'right-ankle',"
+      " 'gaze']\n"
+      "    \n";
+    addCommand("setSpecificJoint",
+	       new command::SetSpecificJoint(*this, docstring));
+
+    docstring = "    \n"
+      "    Set hand parameters\n"
+      "    \n"
+      "      Input:\n"
+      "        - a boolean: whether right hand or not,\n"
+      "        - a vector: the center of the hand in the wrist local frame,\n"
+      "        - a vector: the thumb axis in the wrist local frame,\n"
+      "        - a vector: the forefinger axis in the wrist local frame,\n"
+      "        - a vector: the palm normal in the wrist local frame.\n"
+      "    \n";
+    addCommand("setHandParameters",
+	       new command::SetHandParameters(*this, docstring));
+
+    docstring = "    \n"
+      "    Set foot parameters\n"
+      "    \n"
+      "      Input:\n"
+      "        - a boolean: whether right hand or not,\n"
+      "        - a floating point number: the sole length,\n"
+      "        - a floating point number: the sole width,\n"
+      "        - a vector: the position of the ankle in the foot local frame.\n"
+      "    \n";
+    addCommand("setFootParameters",
+	       new command::SetFootParameters(*this, docstring));
+
+    docstring = "    \n"
+      "    Set parameters of the gaze\n"
+      "    \n"
+      "      Input:\n"
+      "        - a vector: the gaze origin,\n"
+      "        - a vector: the gaze direction.\n"
+      "    \n";
+    addCommand("setGazeParameters",
+	       new command::SetGazeParameters(*this, docstring));
+
   sotDEBUGOUT(5);
 }
 
@@ -1124,5 +1286,252 @@ commandLine( const std::string& cmdLine,
 
 }
 
+void Dynamic::createRobot()
+{