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)
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)
......@@ -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;
......
......@@ -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);
......
......@@ -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;
......
......@@ -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);
......
......@@ -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);
......
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