Merge branch 'topic/python' of github.com:jrl-umi3218/sot-dynamic into topic/python

CMakeLists.txt

@@ -44,8 +44,8 @@ ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.16.1")
 ADD_REQUIRED_DEPENDENCY("hrp2-dynamics >= 1.3.0")
 ADD_REQUIRED_DEPENDENCY("hrp2-10-optimized >= 1.0")
 
-ADD_REQUIRED_DEPENDENCY("hrp2_10 >= 1.0.0")
-ADD_REQUIRED_DEPENDENCY("hrp2_14 >= 1.0.0")
+ADD_REQUIRED_DEPENDENCY("hrp2-10 >= 1.0.0")
+ADD_REQUIRED_DEPENDENCY("hrp2-14 >= 1.0.0")
 
 ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0")
 ADD_REQUIRED_DEPENDENCY("sot-core >= 1.0.0")

include/sot-dynamic/dynamic-hrp2.h

@@ -65,6 +65,10 @@ class SOTDYNAMICHRP2_EXPORT DynamicHrp2
 {
 
  public:
+  virtual const std::string& getClassName () const
+  {
+    return CLASS_NAME;
+  }
   static const std::string CLASS_NAME;
 
  public: /* --- CONSTRUCTION --- */

include/sot-dynamic/dynamic.h

@@ -93,6 +93,10 @@ class SOTDYNAMIC_EXPORT Dynamic
   friend class sot::command::InitializeRobot;
 
  public:
+  virtual const std::string& getClassName () const
+  {
+    return CLASS_NAME;
+  }
   static const std::string CLASS_NAME;
   virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
 

src/dynamic_graph/sot/dynamics/solver.py

@@ -16,7 +16,7 @@
 # dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
 
 from dynamic_graph import plug
-from dynamic_graph.sot.core import SOT
+from dynamic_graph.sot.core import JointLimitator, SOT
 
 class Solver:
     robot = None
@@ -24,14 +24,28 @@ class Solver:
 
     def __init__(self, robot):
         self.robot = robot
+
+        # Make sure control does not exceed joint limits.
+        self.jointLimitator = JointLimitator('joint_limitator')
+        plug(self.robot.dynamic.position, self.jointLimitator.joint)
+        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
+        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
+
+        # Create the solver.
         self.sot = SOT('solver')
         self.sot.signal('damping').value = 1e-6
         self.sot.setNumberDofs(self.robot.dimension)
 
+        # Plug the solver control into the filter.
+        plug(self.sot.control, self.jointLimitator.controlIN)
+
+        # Important: always use 'jointLimitator.control'
+        # and NOT 'sot.control'!
+
         if robot.device:
-            plug(self.sot.signal('control'), robot.device.signal('control'))
-            plug(self.robot.device.state,
-                 self.robot.dynamic.position)
+            plug(self.jointLimitator.control, robot.device.control)
+            plug(self.robot.device.state, self.robot.dynamic.position)
+
     def push(self, taskName):
         """
         Proxy method to push a task in the sot