Commit ec9edbbf authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

correctly initialize device Model in tests,runtime error in WeighedDistance

parent db1e72c8
...@@ -39,7 +39,6 @@ ...@@ -39,7 +39,6 @@
#include <hpp/pinocchio/configuration.hh> #include <hpp/pinocchio/configuration.hh>
#include <hpp/constraints/differentiable-function.hh> #include <hpp/constraints/differentiable-function.hh>
#include <hpp/core/straight-path.hh> #include <hpp/core/straight-path.hh>
#include <hpp/core/config-projector.hh> #include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh> #include <hpp/core/constraint-set.hh>
...@@ -50,6 +49,8 @@ ...@@ -50,6 +49,8 @@
#include <hpp/core/path-projector/progressive.hh> #include <hpp/core/path-projector/progressive.hh>
#include "../tests/utils.hh" #include "../tests/utils.hh"
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
using hpp::pinocchio::Device; using hpp::pinocchio::Device;
...@@ -57,6 +58,8 @@ using hpp::pinocchio::DevicePtr_t; ...@@ -57,6 +58,8 @@ using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t; using hpp::pinocchio::JointPtr_t;
using namespace hpp::core; using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelPX; using ::se3::JointModelPX;
using ::se3::JointModelPY; using ::se3::JointModelPY;
using ::se3::JointModelPZ; using ::se3::JointModelPZ;
...@@ -66,6 +69,10 @@ DevicePtr_t createRobot () ...@@ -66,6 +69,10 @@ DevicePtr_t createRobot ()
{ {
DevicePtr_t robot = Device::create ("test"); DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
...@@ -84,6 +91,8 @@ DevicePtr_t createRobot () ...@@ -84,6 +91,8 @@ DevicePtr_t createRobot ()
robot->model().addJoint(idX,JointModelPY(), mat,jointNameY,max_effortY,max_velocityY,lower_positionY,upper_positionY); robot->model().addJoint(idX,JointModelPY(), mat,jointNameY,max_effortY,max_velocityY,lower_positionY,upper_positionY);
robot->createData();
robot->createGeomData();
return robot; return robot;
/* /*
......
...@@ -37,6 +37,8 @@ ...@@ -37,6 +37,8 @@
#include <hpp/core/straight-path.hh> #include <hpp/core/straight-path.hh>
#include <hpp/core/subchain-path.hh> #include <hpp/core/subchain-path.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -49,6 +51,8 @@ using hpp::pinocchio::DevicePtr_t; ...@@ -49,6 +51,8 @@ using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t; using hpp::pinocchio::JointPtr_t;
using namespace hpp::core; using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelPX; using ::se3::JointModelPX;
using ::se3::JointModelPY; using ::se3::JointModelPY;
using ::se3::JointModelPZ; using ::se3::JointModelPZ;
...@@ -58,6 +62,10 @@ DevicePtr_t createRobot () ...@@ -58,6 +62,10 @@ DevicePtr_t createRobot ()
{ {
DevicePtr_t robot = Device::create ("test"); DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
...@@ -68,6 +76,10 @@ DevicePtr_t createRobot () ...@@ -68,6 +76,10 @@ DevicePtr_t createRobot ()
robot->model().addJoint(0,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);
robot->createData();
robot->createGeomData();
return robot; return robot;
...@@ -95,6 +107,10 @@ DevicePtr_t createRobot2 () ...@@ -95,6 +107,10 @@ DevicePtr_t createRobot2 ()
{ {
DevicePtr_t robot = Device::create ("test"); DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
...@@ -108,6 +124,9 @@ DevicePtr_t createRobot2 () ...@@ -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); idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,jointName + TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
} }
robot->createData();
robot->createGeomData();
return robot; return robot;
/*DevicePtr_t robot = Device::create ("test"); /*DevicePtr_t robot = Device::create ("test");
......
...@@ -51,6 +51,8 @@ ...@@ -51,6 +51,8 @@
#include <hpp/core/locked-joint.hh> #include <hpp/core/locked-joint.hh>
#include <hpp/core/numerical-constraint.hh> #include <hpp/core/numerical-constraint.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -61,6 +63,8 @@ using hpp::pinocchio::JointPtr_t; ...@@ -61,6 +63,8 @@ using hpp::pinocchio::JointPtr_t;
using hpp::constraints::RelativeTransformation; using hpp::constraints::RelativeTransformation;
using namespace hpp::core; using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelPX; using ::se3::JointModelPX;
using ::se3::JointModelPY; using ::se3::JointModelPY;
using ::se3::JointModelPZ; using ::se3::JointModelPZ;
...@@ -74,6 +78,10 @@ DevicePtr_t createRobot () ...@@ -74,6 +78,10 @@ DevicePtr_t createRobot ()
DevicePtr_t robot = Device::create ("test"); DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
...@@ -94,6 +102,8 @@ DevicePtr_t createRobot () ...@@ -94,6 +102,8 @@ DevicePtr_t createRobot ()
} }
robot->createData();
robot->createGeomData();
return robot; return robot;
/* DevicePtr_t robot = Device::create ("test"); /* DevicePtr_t robot = Device::create ("test");
......
...@@ -36,6 +36,8 @@ ...@@ -36,6 +36,8 @@
#include <hpp/core/steering-method-straight.hh> #include <hpp/core/steering-method-straight.hh>
#include <hpp/core/weighed-distance.hh> #include <hpp/core/weighed-distance.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
...@@ -57,6 +59,8 @@ using hpp::core::Roadmap; ...@@ -57,6 +59,8 @@ using hpp::core::Roadmap;
using hpp::core::NodePtr_t; using hpp::core::NodePtr_t;
using hpp::core::WeighedDistance; using hpp::core::WeighedDistance;
using namespace hpp::core; using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelPX; using ::se3::JointModelPX;
using ::se3::JointModelPY; using ::se3::JointModelPY;
using ::se3::JointModelPZ; using ::se3::JointModelPZ;
...@@ -91,6 +95,10 @@ BOOST_AUTO_TEST_CASE (Roadmap1) { ...@@ -91,6 +95,10 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
*/ */
DevicePtr_t robot = Device::create("robot"); DevicePtr_t robot = Device::create("robot");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max()); 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) { ...@@ -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); 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->model().addJoint(idJoint,JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position);
robot->createData();
robot->createGeomData();
// Create steering method // Create steering method
Problem p = Problem (robot); Problem p = Problem (robot);
...@@ -328,6 +338,10 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) { ...@@ -328,6 +338,10 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
DevicePtr_t robot = Device::create("robot"); DevicePtr_t robot = Device::create("robot");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max()); 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) { ...@@ -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); 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->model().addJoint(idJoint,::se3::JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position);
robot->createData();
robot->createGeomData();
// Create steering method // Create steering method
......
...@@ -31,6 +31,8 @@ ...@@ -31,6 +31,8 @@
#include <boost/test/included/unit_test.hpp> #include <boost/test/included/unit_test.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
...@@ -46,6 +48,9 @@ using hpp::pinocchio::ObjectVector_t; ...@@ -46,6 +48,9 @@ using hpp::pinocchio::ObjectVector_t;
using hpp::core::continuousCollisionChecking::dichotomy::BodyPairCollision; using hpp::core::continuousCollisionChecking::dichotomy::BodyPairCollision;
using hpp::core::continuousCollisionChecking::dichotomy::BodyPairCollisionPtr_t; using hpp::core::continuousCollisionChecking::dichotomy::BodyPairCollisionPtr_t;
using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelRX; using ::se3::JointModelRX;
using ::se3::JointModelPX; using ::se3::JointModelPX;
using ::se3::JointModelPY; using ::se3::JointModelPY;
...@@ -63,6 +68,10 @@ DevicePtr_t createRobot (){ ...@@ -63,6 +68,10 @@ DevicePtr_t createRobot (){
DevicePtr_t robot = Device::create ("test"); DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
...@@ -204,6 +213,9 @@ DevicePtr_t createRobot (){ ...@@ -204,6 +213,9 @@ DevicePtr_t createRobot (){
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_5",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot); 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->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY5");
robot->createData();
robot->createGeomData();
return robot; return robot;
} }
......
...@@ -27,6 +27,8 @@ ...@@ -27,6 +27,8 @@
#include <hpp/constraints/generic-transformation.hh> #include <hpp/constraints/generic-transformation.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -40,6 +42,7 @@ using hpp::constraints::PositionPtr_t; ...@@ -40,6 +42,7 @@ using hpp::constraints::PositionPtr_t;
using hpp::constraints::matrix3_t; using hpp::constraints::matrix3_t;
using hpp::constraints::vector3_t; using hpp::constraints::vector3_t;
using namespace hpp::pinocchio;
using namespace hpp::core; using namespace hpp::core;
using ::se3::JointModelRX; using ::se3::JointModelRX;
using ::se3::JointModelPX; using ::se3::JointModelPX;
...@@ -113,6 +116,10 @@ DevicePtr_t createRobot (){ ...@@ -113,6 +116,10 @@ DevicePtr_t createRobot (){
DevicePtr_t robot = Device::create ("test"); DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
...@@ -254,6 +261,9 @@ DevicePtr_t createRobot (){ ...@@ -254,6 +261,9 @@ DevicePtr_t createRobot (){
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_5",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot); 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->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY5");
robot->createData();
robot->createGeomData();
return robot; return robot;
} }
/* /*
......
...@@ -38,6 +38,7 @@ ...@@ -38,6 +38,7 @@
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/spatial/fcl-pinocchio-conversions.hpp> #include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
#include <pinocchio/multibody/geometry.hpp> #include <pinocchio/multibody/geometry.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -62,6 +63,8 @@ using hpp::core::SteeringMethodStraight; ...@@ -62,6 +63,8 @@ using hpp::core::SteeringMethodStraight;
using hpp::core::PathOptimizerPtr_t; using hpp::core::PathOptimizerPtr_t;
using hpp::core::pathOptimization::GradientBased; using hpp::core::pathOptimization::GradientBased;
using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelPX; using ::se3::JointModelPX;
using ::se3::JointModelPY; using ::se3::JointModelPY;
...@@ -76,6 +79,10 @@ DevicePtr_t createRobot () ...@@ -76,6 +79,10 @@ DevicePtr_t createRobot ()
{ {
DevicePtr_t robot = Device::create ("test"); DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name (); 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 (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
...@@ -96,6 +103,8 @@ DevicePtr_t createRobot () ...@@ -96,6 +103,8 @@ DevicePtr_t createRobot ()
robot->geomModel().addInnerObject(idX,idObj); robot->geomModel().addInnerObject(idX,idObj);
robot->createData();
robot->createGeomData();
return robot; return robot;
/* /*
DevicePtr_t robot = Device::create ("planar-robot"); DevicePtr_t robot = Device::create ("planar-robot");
......
...@@ -44,6 +44,7 @@ ...@@ -44,6 +44,7 @@
#define BOOST_TEST_MODULE kdTree #define BOOST_TEST_MODULE kdTree
#include <boost/test/included/unit_test.hpp> #include <boost/test/included/unit_test.hpp>
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -82,6 +83,10 @@ BOOST_AUTO_TEST_CASE (kdTree) { ...@@ -82,6 +83,10 @@ BOOST_AUTO_TEST_CASE (kdTree) {
so3Joint->addChildJoint (so2Joint); so3Joint->addChildJoint (so2Joint);
*/ */
DevicePtr_t robot = Device::create("robot"); 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 (); const std::string& name = robot->name ();
Transform3f mat; mat.setIdentity (); Transform3f mat; mat.setIdentity ();
...@@ -97,6 +102,9 @@ BOOST_AUTO_TEST_CASE (kdTree) { ...@@ -97,6 +102,9 @@ BOOST_AUTO_TEST_CASE (kdTree) {
idJoint = robot->model().addJoint(idJoint,JointModelSpherical(), mat,name + "_SO3"); idJoint = robot->model().addJoint(idJoint,JointModelSpherical(), mat,name + "_SO3");
idJoint = robot->model().addJoint(idJoint,JointModelRUBZ(), mat,name + "_SO2"); idJoint = robot->model().addJoint(idJoint,JointModelRUBZ(), mat,name + "_SO2");
robot->createData();
robot->createGeomData();
// Build Distance, nearestNeighbor, KDTree // Build Distance, nearestNeighbor, KDTree
Problem problem (robot); Problem problem (robot);
......
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