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 ...@@ -29,54 +29,53 @@ config-validations.cc
connected-component.cc connected-component.cc
constraint.cc constraint.cc
constraint-set.cc constraint-set.cc
continuous-collision-checking/dichotomy.cc #continuous-collision-checking/dichotomy.cc
continuous-collision-checking/dichotomy/body-pair-collision.hh #continuous-collision-checking/dichotomy/body-pair-collision.hh
continuous-collision-checking/progressive.cc #continuous-collision-checking/progressive.cc
continuous-collision-checking/progressive/body-pair-collision.hh #continuous-collision-checking/progressive/body-pair-collision.hh
#diffusing-planner.cc diffusing-planner.cc
#discretized-collision-checking.cc discretized-collision-checking.cc
#discretized-path-validation.cc discretized-path-validation.cc
#distance-between-objects.cc distance-between-objects.cc
#explicit-numerical-constraint.cc explicit-numerical-constraint.cc
#extracted-path.hh extracted-path.hh
#equation.cc equation.cc
#interpolated-path.cc interpolated-path.cc
#joint-bound-validation.cc joint-bound-validation.cc
#locked-joint.cc locked-joint.cc
##########################" nearest-neighbor/basic.hh #
# nearest-neighbor/basic.hh # nearest-neighbor/basic.cc #
# nearest-neighbor/basic.cc # nearest-neighbor/k-d-tree.cc #
# nearest-neighbor/k-d-tree.cc # nearest-neighbor/k-d-tree.hh #
# nearest-neighbor/k-d-tree.hh # numerical-constraint.cc #
# numerical-constraint.cc # node.cc #
# node.cc # path.cc #
# path.cc # path-optimizer.cc #
# path-optimizer.cc # path-optimization/collision-constraints-result.hh #
# path-optimization/collision-constraints-result.hh # path-optimization/path-length.cc #
# path-optimization/path-length.cc # path-optimization/gradient-based.cc #
# path-optimization/gradient-based.cc # path-optimization/partial-shortcut.cc #
# path-optimization/partial-shortcut.cc # path-optimization/config-optimization.cc#
# path-optimization/config-optimization.cc# path-planner.cc #
# path-planner.cc # path-vector.cc #
# path-vector.cc # plan-and-optimize.cc #
# plan-and-optimize.cc # problem.cc # TODO : addObstacle
# problem.cc # TODO : addObstacle problem-solver.cc # TODO : addObstacle
# problem-solver.cc # TODO : addObstacle path-projector/progressive.cc #
# path-projector/progressive.cc # path-projector/dichotomy.cc #
# path-projector/dichotomy.cc # path-projector/global.cc #
# path-projector/global.cc # path-projector.cc #
# path-projector.cc # parser/roadmap-factory.cc #
# parser/roadmap-factory.cc # problem-target/goal-configurations.cc #
# problem-target/goal-configurations.cc # problem-target/task-target.cc #
# problem-target/task-target.cc # reeds-shepp-path.cc
# reeds-shepp-path.cc relative-motion.cc
# relative-motion.cc random-shortcut.cc
# random-shortcut.cc roadmap.cc
# roadmap.cc steering-method/reeds-shepp.cc # TODO access type of joint
# steering-method/reeds-shepp.cc # TODO access type of joint straight-path.cc
# straight-path.cc visibility-prm-planner.cc
# visibility-prm-planner.cc weighed-distance.cc
# weighed-distance.cc
) )
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src) INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
......
...@@ -73,7 +73,7 @@ namespace hpp { ...@@ -73,7 +73,7 @@ namespace hpp {
/// \param tolerance allowed penetration should be positive /// \param tolerance allowed penetration should be positive
/// \pre objects_b should not be attached to a joint /// \pre objects_b should not be attached to a joint
static BodyPairCollisionPtr_t create (const JointConstPtr_t& joint_a, 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) value_type tolerance)
{ {
BodyPairCollisionPtr_t shPtr (new BodyPairCollision BodyPairCollisionPtr_t shPtr (new BodyPairCollision
......
...@@ -59,12 +59,12 @@ MACRO(ADD_TESTCASE NAME GENERATED) ...@@ -59,12 +59,12 @@ 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)
...@@ -71,18 +71,18 @@ DevicePtr_t createRobot () ...@@ -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_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::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position(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); 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_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::TangentVector_t max_velocityY = JointModelPY::TangentVector_t::Constant(JointModelPY::NV,std::numeric_limits<double>::max());
JointModelPY::ConfigVector_t lower_positionY(-4); JointModelPY::ConfigVector_t lower_positionY = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPY::ConfigVector_t upper_positionY(4); JointModelPY::ConfigVector_t upper_positionY = JointModelPY::ConfigVector_t::Constant(4);
std::string jointNameY = name + "_y"; 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; return robot;
......
...@@ -100,8 +100,8 @@ DevicePtr_t createRobot2 () ...@@ -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_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::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position(4); JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(4);
JointIndex idJoint = 0; JointIndex idJoint = 0;
for(int i = 0 ; i < 10 ; ++i){ for(int i = 0 ; i < 10 ; ++i){
......
...@@ -77,8 +77,8 @@ DevicePtr_t createRobot () ...@@ -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_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::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position(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); 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) { ...@@ -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_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::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position(-3); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-3);
JointModelPX::ConfigVector_t upper_position(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); 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);
...@@ -328,10 +328,10 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) { ...@@ -328,10 +328,10 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
const std::string& name = robot->name (); const std::string& name = robot->name ();
Transform3f mat; mat.setIdentity (); Transform3f mat; mat.setIdentity ();
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(-3); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-3);
JointModelRX::ConfigVector_t upper_position(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); JointIndex idJoint = robot->model().addJoint(0,::se3::JointModelPX(), mat,name + "_x",max_effort,max_velocity,lower_position,upper_position);
......
...@@ -23,6 +23,7 @@ ...@@ -23,6 +23,7 @@
#include <hpp/pinocchio/device.hh> #include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh> #include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh> #include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/body.hh>
#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>
...@@ -32,7 +33,7 @@ ...@@ -32,7 +33,7 @@
using hpp::pinocchio::Device; using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t; using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t; using hpp::pinocchio::JointPtr_t;
using hpp::pinocchio::BodyPtr_t; using hpp::pinocchio::Body;
using hpp::constraints::Position; using hpp::constraints::Position;
using hpp::constraints::PositionPtr_t; using hpp::constraints::PositionPtr_t;
...@@ -40,6 +41,7 @@ using hpp::constraints::matrix3_t; ...@@ -40,6 +41,7 @@ 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::JointModelRX;
using ::se3::JointModelPX; using ::se3::JointModelPX;
using ::se3::JointModelPY; using ::se3::JointModelPY;
using ::se3::JointModelPZ; using ::se3::JointModelPZ;
...@@ -115,8 +117,8 @@ DevicePtr_t createRobot (){ ...@@ -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_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::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position = JointModelPX::ConfigVector_t::Constant(-4.0);
JointModelPX::ConfigVector_t upper_position(4); 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); 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,JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position);
...@@ -124,6 +126,11 @@ DevicePtr_t createRobot (){ ...@@ -124,6 +126,11 @@ DevicePtr_t createRobot (){
JointIndex waist = robot->model().addJoint(idJoint,JointModelSpherical(), mat,name + "_SO3"); 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; Transform3f pos;
Matrix3 orient; Matrix3 orient;
// Right leg joint 0 // Right leg joint 0
...@@ -133,10 +140,124 @@ DevicePtr_t createRobot (){ ...@@ -133,10 +140,124 @@ DevicePtr_t createRobot (){
pos.rotation (orient); pos.rotation (orient);
pos.translation ( Vector3(0, -0.08, 0)); 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; 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);
...@@ -309,7 +430,7 @@ BOOST_AUTO_TEST_SUITE (config_projector) ...@@ -309,7 +430,7 @@ BOOST_AUTO_TEST_SUITE (config_projector)
BOOST_AUTO_TEST_CASE (ref_zero) BOOST_AUTO_TEST_CASE (ref_zero)
{ {
DevicePtr_t dev = hppPinocchio (); DevicePtr_t dev = createRobot();
JointPtr_t xyz = dev->getJointByName ("test_z"); JointPtr_t xyz = dev->getJointByName ("test_z");
matrix3_t rot; rot.setIdentity (); matrix3_t rot; rot.setIdentity ();
vector3_t zero; zero.setZero(); vector3_t zero; zero.setZero();
...@@ -345,7 +466,7 @@ BOOST_AUTO_TEST_CASE (ref_zero) ...@@ -345,7 +466,7 @@ BOOST_AUTO_TEST_CASE (ref_zero)
BOOST_AUTO_TEST_CASE (ref_not_zero) BOOST_AUTO_TEST_CASE (ref_not_zero)
{ {
DevicePtr_t dev = hppPinocchio (); DevicePtr_t dev = createRobot();
JointPtr_t xyz = dev->getJointByName ("test_z"); JointPtr_t xyz = dev->getJointByName ("test_z");
matrix3_t rot; rot.setIdentity (); matrix3_t rot; rot.setIdentity ();
vector3_t zero; zero.setZero(); 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