Commit 86beca06 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

working on test-configProjector

parent 05ab620a
...@@ -58,13 +58,13 @@ MACRO(ADD_TESTCASE NAME GENERATED) ...@@ -58,13 +58,13 @@ MACRO(ADD_TESTCASE NAME GENERATED)
ENDMACRO(ADD_TESTCASE) ENDMACRO(ADD_TESTCASE)
ADD_TESTCASE (test-kdTree FALSE) #ADD_TESTCASE (test-kdTree FALSE)
ADD_TESTCASE (roadmap-1 FALSE) #ADD_TESTCASE (roadmap-1 FALSE)
ADD_TESTCASE (test-intervals FALSE) #ADD_TESTCASE (test-intervals FALSE)
#ADD_TESTCASE (test-body-pair-collision 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 (test-configprojector FALSE)
ADD_TESTCASE (path-projectors FALSE) # link error #ADD_TESTCASE (path-projectors FALSE) # link error
ADD_TESTCASE (containers FALSE) #ADD_TESTCASE (containers FALSE)
ADD_TESTCASE (paths FALSE) #ADD_TESTCASE (paths FALSE)
ADD_TESTCASE (relative-motion FALSE) #ADD_TESTCASE (relative-motion FALSE)
...@@ -57,8 +57,9 @@ using hpp::pinocchio::DevicePtr_t; ...@@ -57,8 +57,9 @@ using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t; using hpp::pinocchio::JointPtr_t;
using namespace hpp::core; using namespace hpp::core;
using ::se3::JointModelRX; using ::se3::JointModelPX;
using ::se3::JointModelRY; using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex; using ::se3::JointIndex;
DevicePtr_t createRobot () DevicePtr_t createRobot ()
...@@ -68,20 +69,20 @@ DevicePtr_t createRobot () ...@@ -68,20 +69,20 @@ DevicePtr_t createRobot ()
Transform3f mat; mat.setIdentity (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
JointModelRX::TangentVector_t max_effort = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelRX::TangentVector_t max_velocity = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelRX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position(-4);
JointModelRX::ConfigVector_t upper_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()); JointModelPY::TangentVector_t max_effortY = JointModelPY::TangentVector_t::Constant(JointModelPY::NV,std::numeric_limits<double>::max());
JointModelRY::TangentVector_t max_velocityY = JointModelRY::TangentVector_t::Constant(JointModelRY::NV,std::numeric_limits<double>::max()); JointModelPY::TangentVector_t max_velocityY = JointModelPY::TangentVector_t::Constant(JointModelPY::NV,std::numeric_limits<double>::max());
JointModelRY::ConfigVector_t lower_positionY(-4); JointModelPY::ConfigVector_t lower_positionY(-4);
JointModelRY::ConfigVector_t upper_positionY(4); JointModelPY::ConfigVector_t upper_positionY(4);
std::string jointNameY = name + "_y"; 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; return robot;
......
...@@ -49,8 +49,9 @@ using hpp::pinocchio::DevicePtr_t; ...@@ -49,8 +49,9 @@ using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t; using hpp::pinocchio::JointPtr_t;
using namespace hpp::core; using namespace hpp::core;
using ::se3::JointModelRX; using ::se3::JointModelPX;
using ::se3::JointModelRY; using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex; using ::se3::JointIndex;
DevicePtr_t createRobot () DevicePtr_t createRobot ()
...@@ -60,12 +61,12 @@ DevicePtr_t createRobot () ...@@ -60,12 +61,12 @@ DevicePtr_t createRobot ()
Transform3f mat; mat.setIdentity (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
JointModelRX::TangentVector_t max_effort = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelRX::TangentVector_t max_velocity = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelRX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position(-4);
JointModelRX::ConfigVector_t upper_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; return robot;
...@@ -97,14 +98,14 @@ DevicePtr_t createRobot2 () ...@@ -97,14 +98,14 @@ DevicePtr_t createRobot2 ()
Transform3f mat; mat.setIdentity (); Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x"; std::string jointName = name + "_x";
JointModelRX::TangentVector_t max_effort = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelRX::TangentVector_t max_velocity = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max()); JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelRX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position(-4);
JointModelRX::ConfigVector_t upper_position(4); JointModelPX::ConfigVector_t upper_position(4);
JointIndex idJoint = 0; JointIndex idJoint = 0;
for(int i = 0 ; i < 10 ; ++i){ 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; return robot;
...@@ -158,7 +159,7 @@ void checkAt(const PathPtr_t orig, value_type to, ...@@ -158,7 +159,7 @@ void checkAt(const PathPtr_t orig, value_type to,
BOOST_AUTO_TEST_CASE (extracted) BOOST_AUTO_TEST_CASE (extracted)
{ {
DevicePtr_t dev = hppPinocchio (); DevicePtr_t dev = createRobot();
BOOST_REQUIRE (dev); BOOST_REQUIRE (dev);
Problem problem (dev); Problem problem (dev);
...@@ -186,7 +187,7 @@ BOOST_AUTO_TEST_CASE (extracted) ...@@ -186,7 +187,7 @@ BOOST_AUTO_TEST_CASE (extracted)
BOOST_AUTO_TEST_CASE (subchain) BOOST_AUTO_TEST_CASE (subchain)
{ {
DevicePtr_t dev = hppPinocchio (); // 10 translations DevicePtr_t dev = createRobot2(); // 10 translations
BOOST_REQUIRE (dev); BOOST_REQUIRE (dev);
Problem problem (dev); Problem problem (dev);
......
...@@ -50,6 +50,7 @@ ...@@ -50,6 +50,7 @@
#include <hpp/core/constraint-set.hh> #include <hpp/core/constraint-set.hh>
#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 "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -60,11 +61,39 @@ using hpp::pinocchio::JointPtr_t; ...@@ -60,11 +61,39 @@ using hpp::pinocchio::JointPtr_t;
using hpp::constraints::RelativeTransformation; using hpp::constraints::RelativeTransformation;
using namespace hpp::core; using namespace hpp::core;
using ::se3::JointModelPX;
using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex;
/*
DevicePtr_t createRobot () DevicePtr_t createRobot ()
{ {
DevicePtr_t robot = Device::create ("test"); 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 (); const std::string& name = robot->name ();
fcl::Transform3f mat; mat.setIdentity (); fcl::Transform3f mat; mat.setIdentity ();
...@@ -108,8 +137,8 @@ DevicePtr_t createRobot () ...@@ -108,8 +137,8 @@ DevicePtr_t createRobot ()
parent->addChildJoint (joint); parent->addChildJoint (joint);
parent = joint; parent = joint;
} }
return robot; return robot;*/
}*/ }
void lockJoint (ConfigProjectorPtr_t proj, DevicePtr_t dev, std::string name) void lockJoint (ConfigProjectorPtr_t proj, DevicePtr_t dev, std::string name)
{ {
...@@ -132,7 +161,7 @@ struct Jidx { ...@@ -132,7 +161,7 @@ struct Jidx {
BOOST_AUTO_TEST_CASE (relativeMotion) BOOST_AUTO_TEST_CASE (relativeMotion)
{ {
DevicePtr_t dev = hppPinocchio(); DevicePtr_t dev = createRobot();
BOOST_REQUIRE (dev); BOOST_REQUIRE (dev);
Jidx jointid; Jidx jointid;
jointid.dev = dev; jointid.dev = dev;
......
...@@ -34,6 +34,8 @@ ...@@ -34,6 +34,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>
...@@ -53,6 +55,10 @@ using hpp::core::RoadmapPtr_t; ...@@ -53,6 +55,10 @@ using hpp::core::RoadmapPtr_t;
using hpp::core::Roadmap; using hpp::core::Roadmap;
using hpp::core::NodePtr_t; using hpp::core::NodePtr_t;
using hpp::core::WeighedDistance; using hpp::core::WeighedDistance;
using ::se3::JointModelPX;
using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex;
BOOST_AUTO_TEST_SUITE( test_hpp_core ) BOOST_AUTO_TEST_SUITE( test_hpp_core )
...@@ -81,7 +87,19 @@ BOOST_AUTO_TEST_CASE (Roadmap1) { ...@@ -81,7 +87,19 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
robot->rootJoint (xJoint); robot->rootJoint (xJoint);
xJoint->addChildJoint (yJoint); 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 // Create steering method
Problem p = Problem (robot); Problem p = Problem (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p); SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);
...@@ -292,8 +310,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) { ...@@ -292,8 +310,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
BOOST_AUTO_TEST_CASE (nearestNeighbor) { BOOST_AUTO_TEST_CASE (nearestNeighbor) {
// Build robot // Build robot
DevicePtr_t robot = hppPinocchio(); /* DevicePtr_t robot = Device::create("robot");
/*JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f()); JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f());
xJoint->isBounded(0,1); xJoint->isBounded(0,1);
xJoint->lowerBound(0,-3.); xJoint->lowerBound(0,-3.);
xJoint->upperBound(0,3.); xJoint->upperBound(0,3.);
...@@ -306,6 +324,21 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) { ...@@ -306,6 +324,21 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
robot->rootJoint (xJoint); robot->rootJoint (xJoint);
xJoint->addChildJoint (yJoint);*/ 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 // Create steering method
Problem p (robot); Problem p (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p); SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);
......
...@@ -25,7 +25,7 @@ ...@@ -25,7 +25,7 @@
#include <hpp/pinocchio/configuration.hh> #include <hpp/pinocchio/configuration.hh>
#include <hpp/constraints/generic-transformation.hh> #include <hpp/constraints/generic-transformation.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -40,6 +40,16 @@ using hpp::constraints::matrix3_t; ...@@ -40,6 +40,16 @@ using hpp::constraints::matrix3_t;
using hpp::constraints::vector3_t; using hpp::constraints::vector3_t;
using namespace hpp::core; 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) JointPtr_t createFreeflyerJoint (DevicePtr_t robot)
...@@ -95,9 +105,38 @@ JointPtr_t createFreeflyerJoint (DevicePtr_t robot) ...@@ -95,9 +105,38 @@ JointPtr_t createFreeflyerJoint (DevicePtr_t robot)
joint->name (jointName); joint->name (jointName);
parent->addChildJoint (joint); parent->addChildJoint (joint);
return 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"); DevicePtr_t robot = Device::create ("test");
JointPtr_t waist = createFreeflyerJoint (robot); JointPtr_t waist = createFreeflyerJoint (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