correctly initialize device Model in tests,runtime error in WeighedDistance

tests/path-projectors.cc

@@ -39,7 +39,6 @@
 #include <hpp/pinocchio/configuration.hh>
 
 #include <hpp/constraints/differentiable-function.hh>
-
 #include <hpp/core/straight-path.hh>
 #include <hpp/core/config-projector.hh>
 #include <hpp/core/constraint-set.hh>
@@ -50,6 +49,8 @@
 #include <hpp/core/path-projector/progressive.hh>
 #include "../tests/utils.hh"
 #include <pinocchio/multibody/joint/joint-variant.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+
 
 
 using hpp::pinocchio::Device;
@@ -57,6 +58,8 @@ using hpp::pinocchio::DevicePtr_t;
 using hpp::pinocchio::JointPtr_t;
 
 using namespace hpp::core;
+using namespace hpp::pinocchio;
+
 using ::se3::JointModelPX;
 using ::se3::JointModelPY;
 using ::se3::JointModelPZ;
@@ -66,6 +69,10 @@ DevicePtr_t createRobot ()
 {
   DevicePtr_t robot = Device::create ("test");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
@@ -84,6 +91,8 @@ DevicePtr_t createRobot ()
 
   robot->model().addJoint(idX,JointModelPY(), mat,jointNameY,max_effortY,max_velocityY,lower_positionY,upper_positionY);
 
+  robot->createData();
+  robot->createGeomData();
 
   return robot;
   /*

tests/paths.cc

@@ -37,6 +37,8 @@
 #include <hpp/core/straight-path.hh>
 #include <hpp/core/subchain-path.hh>
 #include <pinocchio/multibody/joint/joint-variant.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+
 
 
 #include "../tests/utils.hh"
@@ -49,6 +51,8 @@ using hpp::pinocchio::DevicePtr_t;
 using hpp::pinocchio::JointPtr_t;
 
 using namespace hpp::core;
+using namespace hpp::pinocchio;
+
 using ::se3::JointModelPX;
 using ::se3::JointModelPY;
 using ::se3::JointModelPZ;
@@ -58,6 +62,10 @@ DevicePtr_t createRobot ()
 {
   DevicePtr_t robot = Device::create ("test");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
@@ -68,6 +76,10 @@ DevicePtr_t createRobot ()
 
   robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
 
+
+  robot->createData();
+  robot->createGeomData();
+
   return robot;
 
 
@@ -95,6 +107,10 @@ DevicePtr_t createRobot2 ()
 {
   DevicePtr_t robot = Device::create ("test");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
@@ -108,6 +124,9 @@ DevicePtr_t createRobot2 ()
     idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,jointName + TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
   }
 
+
+  robot->createData();
+  robot->createGeomData();
   return robot;
   /*DevicePtr_t robot = Device::create ("test");
 

tests/relative-motion.cc

@@ -51,6 +51,8 @@
 #include <hpp/core/locked-joint.hh>
 #include <hpp/core/numerical-constraint.hh>
 #include <pinocchio/multibody/joint/joint-variant.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+
 #include "../tests/utils.hh"
 
 
@@ -61,6 +63,8 @@ using hpp::pinocchio::JointPtr_t;
 using hpp::constraints::RelativeTransformation;
 
 using namespace hpp::core;
+using namespace hpp::pinocchio;
+
 using ::se3::JointModelPX;
 using ::se3::JointModelPY;
 using ::se3::JointModelPZ;
@@ -74,6 +78,10 @@ DevicePtr_t createRobot ()
 
   DevicePtr_t robot = Device::create ("test");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
@@ -94,6 +102,8 @@ DevicePtr_t createRobot ()
   }
 
 
+  robot->createData();
+  robot->createGeomData();
   return robot;
  /* DevicePtr_t robot = Device::create ("test");
 

tests/roadmap-1.cc

@@ -36,6 +36,8 @@
 #include <hpp/core/steering-method-straight.hh>
 #include <hpp/core/weighed-distance.hh>
 #include <pinocchio/multibody/joint/joint-variant.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+
 
 
 
@@ -57,6 +59,8 @@ using hpp::core::Roadmap;
 using hpp::core::NodePtr_t;
 using hpp::core::WeighedDistance;
 using namespace hpp::core;
+using namespace hpp::pinocchio;
+
 using ::se3::JointModelPX;
 using ::se3::JointModelPY;
 using ::se3::JointModelPZ;
@@ -91,6 +95,10 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
 */
   DevicePtr_t robot = Device::create("robot");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
 
   JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
@@ -101,6 +109,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
   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);
 
+  robot->createData();
+  robot->createGeomData();
 
   // Create steering method
   Problem p = Problem (robot);
