Commit 3bbef56c authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update to changes in pinocchio

parent 7f4e1bc8
...@@ -245,7 +245,7 @@ namespace hpp { ...@@ -245,7 +245,7 @@ namespace hpp {
{ {
pinocchio::JointVector_t& jv = robot_->getJointVector (); pinocchio::JointVector_t& jv = robot_->getJointVector ();
for (size_type idx = 0; idx < jv.size (); ++idx) { for (size_type idx = 0; idx < jv.size (); ++idx) {
JointPtr_t j = jv[idx]; JointPtr_t j = jv[(int)idx];
BodyPtr_t body = j->linkedBody (); BodyPtr_t body = j->linkedBody ();
bool foundPair = false; bool foundPair = false;
if (body) { if (body) {
......
...@@ -18,6 +18,7 @@ ...@@ -18,6 +18,7 @@
#include <hpp/util/debug.hh> #include <hpp/util/debug.hh>
#include "pinocchio/multibody/joint/joint-composite.hpp" // TODO remove me when pinocchio handles this.
#include <pinocchio/multibody/joint/joint.hpp> #include <pinocchio/multibody/joint/joint.hpp>
#include <hpp/pinocchio/joint.hh> #include <hpp/pinocchio/joint.hh>
......
...@@ -16,6 +16,7 @@ ...@@ -16,6 +16,7 @@
#include <hpp/core/steering-method/reeds-shepp.hh> #include <hpp/core/steering-method/reeds-shepp.hh>
#include <pinocchio/multibody/joint/joint-composite.hpp> // TODO remove me when pinocchio handles this.
#include <pinocchio/multibody/joint/joint.hpp> #include <pinocchio/multibody/joint/joint.hpp>
#include <hpp/pinocchio/device.hh> #include <hpp/pinocchio/device.hh>
......
...@@ -82,7 +82,7 @@ DevicePtr_t createRobot () ...@@ -82,7 +82,7 @@ DevicePtr_t createRobot ()
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(JointModelPX::NQ,-4); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(JointModelPX::NQ,-4);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(JointModelPX::NQ,4); JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(JointModelPX::NQ,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,max_effort,max_velocity,lower_position,upper_position,jointName);
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());
...@@ -90,7 +90,7 @@ DevicePtr_t createRobot () ...@@ -90,7 +90,7 @@ DevicePtr_t createRobot ()
JointModelPY::ConfigVector_t upper_positionY = JointModelPY::ConfigVector_t::Constant(JointModelPY::NQ,4); JointModelPY::ConfigVector_t upper_positionY = JointModelPY::ConfigVector_t::Constant(JointModelPY::NQ,4);
std::string jointNameY = name + "_y"; std::string jointNameY = name + "_y";
robot->model().addJoint(idX,JointModelPY(), mat,jointNameY,max_effortY,max_velocityY,lower_positionY,upper_positionY); robot->model().addJoint(idX,JointModelPY(), mat,max_effortY,max_velocityY,lower_positionY,upper_positionY,jointNameY);
robot->createData(); robot->createData();
robot->createGeomData(); robot->createGeomData();
......
...@@ -74,7 +74,7 @@ DevicePtr_t createRobot () ...@@ -74,7 +74,7 @@ DevicePtr_t createRobot ()
JointModelPX::ConfigVector_t lower_position(-4); JointModelPX::ConfigVector_t lower_position(-4);
JointModelPX::ConfigVector_t upper_position(4); JointModelPX::ConfigVector_t upper_position(4);
robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position); robot->model().addJoint(0,JointModelPX(), mat,max_effort,max_velocity,lower_position,upper_position,jointName);
robot->createData(); robot->createData();
...@@ -121,7 +121,7 @@ DevicePtr_t createRobot2 () ...@@ -121,7 +121,7 @@ DevicePtr_t createRobot2 ()
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,JointModelPX(), mat,jointName + TOSTR(i),max_effort,max_velocity,lower_position,upper_position); idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,max_effort,max_velocity,lower_position,upper_position,jointName + TOSTR(i));
} }
......
...@@ -88,15 +88,15 @@ DevicePtr_t createRobot () ...@@ -88,15 +88,15 @@ DevicePtr_t createRobot ()
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_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); JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,max_effort,max_velocity,lower_position,upper_position,jointName);
std::string bname = "joint_a"; std::string bname = "joint_a";
for (int i = 0; i < 2; ++i) { 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); idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,max_effort,max_velocity,lower_position,upper_position,bname+TOSTR(i));
} }
bname = "joint_b"; bname = "joint_b";
for (int i = 0; i < 3; ++i) { 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); idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,max_effort,max_velocity,lower_position,upper_position,bname+TOSTR(i));
} }
......
...@@ -106,8 +106,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) { ...@@ -106,8 +106,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-3); JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-3);
JointModelPX::ConfigVector_t upper_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); JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,max_effort,max_velocity,lower_position,upper_position,name + "_x");
robot->model().addJoint(idJoint,JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position); robot->model().addJoint(idJoint,JointModelPY(), mat,max_effort,max_velocity,lower_position,upper_position,name + "_y");
robot->createData(); robot->createData();
robot->createGeomData(); robot->createGeomData();
...@@ -350,8 +350,8 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) { ...@@ -350,8 +350,8 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
JointModelPX::ConfigVector_t upper_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); JointIndex idJoint = robot->model().addJoint(0,::se3::JointModelPX(), mat,max_effort,max_velocity,lower_position,upper_position,name + "_x");
robot->model().addJoint(idJoint,::se3::JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position); robot->model().addJoint(idJoint,::se3::JointModelPY(), mat,max_effort,max_velocity,lower_position,upper_position,name + "_y");
robot->createData(); robot->createData();
robot->createGeomData(); robot->createGeomData();
......
...@@ -43,7 +43,7 @@ ...@@ -43,7 +43,7 @@
#define BOOST_TEST_MODULE kdTree #define BOOST_TEST_MODULE kdTree
#include <boost/test/included/unit_test.hpp> #include <boost/test/included/unit_test.hpp>
#include <pinocchio/multibody/joint/joint-variant.hpp> #include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/geometry.hpp> #include <pinocchio/multibody/geometry.hpp>
#include "../tests/utils.hh" #include "../tests/utils.hh"
...@@ -52,9 +52,7 @@ using namespace hpp; ...@@ -52,9 +52,7 @@ using namespace hpp;
using namespace core; using namespace core;
using namespace pinocchio; using namespace pinocchio;
using namespace std; using namespace std;
using ::se3::JointModelPX; using ::se3::JointModelTranslation;
using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex; using ::se3::JointIndex;
using ::se3::JointModelSpherical; using ::se3::JointModelSpherical;
using ::se3::JointModelRUBZ; using ::se3::JointModelRUBZ;
...@@ -90,17 +88,16 @@ BOOST_AUTO_TEST_CASE (kdTree) { ...@@ -90,17 +88,16 @@ BOOST_AUTO_TEST_CASE (kdTree) {
const std::string& name = robot->name (); const std::string& name = robot->name ();
Transform3f mat; mat.setIdentity (); Transform3f mat; mat.setIdentity ();
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max()); JointModelTranslation::TangentVector_t max_effort = JointModelTranslation::TangentVector_t::Constant(std::numeric_limits<double>::max());
JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max()); JointModelTranslation::TangentVector_t max_velocity = JointModelTranslation::TangentVector_t::Constant(std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-3.); JointModelTranslation::ConfigVector_t lower_position = JointModelTranslation::ConfigVector_t::Constant(-3.);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(3.); JointModelTranslation::ConfigVector_t upper_position = JointModelTranslation::ConfigVector_t::Constant(3.);
JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,name + "_x",max_effort,max_velocity,lower_position,upper_position); JointIndex idJoint = 0;
idJoint = robot->model().addJoint(idJoint,JointModelPY(), mat,name + "_y",max_effort,max_velocity,lower_position,upper_position); idJoint = robot->model().addJoint(idJoint,JointModelTranslation(), mat,max_effort,max_velocity,lower_position,upper_position,name + "_xyz");
idJoint = robot->model().addJoint(idJoint,JointModelPZ(), mat,name + "_z",max_effort,max_velocity,lower_position,upper_position); idJoint = robot->model().addJoint(idJoint,JointModelSpherical() , mat,name + "_SO3");
idJoint = robot->model().addJoint(idJoint,JointModelSpherical(), mat,name + "_SO3"); idJoint = robot->model().addJoint(idJoint,JointModelRUBZ() , mat,name + "_SO2");
idJoint = robot->model().addJoint(idJoint,JointModelRUBZ(), mat,name + "_SO2");
robot->createData(); robot->createData();
robot->createGeomData(); robot->createGeomData();
......
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