Commit fa66a493 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[Minor][C++] URDF parser uses JointModelRevoluteUnbounded for continuous

* When not set, the limits are set to infinity.
parent e6c054f0
...@@ -35,6 +35,7 @@ namespace se3 ...@@ -35,6 +35,7 @@ namespace se3
namespace details namespace details
{ {
const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT); const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT);
const double infty = std::numeric_limits<double>::infinity();
FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model) FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model)
{ {
...@@ -89,10 +90,10 @@ namespace se3 ...@@ -89,10 +90,10 @@ namespace se3
const SE3 & joint_placement, const std::string & joint_name, const SE3 & joint_placement, const std::string & joint_name,
const boost::shared_ptr< ::urdf::Inertial> Y, const boost::shared_ptr< ::urdf::Inertial> Y,
const std::string & body_name, const std::string & body_name,
const typename JointModel::TangentVector_t & max_effort = JointModel::TangentVector_t::Constant(std::numeric_limits<double>::max()), const typename JointModel::TangentVector_t & max_effort = JointModel::TangentVector_t::Constant( infty),
const typename JointModel::TangentVector_t & max_velocity = JointModel::TangentVector_t::Constant(std::numeric_limits<double>::max()), const typename JointModel::TangentVector_t & max_velocity = JointModel::TangentVector_t::Constant( infty),
const typename JointModel::ConfigVector_t & min_config = JointModel::ConfigVector_t::Constant(-std::numeric_limits<double>::min()), const typename JointModel::ConfigVector_t & min_config = JointModel::ConfigVector_t ::Constant(-infty),
const typename JointModel::ConfigVector_t & max_config = JointModel::ConfigVector_t::Constant(std::numeric_limits<double>::max())) const typename JointModel::ConfigVector_t & max_config = JointModel::ConfigVector_t ::Constant( infty))
{ {
Model::JointIndex idx; Model::JointIndex idx;
Frame& frame = model.frames[parentFrameId]; Frame& frame = model.frames[parentFrameId];
...@@ -261,20 +262,19 @@ namespace se3 ...@@ -261,20 +262,19 @@ namespace se3
{ {
joint_info << "joint CONTINUOUS with axis"; joint_info << "joint CONTINUOUS with axis";
typedef JointModelRX::ConfigVector_t ConfigVector_t; typedef JointModelRUBX::ConfigVector_t ConfigVector_t;
typedef JointModelRX::TangentVector_t TangentVector_t; typedef JointModelRUBX::TangentVector_t TangentVector_t;
TangentVector_t max_effort; TangentVector_t max_effort;
TangentVector_t max_velocity; TangentVector_t max_velocity;
ConfigVector_t lower_position; const ConfigVector_t::Scalar u = 1.01;
ConfigVector_t upper_position; ConfigVector_t lower_position(-u, -u);
ConfigVector_t upper_position( u, u);
if (joint->limits) if (joint->limits)
{ {
max_effort << joint->limits->effort; max_effort << joint->limits->effort;
max_velocity << joint->limits->velocity; max_velocity << joint->limits->velocity;
lower_position << joint->limits->lower;
upper_position << joint->limits->upper;
} }
CartesianAxis axis = extractCartesianAxis(joint->axis); CartesianAxis axis = extractCartesianAxis(joint->axis);
...@@ -284,7 +284,7 @@ namespace se3 ...@@ -284,7 +284,7 @@ namespace se3
case AXIS_X: case AXIS_X:
{ {
joint_info << " along X"; joint_info << " along X";
addJointAndBody(model,JointModelRX(), addJointAndBody(model,JointModelRUBX(),
parentFrameId,jointPlacement,joint->name, parentFrameId,jointPlacement,joint->name,
Y,link->name, Y,link->name,
max_effort,max_velocity, max_effort,max_velocity,
...@@ -294,7 +294,7 @@ namespace se3 ...@@ -294,7 +294,7 @@ namespace se3
case AXIS_Y: case AXIS_Y:
{ {
joint_info << " along Y"; joint_info << " along Y";
addJointAndBody(model,JointModelRY(), addJointAndBody(model,JointModelRUBY(),
parentFrameId,jointPlacement,joint->name, parentFrameId,jointPlacement,joint->name,
Y,link->name, Y,link->name,
max_effort,max_velocity, max_effort,max_velocity,
...@@ -304,7 +304,7 @@ namespace se3 ...@@ -304,7 +304,7 @@ namespace se3
case AXIS_Z: case AXIS_Z:
{ {
joint_info << " along Z"; joint_info << " along Z";
addJointAndBody(model,JointModelRZ(), addJointAndBody(model,JointModelRUBZ(),
parentFrameId,jointPlacement,joint->name, parentFrameId,jointPlacement,joint->name,
Y,link->name, Y,link->name,
max_effort,max_velocity, max_effort,max_velocity,
...@@ -320,8 +320,7 @@ namespace se3 ...@@ -320,8 +320,7 @@ namespace se3
addJointAndBody(model,JointModelRevoluteUnaligned(joint_axis.normalized()), addJointAndBody(model,JointModelRevoluteUnaligned(joint_axis.normalized()),
parentFrameId,jointPlacement,joint->name, parentFrameId,jointPlacement,joint->name,
Y,link->name, Y,link->name,
max_effort,max_velocity, max_effort,max_velocity);
lower_position, upper_position);
break; break;
} }
default: default:
......
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