@@ -328,6 +338,10 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
 
   DevicePtr_t robot = Device::create("robot");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
 
   JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
@@ -339,6 +353,8 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
   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);
 
+  robot->createData();
+  robot->createGeomData();
 
 
   // Create steering method

tests/test-body-pair-collision.cc

@@ -31,6 +31,8 @@
 #include <boost/test/included/unit_test.hpp>
 #include "../tests/utils.hh"
 #include <pinocchio/multibody/joint/joint-variant.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+
 
 
 
@@ -46,6 +48,9 @@ using hpp::pinocchio::ObjectVector_t;
 using hpp::core::continuousCollisionChecking::dichotomy::BodyPairCollision;
 using hpp::core::continuousCollisionChecking::dichotomy::BodyPairCollisionPtr_t;
 
+using namespace hpp::core;
+using namespace hpp::pinocchio;
+
 using ::se3::JointModelRX;
 using ::se3::JointModelPX;
 using ::se3::JointModelPY;
@@ -63,6 +68,10 @@ DevicePtr_t createRobot (){
 
   DevicePtr_t robot = Device::create ("test");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
 
   JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
@@ -204,6 +213,9 @@ DevicePtr_t createRobot (){
   idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_5",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
   robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY5");
 
+
+  robot->createData();
+  robot->createGeomData();
   return robot;
 }
 

tests/test-configprojector.cc

@@ -27,6 +27,8 @@
 
 #include <hpp/constraints/generic-transformation.hh>
 #include <pinocchio/multibody/joint/joint-variant.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+
 #include "../tests/utils.hh"
 
 
@@ -40,6 +42,7 @@ using hpp::constraints::PositionPtr_t;
 using hpp::constraints::matrix3_t;
 using hpp::constraints::vector3_t;
 
+using namespace hpp::pinocchio;
 using namespace hpp::core;
 using ::se3::JointModelRX;
 using ::se3::JointModelPX;
@@ -113,6 +116,10 @@ DevicePtr_t createRobot (){
 
   DevicePtr_t robot = Device::create ("test");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
 
   JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
@@ -254,6 +261,9 @@ DevicePtr_t createRobot (){
   idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_5",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
   robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY5");
 
+
+  robot->createData();
+  robot->createGeomData();
   return robot;
 }
 /*

tests/test-gradient-based.cc

@@ -38,6 +38,7 @@
 #include <pinocchio/multibody/joint/joint-variant.hpp>
 #include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
 #include <pinocchio/multibody/geometry.hpp>
+
 #include "../tests/utils.hh"
 
 
@@ -62,6 +63,8 @@ using hpp::core::SteeringMethodStraight;
 using hpp::core::PathOptimizerPtr_t;
 using hpp::core::pathOptimization::GradientBased;
 
+using namespace hpp::core;
+using namespace hpp::pinocchio;
 
 using ::se3::JointModelPX;
 using ::se3::JointModelPY;
@@ -76,6 +79,10 @@ DevicePtr_t createRobot ()
 {
   DevicePtr_t robot = Device::create ("test");
   const std::string& name = robot->name ();
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   Transform3f mat; mat.setIdentity ();
   std::string jointName = name + "_x";
 
@@ -96,6 +103,8 @@ DevicePtr_t createRobot ()
   robot->geomModel().addInnerObject(idX,idObj);
 
 
+  robot->createData();
+  robot->createGeomData();
   return robot;
   /*
   DevicePtr_t robot = Device::create ("planar-robot");

tests/test-kdTree.cc

@@ -44,6 +44,7 @@
 #define BOOST_TEST_MODULE kdTree
 #include <boost/test/included/unit_test.hpp>
 #include <pinocchio/multibody/joint/joint-variant.hpp>
+#include <pinocchio/multibody/geometry.hpp>
 #include "../tests/utils.hh"
 
 
@@ -82,6 +83,10 @@ BOOST_AUTO_TEST_CASE (kdTree) {
   so3Joint->addChildJoint (so2Joint);
 */
   DevicePtr_t robot = Device::create("robot");
+  ModelPtr_t m = ModelPtr_t(new ::se3::Model());
+  GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
+  robot->model(m);
+  robot->geomModel(gm);
   const std::string& name = robot->name ();
   Transform3f mat; mat.setIdentity ();
 
@@ -97,6 +102,9 @@ BOOST_AUTO_TEST_CASE (kdTree) {
   idJoint = robot->model().addJoint(idJoint,JointModelSpherical(), mat,name + "_SO3");
   idJoint = robot->model().addJoint(idJoint,JointModelRUBZ(), mat,name + "_SO2");
 
+  robot->createData();
+  robot->createGeomData();
+
 
   // Build Distance, nearestNeighbor, KDTree
   Problem problem (robot);