Skip to content
Snippets Groups Projects
Commit 6d172e4e authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #70 from jcarpent/topic/urdf 

Fix bug when parsing urdf file model by considering free floating joint
parents 007b06b2 d610392b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/joint/joint-base.hpp
+ 15
− 13
View file @ 6d172e4e
...@@ -204,6 +204,9 @@ namespace se3 ...@@ -204,6 +204,9 @@ namespace se3
{ {
typedef typename traits<_JointModel>::Joint Joint; typedef typename traits<_JointModel>::Joint Joint;
SE3_JOINT_TYPEDEF_TEMPLATE; SE3_JOINT_TYPEDEF_TEMPLATE;
typedef Eigen::Matrix<double,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<double,NV,1> TangentVector_t;
JointModel& derived() { return *static_cast<JointModel*>(this); } JointModel& derived() { return *static_cast<JointModel*>(this); }
const JointModel& derived() const { return *static_cast<const JointModel*>(this); } const JointModel& derived() const { return *static_cast<const JointModel*>(this); }
...@@ -221,12 +224,11 @@ namespace se3 ...@@ -221,12 +224,11 @@ namespace se3
int i_q; // Index of the joint configuration in the joint configuration vector. int i_q; // Index of the joint configuration in the joint configuration vector.
int i_v; // Index of the joint velocity in the joint velocity vector. int i_v; // Index of the joint velocity in the joint velocity vector.
Eigen::Matrix<double,NQ,1> position_lower; ConfigVector_t position_lower;
Eigen::Matrix<double,NQ,1> position_upper; ConfigVector_t position_upper;
Eigen::Matrix<double,NV,1> effortMax;
Eigen::Matrix<double,NV,1> velocityMax;
TangentVector_t effortMax;
TangentVector_t velocityMax;
int nv() const { return derived().nv_impl(); } int nv() const { return derived().nv_impl(); }
int nq() const { return derived().nq_impl(); } int nq() const { return derived().nq_impl(); }
...@@ -238,30 +240,30 @@ namespace se3 ...@@ -238,30 +240,30 @@ namespace se3
const int & idx_v() const { return i_v; } const int & idx_v() const { return i_v; }
const Index & id() const { return i_id; } const Index & id() const { return i_id; }
const Eigen::Matrix<double,NQ,1> & lowerPosLimit() const { return position_lower;} const ConfigVector_t & lowerPosLimit() const { return position_lower;}
const Eigen::Matrix<double,NQ,1> & upperPosLimit() const { return position_upper;} const ConfigVector_t & upperPosLimit() const { return position_upper;}
const Eigen::Matrix<double,NV,1> & maxEffortLimit() const { return effortMax;} const TangentVector_t & maxEffortLimit() const { return effortMax;}
const Eigen::Matrix<double,NV,1> & maxVelocityLimit() const { return velocityMax;} const TangentVector_t & maxVelocityLimit() const { return velocityMax;}
void setIndexes(Index id,int q,int v) { i_id = id, i_q = q; i_v = v; } void setIndexes(Index id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void setLowerPositionLimit(const Eigen::VectorXd & lowerPos) void setLowerPositionLimit(const ConfigVector_t & lowerPos)
{ {
position_lower = lowerPos; position_lower = lowerPos;
} }
void setUpperPositionLimit(const Eigen::VectorXd & upperPos) void setUpperPositionLimit(const ConfigVector_t & upperPos)
{ {
position_upper = upperPos; position_upper = upperPos;
} }
void setMaxEffortLimit(const Eigen::VectorXd & effort) void setMaxEffortLimit(const TangentVector_t & effort)
{ {
effortMax = effort; effortMax = effort;
} }
void setMaxVelocityLimit(const Eigen::VectorXd & v) void setMaxVelocityLimit(const TangentVector_t & v)
{ {
velocityMax = v; velocityMax = v;
} }
......
src/multibody/parser/urdf.hpp
+ 76
− 36
View file @ 6d172e4e
...@@ -29,6 +29,7 @@ ...@@ -29,6 +29,7 @@
#include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/model.hpp"
#include <exception> #include <exception>
#include <limits>
namespace urdf namespace urdf
{ {
...@@ -150,20 +151,50 @@ namespace se3 ...@@ -150,20 +151,50 @@ namespace se3
switch(joint->type) switch(joint->type)
{ {
case ::urdf::Joint::FLOATING:
{
joint_info = "joint FreeFlyer";
typedef JointModelFreeFlyer::ConfigVector_t ConfigVector_t;
typedef JointModelFreeFlyer::TangentVector_t TangentVector_t;
typedef ConfigVector_t::Scalar Scalar_t;
ConfigVector_t lower_position;
lower_position.fill(std::numeric_limits<Scalar_t>::min());
ConfigVector_t upper_position;
upper_position.fill(std::numeric_limits<Scalar_t>::max());
TangentVector_t max_effort;
max_effort.fill(std::numeric_limits<Scalar_t>::max());
TangentVector_t max_velocity;
max_velocity.fill(std::numeric_limits<Scalar_t>::max());
model.addBody(parent_joint_id, JointModelFreeFlyer(), jointPlacement, Y,
max_effort, max_velocity, lower_position, upper_position,
joint->name, link->name, has_visual);
break;
}
case ::urdf::Joint::REVOLUTE: case ::urdf::Joint::REVOLUTE:
{ {
joint_info = "joint REVOLUTE with axis"; joint_info = "joint REVOLUTE with axis";
Eigen::VectorXd maxEffort;
Eigen::VectorXd velocity; typedef JointModelRX::ConfigVector_t ConfigVector_t;
Eigen::VectorXd lowerPosition; typedef JointModelRX::TangentVector_t TangentVector_t;
Eigen::VectorXd upperPosition;
TangentVector_t max_effort;
TangentVector_t max_velocity;
ConfigVector_t lower_position;
ConfigVector_t upper_position;
if (joint->limits) if (joint->limits)
{ {
maxEffort.resize(1); maxEffort << joint->limits->effort; max_effort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity; max_velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower; lower_position << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper; upper_position << joint->limits->upper;
} }
Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero()); Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
...@@ -174,19 +205,19 @@ namespace se3 ...@@ -174,19 +205,19 @@ namespace se3
case AXIS_X: case AXIS_X:
joint_info += " along X"; joint_info += " along X";
model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_Y: case AXIS_Y:
joint_info += " along Y"; joint_info += " along Y";
model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_Z: case AXIS_Z:
joint_info += " along Z"; joint_info += " along Z";
model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_UNALIGNED: case AXIS_UNALIGNED:
...@@ -201,7 +232,7 @@ namespace se3 ...@@ -201,7 +232,7 @@ namespace se3
jointAxis.normalize(); jointAxis.normalize();
model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis), model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
jointPlacement, Y, jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
} }
...@@ -214,17 +245,21 @@ namespace se3 ...@@ -214,17 +245,21 @@ namespace se3
case ::urdf::Joint::CONTINUOUS: // Revolute with no joint limits case ::urdf::Joint::CONTINUOUS: // Revolute with no joint limits
{ {
joint_info = "joint CONTINUOUS with axis"; joint_info = "joint CONTINUOUS with axis";
Eigen::VectorXd maxEffort;
Eigen::VectorXd velocity; typedef JointModelRX::ConfigVector_t ConfigVector_t;
Eigen::VectorXd lowerPosition; typedef JointModelRX::TangentVector_t TangentVector_t;
Eigen::VectorXd upperPosition;
TangentVector_t max_effort;
TangentVector_t max_velocity;
ConfigVector_t lower_position;
ConfigVector_t upper_position;
if (joint->limits) if (joint->limits)
{ {
maxEffort.resize(1); maxEffort << joint->limits->effort; max_effort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity; max_velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower; lower_position << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper; upper_position << joint->limits->upper;
} }
Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero()); Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
...@@ -235,19 +270,19 @@ namespace se3 ...@@ -235,19 +270,19 @@ namespace se3
case AXIS_X: case AXIS_X:
joint_info += " along X"; joint_info += " along X";
model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_Y: case AXIS_Y:
joint_info += " along Y"; joint_info += " along Y";
model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_Z: case AXIS_Z:
joint_info += " along Z"; joint_info += " along Z";
model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_UNALIGNED: case AXIS_UNALIGNED:
...@@ -261,7 +296,7 @@ namespace se3 ...@@ -261,7 +296,7 @@ namespace se3
jointAxis.normalize(); jointAxis.normalize();
model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis), model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
jointPlacement, Y, jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
} }
...@@ -274,37 +309,42 @@ namespace se3 ...@@ -274,37 +309,42 @@ namespace se3
case ::urdf::Joint::PRISMATIC: case ::urdf::Joint::PRISMATIC:
{ {
joint_info = "joint PRISMATIC with axis"; joint_info = "joint PRISMATIC with axis";
Eigen::VectorXd maxEffort = Eigen::VectorXd(0.);
Eigen::VectorXd velocity = Eigen::VectorXd(0.); typedef JointModelRX::ConfigVector_t ConfigVector_t;
Eigen::VectorXd lowerPosition = Eigen::VectorXd(0.); typedef JointModelRX::TangentVector_t TangentVector_t;
Eigen::VectorXd upperPosition = Eigen::VectorXd(0.);
TangentVector_t max_effort;
TangentVector_t max_velocity;
ConfigVector_t lower_position;
ConfigVector_t upper_position;
if (joint->limits) if (joint->limits)
{ {
maxEffort.resize(1); maxEffort << joint->limits->effort; max_effort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity; max_velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower; lower_position << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper; upper_position << joint->limits->upper;
} }
AxisCartesian axis = extractCartesianAxis(joint->axis); AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis) switch(axis)
{ {
case AXIS_X: case AXIS_X:
joint_info += " along X"; joint_info += " along X";
model.addBody( parent_joint_id, JointModelPX(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelPX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_Y: case AXIS_Y:
joint_info += " along Y"; joint_info += " along Y";
model.addBody( parent_joint_id, JointModelPY(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelPY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_Z: case AXIS_Z:
joint_info += " along Z"; joint_info += " along Z";
model.addBody( parent_joint_id, JointModelPZ(), jointPlacement, Y, model.addBody( parent_joint_id, JointModelPZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition, max_effort, max_velocity, lower_position, upper_position,
joint->name,link->name, has_visual ); joint->name,link->name, has_visual );
break; break;
case AXIS_UNALIGNED: case AXIS_UNALIGNED:
......
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