Skip to content
Snippets Groups Projects
Commit 52d0b34f authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Solved a minor compile+namespace problem on the urdf parser.

parent 1c738aee
No related branches found
Tags v1.0.2
No related merge requests found
Hide whitespace changes
Inline Side-by-side
benchmark/timings.cpp
+ 3
− 3
View file @ 52d0b34f
......@@ -28,11 +28,11 @@ int main(int argc, const char ** argv)
std::string filename = "../models/simple_humanoid.urdf";
if(argc>1) filename = argv[1];
if( filename == "HS")
se3::buildModels::humanoidSimple(model,true);
se3::urdf::buildModels::humanoidSimple(model,true);
else if( filename == "H2" )
se3::buildModels::humanoid2d(model);
se3::urdf::buildModels::humanoid2d(model);
else
model = se3::buildModel(filename,true);
model = se3::urdf::buildModel(filename,true);
std::cout << "nq = " << model.nq << std::endl;
se3::Data data(model);
......
src/multibody/parser/urdf.hpp
+ 116
− 112
View file @ 52d0b34f
......@@ -20,126 +20,130 @@ namespace urdf
namespace se3
{
Inertia convertFromUrdf( const urdf::Inertial& Y )
namespace urdf
{
const urdf::Vector3 & p = Y.origin.position;
const urdf::Rotation & q = Y.origin.rotation;
const Eigen::Vector3d com(p.x,p.y,p.z);
const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
Inertia convertFromUrdf( const ::urdf::Inertial& Y )
{
const ::urdf::Vector3 & p = Y.origin.position;
const ::urdf::Rotation & q = Y.origin.rotation;
const Eigen::Vector3d com(p.x,p.y,p.z);
const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
, Y.ixy,Y.iyy,Y.iyz
, Y.ixz,Y.iyz,Y.izz;
return Inertia(Y.mass,com,R*I*R.transpose());
}
SE3 convertFromUrdf( const ::urdf::Pose & M )
{
const ::urdf::Vector3 & p = M.position;
const ::urdf::Rotation & q = M.rotation;
return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
}
enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z, AXIS_ERROR };
AxisCartesian extractCartesianAxis( const ::urdf::Vector3 & axis )
{
if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
return AXIS_X;
else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
return AXIS_Y;
else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
return AXIS_Z;
std::cerr << "Axis = (" <<axis.x<<","<<axis.y<<","<<axis.z<<")" << std::endl;
assert( false && "Only cartesian axis are accepted." );
return AXIS_ERROR;
}
void parseTree( ::urdf::LinkConstPtr link, Model & model, bool freeFlyer )
{
::urdf::JointConstPtr joint = link->parent_joint;
// std::cout << " *** " << link->name << " < attached by joint ";
// if(joint)
// std::cout << "#" << link->parent_joint->name << std::endl;
// else std::cout << "###ROOT" << std::endl;
//assert(link->inertial && "The parser cannot accept trivial mass");
const Inertia & Y = (link->inertial) ?
convertFromUrdf(*link->inertial)
: Inertia::Identity();
Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
, Y.ixy,Y.iyy,Y.iyz
, Y.ixz,Y.iyz,Y.izz;
//std::cout << "Inertia: " << Y << std::endl;
return Inertia(Y.mass,com,R*I*R.transpose());
}
if(joint!=NULL)
{
assert(link->getParent()!=NULL);
Model::Index parent
= (link->getParent()->parent_joint==NULL) ?
(freeFlyer ? 1 : 0)
: model.getBodyId( link->getParent()->parent_joint->name );
// std::cout << joint->name << " === " << parent << std::endl;
SE3 convertFromUrdf( const urdf::Pose & M )
{
const urdf::Vector3 & p = M.position;
const urdf::Rotation & q = M.rotation;
return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
}
const SE3 & jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z, AXIS_ERROR };
AxisCartesian extractCartesianAxis( const urdf::Vector3 & axis )
{
if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
return AXIS_X;
else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
return AXIS_Y;
else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
return AXIS_Z;
std::cerr << "Axis = (" <<axis.x<<","<<axis.y<<","<<axis.z<<")" << std::endl;
assert( false && "Only cartesian axis are accepted." );
return AXIS_ERROR;
}
void parseTree( urdf::LinkConstPtr link, Model & model, bool freeFlyer )
{
urdf::JointConstPtr joint = link->parent_joint;
//std::cout << "Parent = " << parent << std::endl;
//std::cout << "Placement = " << (Matrix4)jointPlacement << std::endl;
// std::cout << " *** " << link->name << " < attached by joint ";
// if(joint)
// std::cout << "#" << link->parent_joint->name << std::endl;
// else std::cout << "###ROOT" << std::endl;
//assert(link->inertial && "The parser cannot accept trivial mass");
const Inertia & Y = (link->inertial) ?
convertFromUrdf(*link->inertial)
: Inertia::Identity();
//std::cout << "Inertia: " << Y << std::endl;
if(joint!=NULL)
{
assert(link->getParent()!=NULL);
Model::Index parent
= (link->getParent()->parent_joint==NULL) ?
(freeFlyer ? 1 : 0)
: model.getBodyId( link->getParent()->parent_joint->name );
// std::cout << joint->name << " === " << parent << std::endl;
const SE3 & jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
//std::cout << "Parent = " << parent << std::endl;
//std::cout << "Placement = " << (Matrix4)jointPlacement << std::endl;
switch(joint->type)
{
case urdf::Joint::REVOLUTE:
case urdf::Joint::CONTINUOUS: // Revolute with no joint limits
switch(joint->type)
{
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelRX(), jointPlacement, Y, joint->name );
break;
case AXIS_Y:
model.addBody( parent, JointModelRY(), jointPlacement, Y, joint->name );
break;
case AXIS_Z:
model.addBody( parent, JointModelRZ(), jointPlacement, Y, joint->name );
break;
default:
std::cerr << "Bad axis = (" <<joint->axis.x<<","<<joint->axis.y
<<","<<joint->axis.z<<")" << std::endl;
assert(false && "Only X, Y or Z axis are accepted." );
break;
}
break;
case ::urdf::Joint::REVOLUTE:
case ::urdf::Joint::CONTINUOUS: // Revolute with no joint limits
{
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelRX(), jointPlacement, Y, joint->name );
break;
case AXIS_Y:
model.addBody( parent, JointModelRY(), jointPlacement, Y, joint->name );
break;
case AXIS_Z:
model.addBody( parent, JointModelRZ(), jointPlacement, Y, joint->name );
break;
default:
std::cerr << "Bad axis = (" <<joint->axis.x<<","<<joint->axis.y
<<","<<joint->axis.z<<")" << std::endl;
assert(false && "Only X, Y or Z axis are accepted." );
break;
}
break;
}
default:
{
std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
assert(false && "Only revolute joint are accepted." );
break;
}
}
default:
{
std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
assert(false && "Only revolute joint are accepted." );
break;
}
}
}
else if(freeFlyer)/* (joint==NULL) */
{ /* The link is the root of the body. */
model.addBody( 0, JointModelFreeFlyer(), SE3::Identity(), Y, "root" );
}
BOOST_FOREACH(urdf::LinkConstPtr child,link->child_links)
{
parseTree( child,model,freeFlyer );
}
}
Model buildModel( const std::string & filename, bool freeFlyer = false )
{
Model model;
urdf::ModelInterfacePtr urdfTree = urdf::parseURDFFile (filename);
parseTree(urdfTree->getRoot(),model,freeFlyer);
return model;
}
}
else if(freeFlyer)/* (joint==NULL) */
{ /* The link is the root of the body. */
model.addBody( 0, JointModelFreeFlyer(), SE3::Identity(), Y, "root" );
}
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
parseTree( child,model,freeFlyer );
}
}
Model buildModel( const std::string & filename, bool freeFlyer = false )
{
Model model;
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
parseTree(urdfTree->getRoot(),model,freeFlyer);
return model;
}
} // namespace urdf
} // namespace se3
#endif // ifndef __se3_urdf_hpp__
......
unittest/CMakeLists.txt
+ 1
− 1
View file @ 52d0b34f
......@@ -11,7 +11,7 @@ ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL ./rnea.cpp)
PKG_CONFIG_USE_DEPENDENCY(rnea eigen3)
ADD_EXECUTABLE(crba EXCLUDE_FROM_ALL ./crba.cpp)
PKG_CONFIG_USE_DEPENDENCY(crba eigen3)
PKG_CONFIG_USE_DEPENDENCY(crba eigenpy) # Should be eigen3 as well. Problem of link with ROS.
PKG_CONFIG_USE_DEPENDENCY(crba urdfdom)
ADD_EXECUTABLE(jacobian EXCLUDE_FROM_ALL ./jacobian.cpp)
......
unittest/crba.cpp
+ 1
− 1
View file @ 52d0b34f
......@@ -23,7 +23,7 @@ int main(int argc, const char ** argv)
std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
if(argc>1) filename = argv[1];
se3::buildModels::humanoidSimple(model);
//model = se3::buildModel(filename,argc>1);
//model = se3::urdf::buildModel(filename,argc>1);
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
......
unittest/urdf.cpp
+ 1
− 1
View file @ 52d0b34f
......@@ -8,7 +8,7 @@ int main(int argc, const char**argv)
std::string filename = "/home/nmansard/src/rbdl/rbdl_evaluate_performances/models/simple_humanoid.urdf";
if(argc>1) filename = argv[1];
se3::Model model = se3::buildModel(filename);
se3::Model model = se3::urdf::buildModel(filename);
return 0;
}
unittest/value.cpp
+ 1
− 1
View file @ 52d0b34f
......@@ -11,7 +11,7 @@ int main(int argc, const char**argv)
std::string filename = "/home/nmansard/src/rbdl/rbdl_evaluate_performances/models/simple_humanoid.urdf";
if(argc>1) filename = argv[1];
se3::Model model = se3::buildModel(filename);
se3::Model model = se3::urdf::buildModel(filename);
model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
se3::Data data(model);
......
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