Fix lower position limit.

The initialization was using std::numeric_limits<double>::min
instead of std::numeric_limits<double>::lowest.
Add a unit test.
Fix #71

Author: olivier-stasse

CMakeLists.txt

@@ -34,6 +34,7 @@ PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
 # Project dependencies
 ADD_PROJECT_DEPENDENCY(sot-tools REQUIRED PKG_CONFIG_REQUIRES sot-tools)
+ADD_PROJECT_DEPENDENCY(example-robot-data)
 
 SET(BOOST_COMPONENTS filesystem system unit_test_framework)
 

package.xml

@@ -27,6 +27,7 @@
   <exec_depend>sot-tools</exec_depend>
   <exec_depend>sot-core</exec_depend>
   <exec_depend>pinocchio</exec_depend>
+  <exec_depend>example-robot-data</exec_depend>
 
   <test_depend>gtest</test_depend>
 

src/sot-dynamic-pinocchio.cpp

@@ -253,6 +253,7 @@ dg::Vector& DynamicPinocchio::getLowerPositionLimits(dg::Vector& res, const int&
 
   res.resize(m_model->nv);
   if (!sphericalJoints.empty()) {
+
     int fillingIndex = 0;  // SoTValue
     int origIndex = 0;     // PinocchioValue
     for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++) {
@@ -266,12 +267,13 @@ dg::Vector& DynamicPinocchio::getLowerPositionLimits(dg::Vector& res, const int&
       }
       // Found a Spherical Joint.
       // Assuming that spherical joint limits are unset
-      res(fillingIndex) = std::numeric_limits<double>::min();
-      res(fillingIndex + 1) = std::numeric_limits<double>::min();
-      res(fillingIndex + 2) = std::numeric_limits<double>::min();
+      res(fillingIndex) = std::numeric_limits<double>::lowest();
+      res(fillingIndex + 1) = std::numeric_limits<double>::lowest();
+      res(fillingIndex + 2) = std::numeric_limits<double>::lowest();
       fillingIndex += 3;
       origIndex += 4;
     }
+
     assert(m_model->nv - fillingIndex == m_model->nq - origIndex);
     if (m_model->nv > fillingIndex)
       res.segment(fillingIndex, m_model->nv - fillingIndex) =

tests/python/humanoid_robot.py

@@ -3,7 +3,11 @@
 # Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
 
 import unittest
+import pinocchio
 
+import numpy as np
+
+from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
 from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
 
 
@@ -26,7 +30,10 @@ class HumanoidRobotTest(unittest.TestCase):
     def setUp(self):
         import os
         dir_path = os.path.dirname(os.path.realpath(__file__))
-        self.r2d2_urdf_file = os.path.join(dir_path, "r2d2.urdf")
+        from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR
+        self.urdf_file_name=EXAMPLE_ROBOT_DATA_MODEL_DIR+\
+           '/talos_data/robots/talos_reduced.urdf'
+        self.name = "talos"
 
     def test_non_instanciable_robot(self):
         class NonInstanciableRobot(AbstractHumanoidRobot):
@@ -36,12 +43,30 @@ class NonInstanciableRobot(AbstractHumanoidRobot):
 
     def test_build_robot_from_string(self):
 
-        with open(self.r2d2_urdf_file, 'r') as urdf:
+        with open(self.urdf_file_name, 'r') as urdf:
             urdfString = urdf.read()
-        Robot("test_build_robot_from_string", urdfString=urdfString)
+        arobot = Robot("test_build_robot_from_string", urdfString=urdfString)
+
+        # Test if the two vectors are identical:
+        arobot.dynamic = DynamicPinocchio(self.name + "_dynamic")
+        arobot.dynamic.setModel(arobot.pinocchioModel)
+        arobot.dynamic.setData(arobot.pinocchioData)
+
+        def get(s):
+            s.recompute(0)
+            return s.value
+
+        loc_lowerJl=np.array(get(arobot.dynamic.lowerJl))
+        pin_lowerJl=np.array(arobot.pinocchioModel.lowerPositionLimit[1:len(arobot.pinocchioModel.lowerPositionLimit
+)])
+
+        for i in range(0,len(loc_lowerJl),1):
+            if not loc_lowerJl[i] == pin_lowerJl[i]:
+                self.assertTrue(False,"lowerJl is not working")
+
 
     def test_build_robot_from_urdf(self):
-        Robot("test_build_robot_from_string", urdfFile=self.r2d2_urdf_file)
+        Robot("test_build_robot_from_string", urdfFile=self.urdf_file_name)
 
 
 if __name__ == '__main__':