working on test-configProjector

tests/CMakeLists.txt

@@ -58,13 +58,13 @@ MACRO(ADD_TESTCASE NAME GENERATED)
 
 ENDMACRO(ADD_TESTCASE)
 
-ADD_TESTCASE (test-kdTree FALSE)
-ADD_TESTCASE (roadmap-1 FALSE)
-ADD_TESTCASE (test-intervals FALSE)
+#ADD_TESTCASE (test-kdTree FALSE)
+#ADD_TESTCASE (roadmap-1 FALSE)
+#ADD_TESTCASE (test-intervals FALSE)
 #ADD_TESTCASE (test-body-pair-collision FALSE)
-ADD_TESTCASE (test-gradient-based FALSE)
+#ADD_TESTCASE (test-gradient-based FALSE)
 ADD_TESTCASE (test-configprojector FALSE)
-ADD_TESTCASE (path-projectors FALSE) # link error
-ADD_TESTCASE (containers FALSE)
-ADD_TESTCASE (paths FALSE)
-ADD_TESTCASE (relative-motion FALSE)
+#ADD_TESTCASE (path-projectors FALSE) # link error
+#ADD_TESTCASE (containers FALSE)
+#ADD_TESTCASE (paths FALSE)
+#ADD_TESTCASE (relative-motion FALSE)

tests/path-projectors.cc

@@ -57,8 +57,9 @@ using hpp::pinocchio::DevicePtr_t;
 using hpp::pinocchio::JointPtr_t;
 
 using namespace hpp::core;
-using ::se3::JointModelRX;
-using ::se3::JointModelRY;
+using ::se3::JointModelPX;
+using ::se3::JointModelPY;
+using ::se3::JointModelPZ;
 using ::se3::JointIndex;
 
 DevicePtr_t createRobot ()
@@ -68,20 +69,20 @@ DevicePtr_t createRobot ()
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
-  JointModelRX::TangentVector_t max_effort = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
-  JointModelRX::TangentVector_t max_velocity = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
-  JointModelRX::ConfigVector_t lower_position(-4);
-  JointModelRX::ConfigVector_t upper_position(4);
+  JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::ConfigVector_t lower_position(-4);
+  JointModelPX::ConfigVector_t upper_position(4);
 
