Implement the one robot one sot-library strategy.

src/CMakeLists.txt

@@ -58,27 +58,37 @@ SET(PYTHON_MODULE_BUILD_DIR
 SET(PYTHON_MODULE dynamic_graph/sot/hrp2)
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "hrp2.py" )
-
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py" )
 SET(FILES __init__.py robot.py)
+
 # Install dynamic_graph.sot.hrp2_14
 SET(PYTHON_MODULE dynamic_graph/sot/hrp2_14)
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
 PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" ${PYTHON_SITELIB})
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
 
 # Install dynamic_graph.sot.hrp2_10
 SET(PYTHON_MODULE dynamic_graph/sot/hrp2_10)
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
 PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" ${PYTHON_SITELIB})
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
 
-# Add the library to wrap the controller of HRP2.
-ADD_LIBRARY(sot-hrp2-controller
+# Add the library to wrap the controller of HRP2-JRL.
+MACRO(build_hrp2_controller robotnumber)
+
+ADD_LIBRARY(sot-hrp2-${robotnumber}-controller
   SHARED
   sot-hrp2-controller.cpp
+  sot-hrp2-${robotnumber}-controller.cpp
 )
 
 # Link the dynamic library containing the SoT with its dependencies.
-PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-controller "dynamic-graph")
-PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-controller "sot-core")
-PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-controller "dynamic-graph-corba")
+PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-${robotnumber}-controller "dynamic-graph")
+PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-${robotnumber}-controller "sot-core")
+PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-${robotnumber}-controller "dynamic-graph-corba")
+
+INSTALL(TARGETS sot-hrp2-${robotnumber}-controller DESTINATION lib)
+ENDMACRO()
 
-INSTALL(TARGETS sot-hrp2-controller DESTINATION lib/${ROBOTNAME})
\ No newline at end of file
+build_hrp2_controller("10")
+build_hrp2_controller("14")
\ No newline at end of file

src/dynamic_graph/sot/hrp2/prologue.py

+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+print("Prologue HRP-2")
+
+from dynamic_graph import plug
+from dynamic_graph.entity import PyEntityFactoryClass
+from dynamic_graph.sot.dynamics.solver import Solver
+
+def prologue_hrp2(hrp2class):
+    # Create the device.
+    # This entity behaves exactly like robotsimu except:
+    # 1. it does not provide the increment method
+    # 2. it forwards the robot control to the sot-abstract
+    #    controller.
+    Device = PyEntityFactoryClass('Device')
+
+    # Create the robot using the device.
+    robot = hrp2class(name = 'robot', device = Device('robot_device'))
+
+    # Initialize the zmp signal to the current com.
+    _com = robot.dynamic.com.value
+    robot.device.zmp.value = (_com[0], _com[1], 0.)
+
+    # Create a solver.
+    solver = Solver(robot)
+
+    # Make sure com and feet desired positions match the current
+    # positions.
+    s = ['left-ankle', 'right-ankle']
+    for i in s:
+        robot.dynamic.signal(i).recompute(robot.dynamic.signal(i).time + 1)
+        robot.features[i].reference.value = \
+        robot.dynamic.signal(i).value
+        robot.features[i]._feature.selec.value = '111111'
+        robot.tasks[i].controlGain.value = 180.
+
+    robot.featureComDes.errorIN.value = robot.dynamic.com.value
+    robot.featureComDes.selec.value = '111'
+    robot.comTask.controlGain.value = 180.
+
+    # Push com and feet tasks.
+    #
+    # The robot is currently in half-sitting, so this script freezes com
+    # and feet position so that the robot will remain stable while the
+    # user program is starting.
+    solver.push(robot.comTask.name)
+    for i in s:
+        solver.push(robot.tasks[i].name)
+
+        print("Prologue ran successfully.")
+
+    # Make sure only robot and solver are visible from the outside.
+    __all__ = ["robot", "solver"]
+
+
+####################################
+#        --- IMPORTANT ---         #
+#                                  #
+# THIS FILE MUST NEVER BE CHANGED. #
+# TO RUN YOUR EXPERIMENT, PLEASE   #
+# WRITE A SEPARATE PYTHON MODULE   #
+# AND LAUNCH IT USING dg-remote!   #
+####################################

src/dynamic_graph/sot/hrp2_10/prologue.py

+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+print("Prologue HRP2-14")
+
+from dynamic_graph.sot.hrp2_10.robot import *
+from dynamic_graph.sot.hrp2.prologue import *
+
+robot,solver = prologue_hrp2("Robot")
+__all__ = ["robot","solver"]
+
+####################################
+#        --- IMPORTANT ---         #
+#                                  #
+# THIS FILE MUST NEVER BE CHANGED. #
+# TO RUN YOUR EXPERIMENT, PLEASE   #
+# WRITE A SEPARATE PYTHON MODULE   #
+# AND LAUNCH IT USING dg-remote!   #
+####################################

