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 @@
#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;
/*
......
......@@ -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");
......
......@@ -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");
......
......@@ -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
......
......@@ -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;
}
......
......@@ -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;
}
/*
......
......@@ -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");
......
......@@ -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);
......
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