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

tests roadmap, config-projector and relative-motion done

parent 86beca06
......@@ -29,54 +29,53 @@ config-validations.cc
connected-component.cc
constraint.cc
constraint-set.cc
continuous-collision-checking/dichotomy.cc
continuous-collision-checking/dichotomy/body-pair-collision.hh
continuous-collision-checking/progressive.cc
continuous-collision-checking/progressive/body-pair-collision.hh
#diffusing-planner.cc
#discretized-collision-checking.cc
#discretized-path-validation.cc
#distance-between-objects.cc
#explicit-numerical-constraint.cc
#extracted-path.hh
#equation.cc
#interpolated-path.cc
#joint-bound-validation.cc
#locked-joint.cc
##########################"
# nearest-neighbor/basic.hh #
# nearest-neighbor/basic.cc #
# nearest-neighbor/k-d-tree.cc #
# nearest-neighbor/k-d-tree.hh #
# numerical-constraint.cc #
# node.cc #
# path.cc #
# path-optimizer.cc #
# path-optimization/collision-constraints-result.hh #
# path-optimization/path-length.cc #
# path-optimization/gradient-based.cc #
# path-optimization/partial-shortcut.cc #
# path-optimization/config-optimization.cc#
# path-planner.cc #
# path-vector.cc #
# plan-and-optimize.cc #
# problem.cc # TODO : addObstacle
# problem-solver.cc # TODO : addObstacle
# path-projector/progressive.cc #
# path-projector/dichotomy.cc #
# path-projector/global.cc #
# path-projector.cc #
# parser/roadmap-factory.cc #
# problem-target/goal-configurations.cc #
# problem-target/task-target.cc #
# reeds-shepp-path.cc
# relative-motion.cc
# random-shortcut.cc
# roadmap.cc
# steering-method/reeds-shepp.cc # TODO access type of joint
# straight-path.cc
# visibility-prm-planner.cc
# weighed-distance.cc
#continuous-collision-checking/dichotomy.cc
#continuous-collision-checking/dichotomy/body-pair-collision.hh
#continuous-collision-checking/progressive.cc
#continuous-collision-checking/progressive/body-pair-collision.hh
diffusing-planner.cc
discretized-collision-checking.cc
discretized-path-validation.cc
distance-between-objects.cc
explicit-numerical-constraint.cc
extracted-path.hh
equation.cc
interpolated-path.cc
joint-bound-validation.cc
locked-joint.cc
nearest-neighbor/basic.hh #
nearest-neighbor/basic.cc #
nearest-neighbor/k-d-tree.cc #
nearest-neighbor/k-d-tree.hh #
numerical-constraint.cc #
node.cc #
path.cc #
path-optimizer.cc #
path-optimization/collision-constraints-result.hh #
path-optimization/path-length.cc #
path-optimization/gradient-based.cc #
path-optimization/partial-shortcut.cc #
path-optimization/config-optimization.cc#
path-planner.cc #
path-vector.cc #
plan-and-optimize.cc #
problem.cc # TODO : addObstacle
problem-solver.cc # TODO : addObstacle
path-projector/progressive.cc #
path-projector/dichotomy.cc #
path-projector/global.cc #
path-projector.cc #
parser/roadmap-factory.cc #
problem-target/goal-configurations.cc #
problem-target/task-target.cc #
reeds-shepp-path.cc
relative-motion.cc
random-shortcut.cc
roadmap.cc
steering-method/reeds-shepp.cc # TODO access type of joint
straight-path.cc
visibility-prm-planner.cc
weighed-distance.cc
)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
......
......@@ -73,7 +73,7 @@ namespace hpp {
/// \param tolerance allowed penetration should be positive
/// \pre objects_b should not be attached to a joint
static BodyPairCollisionPtr_t create (const JointConstPtr_t& joint_a,
const std::vector<CollisionObjectPtr_t>& objects_b,
const std::vector<CollisionObjectPtr_t>& objects_b,
value_type tolerance)
{
BodyPairCollisionPtr_t shPtr (new BodyPairCollision
......
......@@ -59,12 +59,12 @@ MACRO(ADD_TESTCASE NAME GENERATED)
ENDMACRO(ADD_TESTCASE)
#ADD_TESTCASE (test-kdTree FALSE)
#ADD_TESTCASE (roadmap-1 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-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)
......@@ -71,18 +71,18 @@ DevicePtr_t createRobot ()
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);
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(4);
JointIndex idX = robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
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);
JointModelPY::ConfigVector_t lower_positionY = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPY::ConfigVector_t upper_positionY = JointModelPY::ConfigVector_t::Constant(4);
std::string jointNameY = name + "_y";
robot->model().addJoint(idX,JointModelRY(), 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);
return robot;
......
......@@ -100,8 +100,8 @@ DevicePtr_t createRobot2 ()
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);
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(4);
JointIndex idJoint = 0;
for(int i = 0 ; i < 10 ; ++i){
......
......@@ -77,8 +77,8 @@ DevicePtr_t createRobot ()
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);
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(4);
JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
......
......@@ -93,8 +93,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
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);
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-3);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(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);
......@@ -328,10 +328,10 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
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);
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 = JointModelPY::ConfigVector_t::Constant(-3);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(3);
JointIndex idJoint = robot->model().addJoint(0,::se3::JointModelPX(), mat,name + "_x",max_effort,max_velocity,lower_position,upper_position);
......
......@@ -23,6 +23,7 @@
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/body.hh>
#include <hpp/constraints/generic-transformation.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp>
......@@ -32,7 +33,7 @@
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
using hpp::pinocchio::BodyPtr_t;
using hpp::pinocchio::Body;
using hpp::constraints::Position;
using hpp::constraints::PositionPtr_t;
......@@ -40,6 +41,7 @@ using hpp::constraints::matrix3_t;
using hpp::constraints::vector3_t;
using namespace hpp::core;
using ::se3::JointModelRX;
using ::se3::JointModelPX;
using ::se3::JointModelPY;
using ::se3::JointModelPZ;
......@@ -115,8 +117,8 @@ DevicePtr_t createRobot (){
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);
JointModelPX::ConfigVector_t lower_position = JointModelPX::ConfigVector_t::Constant(-4.0);
JointModelPX::ConfigVector_t upper_position = JointModelPX::ConfigVector_t::Constant(4.0);
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);
......@@ -124,6 +126,11 @@ DevicePtr_t createRobot (){
JointIndex waist = robot->model().addJoint(idJoint,JointModelSpherical(), mat,name + "_SO3");
JointModelRX::TangentVector_t max_effortRot = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
JointModelRX::TangentVector_t max_velocityRot = JointModelRX::TangentVector_t::Constant(JointModelRX::NV,std::numeric_limits<double>::max());
JointModelRX::ConfigVector_t lower_positionRot = JointModelRX::ConfigVector_t::Constant(-M_PI);
JointModelRX::ConfigVector_t upper_positionRot = JointModelRX::ConfigVector_t::Constant(M_PI);
Transform3f pos;
Matrix3 orient;
// Right leg joint 0
......@@ -133,10 +140,124 @@ DevicePtr_t createRobot (){
pos.rotation (orient);
pos.translation ( Vector3(0, -0.08, 0));
JointIndex idLeg = robot->model().addJoint(waist,JointModelRX(), pos,"RLEG_0",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"RLEG_BODY0");
// Right leg joint 1
orient (0,0) = 1; orient (0,1) = 0; orient (0,2) = 0;
orient (1,0) = 0; orient (1,1) = 1; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, 0));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"RLEG_1",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"RLEG_BODY1");
// Right leg joint 2
orient (0,0) = 0; orient (0,1) = -1; orient (0,2) = 0;
orient (1,0) = 1; orient (1,1) = 0; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, 0));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"RLEG_2",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"RLEG_BODY2");
// Right leg joint 3: knee
orient (0,0) = 0; orient (0,1) = -1; orient (0,2) = 0;
orient (1,0) = 1; orient (1,1) = 0; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, -0.35));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"RLEG_3",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"RLEG_BODY3");
// Right leg joint 4: ankle
orient (0,0) = 0; orient (0,1) = -1; orient (0,2) = 0;
orient (1,0) = 1; orient (1,1) = 0; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, -0.70));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"RLEG_4",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"RLEG_BODY4");
// Right leg joint 5: ankle
orient (0,0) = 1; orient (0,1) = 0; orient (0,2) = 0;
orient (1,0) = 0; orient (1,1) = 1; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, -0.70));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"RLEG_5",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"RLEG_BODY5");
// left leg
// left 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));
idLeg = robot->model().addJoint(waist,JointModelRX(), pos,"LLEG_0",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY0");
// left leg joint 1
orient (0,0) = 1; orient (0,1) = 0; orient (0,2) = 0;
orient (1,0) = 0; orient (1,1) = 1; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, 0));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_1",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY1");
// left leg joint 2
orient (0,0) = 0; orient (0,1) = -1; orient (0,2) = 0;
orient (1,0) = 1; orient (1,1) = 0; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, 0));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_2",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY2");
// left leg joint 3: knee
orient (0,0) = 0; orient (0,1) = -1; orient (0,2) = 0;
orient (1,0) = 1; orient (1,1) = 0; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, -0.35));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_3",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY3");
// left leg joint 4: ankle
orient (0,0) = 0; orient (0,1) = -1; orient (0,2) = 0;
orient (1,0) = 1; orient (1,1) = 0; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, -0.70));
idLeg = robot->model().addJoint(idLeg,JointModelRX(), pos,"LLEG_4",max_effortRot,max_velocityRot,lower_positionRot,upper_positionRot);
robot->model().appendBodyToJoint(idLeg,::se3::Inertia::Identity(),::se3::SE3::Identity(),"LLEG_BODY4");
// left leg joint 5: ankle
orient (0,0) = 1; orient (0,1) = 0; orient (0,2) = 0;
orient (1,0) = 0; orient (1,1) = 1; orient (1,2) = 0;
orient (2,0) = 0; orient (2,1) = 0; orient (2,2) = 1;
pos.rotation (orient);
pos.translation (fcl::Vec3f (0, -0.08, -0.70));
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");
return robot;
}
/*DevicePtr_t createRobot ()
/*
DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("test");
JointPtr_t waist = createFreeflyerJoint (robot);
......@@ -309,7 +430,7 @@ BOOST_AUTO_TEST_SUITE (config_projector)
BOOST_AUTO_TEST_CASE (ref_zero)
{
DevicePtr_t dev = hppPinocchio ();
DevicePtr_t dev = createRobot();
JointPtr_t xyz = dev->getJointByName ("test_z");
matrix3_t rot; rot.setIdentity ();
vector3_t zero; zero.setZero();
......@@ -345,7 +466,7 @@ BOOST_AUTO_TEST_CASE (ref_zero)
BOOST_AUTO_TEST_CASE (ref_not_zero)
{
DevicePtr_t dev = hppPinocchio ();
DevicePtr_t dev = createRobot();
JointPtr_t xyz = dev->getJointByName ("test_z");
matrix3_t rot; rot.setIdentity ();
vector3_t zero; zero.setZero();
......
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