src/dynamic_graph/sot/hrp2_10/robot.py.in

@@ -47,4 +47,4 @@ class Robot (Hrp2):
         Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
                       tracer)
 
-__all__ = [Robot]
+__all__ = ["Robot"]

src/dynamic_graph/sot/hrp2_14/prologue.py

+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+print("Prologue HRP2-14")
+
+from dynamic_graph.sot.hrp2_14.robot import *
+from dynamic_graph.sot.hrp2.prologue import *
+
+robot,solver = prologue_hrp2("Robot")
+__all__ = ["robot","solver"]
+
+####################################
+#        --- IMPORTANT ---         #
+#                                  #
+# THIS FILE MUST NEVER BE CHANGED. #
+# TO RUN YOUR EXPERIMENT, PLEASE   #
+# WRITE A SEPARATE PYTHON MODULE   #
+# AND LAUNCH IT USING dg-remote!   #
+####################################

src/dynamic_graph/sot/hrp2_14/robot.py.in

@@ -111,4 +111,4 @@ class Robot (Hrp2):
 
         Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2, tracer)
 
-__all__ = [Robot]
+__all__ = ["Robot"]

src/sot-hrp2-10-controller.cpp

+/*
+ * Copyright 2011,
+ *
+ * Olivier Stasse
+ *
+ * LAAS, CNRS
+ *
+ * This file is part of HRP2Controller.
+ * HRP2Controller is not a free software, 
+ * it contains information related to HRP-2 which involves
+ * that you either purchased the proper license to havec access to
+ * those informations, or that you signed the appropriate
+ * Non-Disclosure agreement.
+ *
+ *
+ */
+
+#include <sot/core/debug.hh>
+
+#define ROBOTNAME std::string("HRP2JRL")
+
+#include "sot-hrp2-10-controller.hh"
+
+const std::string SoTHRP2_10_Controller::LOG_PYTHON_10="/tmp/HRP2Controller_10_python.out";
+
+SoTHRP2_10_Controller::SoTHRP2_10_Controller():
+  SoTHRP2Controller(ROBOTNAME)
+{
+  std::cout << __FILE__ << " " << __FUNCTION__ <<":" << getClassName() << std::endl;
+  startupPython();
+}
+
+void SoTHRP2_10_Controller::startupPython()
+{
+  SoTHRP2Controller::startupPython();
+
+  std::ofstream aof(LOG_PYTHON_10.c_str());
+  runPython
+    (aof,
+     "from dynamic_graph.sot.hrp2_14.prologue import robot, solver",
+     interpreter_);
+  interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
+  aof.close();
+}
+
+extern "C" 
+{
+  dgsot::AbstractSotExternalInterface * createSotExternalInterface()
+  {
+    return new SoTHRP2_10_Controller;
+  }
+}
+
+extern "C"
+{
+  void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
+  {
+    delete p;
+  }
+}

src/sot-hrp2-10-controller.hh

+/*
+ * Copyright 2011,
+ *
+ * Olivier Stasse
+ *
+ * LAAS, CNRS
+ *
+ * This file is part of HRP2Controller.
+ * HRP2Controller is not a free software, 
+ * it contains information related to HRP-2 which involves
+ * that you either purchased the proper license to havec access to
+ * those informations, or that you signed the appropriate
+ * Non-Disclosure agreement.
+ *
+ *
+ */
+
+#ifndef _SOT_HRP2_10_Controller_H_
+#define _SOT_HRP2_10_Controller_H_
+
+#include "sot-hrp2-controller.hh"
+namespace dgsot=dynamicgraph::sot;
+
+class SoTHRP2_10_Controller: public SoTHRP2Controller
+{
+ public:
+  static const std::string LOG_PYTHON_10;
+
+  SoTHRP2_10_Controller();
+  virtual ~SoTHRP2_10_Controller() {};
+
+
+ protected:
+
+  virtual void startupPython();
+  
+
+};
+
+#endif /* _SOT_HRP2Controller_H_ */

src/sot-hrp2-14-controller.cpp

+/*
+ * Copyright 2011,
+ *
+ * Olivier Stasse
+ *
+ * LAAS, CNRS
+ *
+ * This file is part of HRP2Controller.
+ * HRP2Controller is not a free software, 
+ * it contains information related to HRP-2 which involves
+ * that you either purchased the proper license to havec access to
+ * those informations, or that you signed the appropriate
+ * Non-Disclosure agreement.
+ *
+ *
+ */
+
+#include <sot/core/debug.hh>
+
+#define ROBOTNAME std::string("HRP2JRL")
+
+#include "sot-hrp2-14-controller.hh"
+
+const std::string SoTHRP2_14_Controller::LOG_PYTHON_14="/tmp/HRP2Controller_14_python.out";
+
+SoTHRP2_14_Controller::SoTHRP2_14_Controller():
+  SoTHRP2Controller(ROBOTNAME)
+{
+  startupPython();
+}
+
+void SoTHRP2_14_Controller::startupPython()
+{
+  SoTHRP2Controller::startupPython();
+  std::ofstream aof(LOG_PYTHON_14.c_str());
+  runPython
+    (aof,
+     "from dynamic_graph.sot.hrp2_14.prologue import robot, solver",
+     interpreter_);
+  interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
+}
+
+extern "C" 
+{
+  dgsot::AbstractSotExternalInterface * createSotExternalInterface()
+  {
+    return new SoTHRP2_14_Controller;
+  }
+}
+
+extern "C"
+{
+  void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
+  {
+    delete p;
+  }
+}

src/sot-hrp2-14-controller.hh

+/*
+ * Copyright 2011,
+ *
+ * Olivier Stasse
+ *
+ * LAAS, CNRS
+ *
+ * This file is part of HRP2Controller.
+ * HRP2Controller is not a free software, 
+ * it contains information related to HRP-2 which involves
+ * that you either purchased the proper license to havec access to
+ * those informations, or that you signed the appropriate
+ * Non-Disclosure agreement.
+ *
+ *
+ */
+
+#ifndef _SOT_HRP2_14_Controller_H_
+#define _SOT_HRP2_14_Controller_H_
+
+#include "sot-hrp2-controller.hh"
+namespace dgsot=dynamicgraph::sot;
+
+class SoTHRP2_14_Controller: public SoTHRP2Controller
+{
+ public:
+  static const std::string LOG_PYTHON_14;
+
+  SoTHRP2_14_Controller();
+  virtual ~SoTHRP2_14_Controller() {};
+
+
+ protected:
+
+  virtual void startupPython();
+  
+
+};
+
+#endif /* _SOT_HRP2Controller_H_ */

src/sot-hrp2-controller.cpp

@@ -20,7 +20,6 @@
 #include "sot-hrp2-controller.hh"
 
 
-#define ENTITYNAME std::string("DLRBiped")
 
 const std::string SoTHRP2Controller::CLASS_NAME = "Device";
 const std::string SoTHRP2Controller::LOG_PYTHON="/tmp/HRP2Controller_python.out";
@@ -28,17 +27,16 @@ const double SoTHRP2Controller::TIMESTEP_DEFAULT = 0.005;
 
 using namespace std;
 
-SoTHRP2Controller::SoTHRP2Controller():
-  dgsot::Device(ENTITYNAME),
+SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
+  dgsot::Device(RobotName),
   timestep_ (TIMESTEP_DEFAULT),
   previousState_ (),
-  robotState_ ("StackOfTasks(" + ENTITYNAME + ")::output(vector)::robotState"),
+  robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
   interpreter_()
 
 {
   signalRegistration (robotState_);
   std::cout << __FILE__ << ":" << __FUNCTION__ <<"(#" << __LINE__ << " )" << std::endl;
-  startupPython();
 
 }
 
@@ -212,25 +210,7 @@ void SoTHRP2Controller::startupPython()
 	     "    path.append(p)", interpreter_);
   runPython (aof, "path.extend(sys.path)", interpreter_);
   runPython (aof, "sys.path = path", interpreter_);
-  runPython
-    (aof,
-     "from dynamic_graph.sot.openhrp.prologue_dlr_biped import robot, solver",
-     interpreter_);
-  interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
+  aof.close();
 }
 
-extern "C" 
-{
-  dgsot::AbstractSotExternalInterface * createSotExternalInterface()
-  {
-    return new SoTHRP2Controller;
-  }
-}
 
-extern "C"
-{
-  void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
-  {
-    delete p;
-  }
-}

src/sot-hrp2-controller.hh

@@ -43,8 +43,8 @@ class SoTHRP2Controller: public
     return CLASS_NAME;							    
   }
 
-  SoTHRP2Controller();
-  ~SoTHRP2Controller();
+  SoTHRP2Controller(std::string RobotName);
+  virtual ~SoTHRP2Controller();
 
   void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
 
@@ -54,7 +54,7 @@ class SoTHRP2Controller: public
 
   void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
 
- private:
+ protected:
   // Update output port with the control computed from the
   // dynamic graph.
   void updateRobotState(std::vector<double> &anglesIn);
@@ -63,7 +63,7 @@ class SoTHRP2Controller: public
 		 const std::string& command,
 		 dynamicgraph::corba::Interpreter& interpreter);
   
-  void startupPython();
+  virtual void startupPython();
   
   
   /// \brief Current integration step.