-  JointIndex idX = robot->model().addJoint(0,::se3::JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
+  JointIndex idX = robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
 
-  JointModelRY::TangentVector_t max_effortY = JointModelRY::TangentVector_t::Constant(JointModelRY::NV,std::numeric_limits<double>::max());
-  JointModelRY::TangentVector_t max_velocityY = JointModelRY::TangentVector_t::Constant(JointModelRY::NV,std::numeric_limits<double>::max());
-  JointModelRY::ConfigVector_t lower_positionY(-4);
-  JointModelRY::ConfigVector_t upper_positionY(4);
+  JointModelPY::TangentVector_t max_effortY = JointModelPY::TangentVector_t::Constant(JointModelPY::NV,std::numeric_limits<double>::max());
+  JointModelPY::TangentVector_t max_velocityY = JointModelPY::TangentVector_t::Constant(JointModelPY::NV,std::numeric_limits<double>::max());
+  JointModelPY::ConfigVector_t lower_positionY(-4);
+  JointModelPY::ConfigVector_t upper_positionY(4);
   std::string jointNameY = name + "_y";
 
-  robot->model().addJoint(idX,::se3::JointModelRY(), mat,jointNameY,max_effortY,max_velocityY,lower_positionY,upper_positionY);
+  robot->model().addJoint(idX,JointModelRY(), mat,jointNameY,max_effortY,max_velocityY,lower_positionY,upper_positionY);
 
 
   return robot;

tests/paths.cc

@@ -49,8 +49,9 @@ using hpp::pinocchio::DevicePtr_t;
 using hpp::pinocchio::JointPtr_t;
 
 using namespace hpp::core;
-using ::se3::JointModelRX;
-using ::se3::JointModelRY;
+using ::se3::JointModelPX;
+using ::se3::JointModelPY;
+using ::se3::JointModelPZ;
 using ::se3::JointIndex;
 
 DevicePtr_t createRobot ()
@@ -60,12 +61,12 @@ DevicePtr_t createRobot ()
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
-  JointModelRX::TangentVector_t max_effort = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
-  JointModelRX::TangentVector_t max_velocity = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
-  JointModelRX::ConfigVector_t lower_position(-4);
-  JointModelRX::ConfigVector_t upper_position(4);
+  JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::ConfigVector_t lower_position(-4);
+  JointModelPX::ConfigVector_t upper_position(4);
 
-  robot->model().addJoint(0,::se3::JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
+  robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
 
   return robot;
 
@@ -97,14 +98,14 @@ DevicePtr_t createRobot2 ()
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
-  JointModelRX::TangentVector_t max_effort = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
-  JointModelRX::TangentVector_t max_velocity = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
-  JointModelRX::ConfigVector_t lower_position(-4);
-  JointModelRX::ConfigVector_t upper_position(4);
+  JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::ConfigVector_t lower_position(-4);
+  JointModelPX::ConfigVector_t upper_position(4);
   JointIndex idJoint = 0;
 
   for(int i = 0 ; i < 10 ; ++i){
-    idJoint = robot->model().addJoint(idJoint,::se3::JointModelPX(), mat,jointName + TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
+    idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,jointName + TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
   }
 
   return robot;
@@ -158,7 +159,7 @@ void checkAt(const PathPtr_t orig, value_type to,
 
 BOOST_AUTO_TEST_CASE (extracted)
 {
-  DevicePtr_t dev = hppPinocchio ();
+  DevicePtr_t dev = createRobot();
   BOOST_REQUIRE (dev);
   Problem problem (dev);
 
@@ -186,7 +187,7 @@ BOOST_AUTO_TEST_CASE (extracted)
 
 BOOST_AUTO_TEST_CASE (subchain)
 {
-  DevicePtr_t dev = hppPinocchio (); // 10 translations
+  DevicePtr_t dev = createRobot2(); // 10 translations
   BOOST_REQUIRE (dev);
   Problem problem (dev);
 

tests/relative-motion.cc

@@ -50,6 +50,7 @@
 #include <hpp/core/constraint-set.hh>
 #include <hpp/core/locked-joint.hh>
 #include <hpp/core/numerical-constraint.hh>
+#include <pinocchio/multibody/joint/joint-variant.hpp>
 #include "../tests/utils.hh"
 
 
@@ -60,11 +61,39 @@ using hpp::pinocchio::JointPtr_t;
 using hpp::constraints::RelativeTransformation;
 
 using namespace hpp::core;
+using ::se3::JointModelPX;
+using ::se3::JointModelPY;
+using ::se3::JointModelPZ;
+using ::se3::JointIndex;
+
 
-/*
 DevicePtr_t createRobot ()
 {
+
   DevicePtr_t robot = Device::create ("test");
+  const std::string& name = robot->name ();
+  Transform3f mat; mat.setIdentity ();
+  std::string jointName = name + "_x";
+
+  JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::ConfigVector_t lower_position(-4);
+  JointModelPX::ConfigVector_t upper_position(4);
+
+  JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
+
+  std::string bname = "joint_a";
+  for (int i = 0; i < 2; ++i) {
+    idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,bname+TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
+  }
+  bname = "joint_b";
+  for (int i = 0; i < 3; ++i) {
+    idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,bname+TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
+  }
+
+
+  return robot;
+ /* DevicePtr_t robot = Device::create ("test");
 
   const std::string& name = robot->name ();
   fcl::Transform3f mat; mat.setIdentity ();
@@ -108,8 +137,8 @@ DevicePtr_t createRobot ()
     parent->addChildJoint (joint);
     parent = joint;
   }
-  return robot;
-}*/
+  return robot;*/
+}
 
 void lockJoint (ConfigProjectorPtr_t proj, DevicePtr_t dev, std::string name)
 {
@@ -132,7 +161,7 @@ struct Jidx {
 
 BOOST_AUTO_TEST_CASE (relativeMotion)
 {
-  DevicePtr_t dev = hppPinocchio();
+  DevicePtr_t dev = createRobot();
   BOOST_REQUIRE (dev);
   Jidx jointid;
   jointid.dev = dev;

tests/roadmap-1.cc

@@ -34,6 +34,8 @@
 
 #include <hpp/core/steering-method-straight.hh>
 #include <hpp/core/weighed-distance.hh>
+#include <pinocchio/multibody/joint/joint-variant.hpp>
+
 
 
 
@@ -53,6 +55,10 @@ using hpp::core::RoadmapPtr_t;
 using hpp::core::Roadmap;
 using hpp::core::NodePtr_t;
 using hpp::core::WeighedDistance;
+using ::se3::JointModelPX;
+using ::se3::JointModelPY;
+using ::se3::JointModelPZ;
+using ::se3::JointIndex;
 
 BOOST_AUTO_TEST_SUITE( test_hpp_core )
 
@@ -81,7 +87,19 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
   robot->rootJoint (xJoint);
   xJoint->addChildJoint (yJoint);
 */
-  DevicePtr_t robot = hppPinocchio();
+  DevicePtr_t robot = Device::create("robot");
+  const std::string& name = robot->name ();
+  Transform3f mat; mat.setIdentity ();
+
+  JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::ConfigVector_t lower_position(-3);
+  JointModelPX::ConfigVector_t upper_position(3);
+
+  JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,name + "_x",max_effort,max_velocity,lower_position,upper_position);
+  robot->model().addJoint(idJoint,JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position);
+
+
   // Create steering method
   Problem p = Problem (robot);
   SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);
@@ -292,8 +310,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
 
 BOOST_AUTO_TEST_CASE (nearestNeighbor) {
   // Build robot
-  DevicePtr_t robot = hppPinocchio();
-  /*JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f());
+ /* DevicePtr_t robot = Device::create("robot");
+  JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f());
   xJoint->isBounded(0,1);
   xJoint->lowerBound(0,-3.);
   xJoint->upperBound(0,3.);
@@ -306,6 +324,21 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
   robot->rootJoint (xJoint);
   xJoint->addChildJoint (yJoint);*/
 
+  DevicePtr_t robot = Device::create("robot");
+  const std::string& name = robot->name ();
+  Transform3f mat; mat.setIdentity ();
+
+  JointModelRX::TangentVector_t max_effort = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
+  JointModelRX::TangentVector_t max_velocity = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
+  JointModelRX::ConfigVector_t lower_position(-3);
+  JointModelRX::ConfigVector_t upper_position(3);
+
+
+  JointIndex idJoint = robot->model().addJoint(0,::se3::JointModelPX(), mat,name + "_x",max_effort,max_velocity,lower_position,upper_position);
+  robot->model().addJoint(idJoint,::se3::JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position);
+
+
+
   // Create steering method
   Problem p (robot);
   SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);

tests/test-configprojector.cc

@@ -25,7 +25,7 @@
 #include <hpp/pinocchio/configuration.hh>
 
 #include <hpp/constraints/generic-transformation.hh>
-
+#include <pinocchio/multibody/joint/joint-variant.hpp>
 #include "../tests/utils.hh"
 
 
@@ -40,6 +40,16 @@ using hpp::constraints::matrix3_t;
 using hpp::constraints::vector3_t;
 
 using namespace hpp::core;
+using ::se3::JointModelPX;
+using ::se3::JointModelPY;
+using ::se3::JointModelPZ;
+using ::se3::JointModelRUBX;
+using ::se3::JointModelFreeFlyer;
+using ::se3::JointModelSpherical;
+using ::se3::JointIndex;
+
+typedef Eigen::Matrix<value_type,3,1> Vector3;
+typedef Eigen::Matrix<value_type,3,3> Matrix3;
 
 /*
 JointPtr_t createFreeflyerJoint (DevicePtr_t robot)
@@ -95,9 +105,38 @@ JointPtr_t createFreeflyerJoint (DevicePtr_t robot)
   joint->name (jointName);
   parent->addChildJoint (joint);
   return joint;
+}*/
+
+DevicePtr_t createRobot (){
+
+  DevicePtr_t robot = Device::create ("test");
+  const std::string& name = robot->name ();
+  Transform3f mat; mat.setIdentity ();
+
+  JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
+  JointModelPX::ConfigVector_t lower_position(-4);
+  JointModelPX::ConfigVector_t upper_position(4);
+
+  JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,name + "_x",max_effort,max_velocity,lower_position,upper_position);
+  idJoint = robot->model().addJoint(idJoint,JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position);
+  idJoint = robot->model().addJoint(idJoint,JointModelPZ(), mat,name + "_z",max_effort,max_velocity,lower_position,upper_position);
+
+  JointIndex waist = robot->model().addJoint(idJoint,JointModelSpherical(), mat,name + "_SO3");
+
+  Transform3f pos;
+  Matrix3 orient;
+  // Right leg joint 0
+  orient (0,0) = 0; orient (0,1) = 0; orient (0,2) = -1;
+  orient (1,0) = 0; orient (1,1) = 1; orient (1,2) =  0;
+  orient (2,0) = 1; orient (2,1) = 0; orient (2,2) =  0;
+  pos.rotation (orient);
+  pos.translation ( Vector3(0, -0.08, 0));
+
+  return robot;
 }
 
-DevicePtr_t createRobot ()
+/*DevicePtr_t createRobot ()
 {
   DevicePtr_t robot = Device::create ("test");
   JointPtr_t waist = createFreeflyerJoint (robot);