Commit 58221d7d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

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
parent e50a87ac
......@@ -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)
......
......@@ -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>
......
......@@ -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) =
......
......@@ -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 HumanoidRobotTest(unittest.TestCase):
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__':
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment