Add command to initialize the robot

    * include/sot-dynamic/dynamic.h,
    * src/dynamic-command.h,
    * src/dynamic.cpp,
    * src/dynamic_graph/sot/dynamics/parser.py.

include/sot-dynamic/dynamic.h

@@ -71,6 +71,7 @@ namespace dg = dynamicgraph;
     class SetFiles;
     class Parse;
     class CreateOpPoint;
+    class InitializeRobot;
   }
 /* --------------------------------------------------------------------- */
 /* --- CLASS ----------------------------------------------------------- */
@@ -89,6 +90,8 @@ class SOTDYNAMIC_EXPORT Dynamic
   friend class sot::command::SetFiles;
   friend class sot::command::Parse;
   friend class sot::command::CreateOpPoint;
+  friend class sot::command::InitializeRobot;
+
  public:
   static const std::string CLASS_NAME;
 

src/dynamic-command.h

@@ -472,6 +472,27 @@ namespace sot {
 	return Value();
       }
     }; // class SetGazeParameters
+
+    // Command InitializeRobot
+    class InitializeRobot : public Command
+    {
+    public:
+      virtual ~InitializeRobot() {}
+      /// Create command and store it in Entity
+      /// \param entity instance of Entity owning this command
+      /// \param docstring documentation of the command
+      InitializeRobot(Dynamic& entity, const std::string& docstring) :
+	Command(entity, std::vector<Value::Type>(),
+		docstring)
+      {
+      }
+      virtual Value doExecute()
+      {
+	Dynamic& robot = static_cast<Dynamic&>(owner());
+	robot.m_HDR->initialize();
+	return Value();
+      }
+    }; // class InitializeRobot
   } // namespace command
 } //namespace sot
 

src/dynamic.cpp

@@ -326,6 +326,12 @@ Dynamic( const std::string & name, bool build )
     addCommand("setGazeParameters",
 	       new command::SetGazeParameters(*this, docstring));
 
+    docstring = "    \n"
+      "    Initialize kinematic chain of robot\n"
+      "    \n";
+    addCommand("initializeRobot",
+	       new command::InitializeRobot(*this, docstring));
+
   sotDEBUGOUT(5);
 }
 

src/dynamic_graph/sot/dynamics/parser.py

@@ -114,6 +114,7 @@ class Parser (object):
                          self.GAZEDIRECTIONZ)
 
         self.entity.setGazeParameters(gazeOrigin, gazeDirection)
+        self.entity.initializeRobot()
         return self.entity
 
     def createJoint (self, node, parentName):