Skip to content
Snippets Groups Projects
Commit 66c4b95c authored by Valenza Florian's avatar Valenza Florian
Browse files

[Major][C++][Python]Breaking retrocompatibilty. Added the possibility to... 

[Major][C++][Python]Breaking retrocompatibilty. Added the possibility to choose root joint when creating a model (in C++) or a robot (in Python). May be a check can be done on the value of root parameter in python if it's not None
parent 9ebc09bf
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
benchmark/timings.cpp
+ 1
− 1
View file @ 66c4b95c
......@@ -43,7 +43,7 @@ int main(int argc, const char ** argv)
else if( filename == "H2" )
se3::buildModels::humanoid2d(model);
else
model = se3::urdf::buildModel(filename,true);
model = se3::urdf::buildModel(filename,JointModelFreeFlyer());
std::cout << "nq = " << model.nq << std::endl;
se3::Data data(model);
......
src/multibody/parser/urdf.hpp
+ 190
− 181
View file @ 66c4b95c
......@@ -73,196 +73,205 @@ namespace se3
return AXIS_UNALIGNED;
}
void parseTree( ::urdf::LinkConstPtr link, Model & model, bool freeFlyer, SE3 placementOffset = SE3::Identity() )
void parseTree( ::urdf::LinkConstPtr link, Model & model,SE3 placementOffset = SE3::Identity())
{
::urdf::JointConstPtr joint = link->parent_joint;
SE3 nextPlacementOffset = SE3::Identity(); // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the length of its attached body to next 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();
// std::cout << "placementOffset: " << placementOffset << std::endl;
bool visual = (link->visual) ? true : false;
if(joint!=NULL)
{
assert(link->getParent()!=NULL);
Model::Index parent = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
model.getJointId( link->getParent()->parent_joint->name );
//std::cout << joint->name << " === " << parent << std::endl;
const SE3 & jointPlacement = placementOffset*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
{
Eigen::VectorXd maxEffort;
Eigen::VectorXd velocity;
Eigen::VectorXd lowerPosition;
Eigen::VectorXd upperPosition;
if (joint->limits)
{
maxEffort.resize(1); maxEffort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper;
}
Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelRX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Y:
model.addBody( parent, JointModelRY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Z:
model.addBody( parent, JointModelRZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_UNALIGNED:
jointAxis= Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
jointAxis.normalize();
model.addBody( parent, JointModelRevoluteUnaligned(jointAxis),
jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
default:
assert( false && "Fatal Error while extracting revolute joint axis");
break;
}
break;
}
case ::urdf::Joint::PRISMATIC:
{
Eigen::VectorXd maxEffort = Eigen::VectorXd(0.);
Eigen::VectorXd velocity = Eigen::VectorXd(0.);
Eigen::VectorXd lowerPosition = Eigen::VectorXd(0.);
Eigen::VectorXd upperPosition = Eigen::VectorXd(0.);
if (joint->limits)
{
maxEffort.resize(1); maxEffort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper;
}
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelPX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Y:
model.addBody( parent, JointModelPY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Z:
model.addBody( parent, JointModelPZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_UNALIGNED:
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;
default:
assert( false && "Fatal Error while extracting prismatic joint axis");
break;
}
break;
}
case ::urdf::Joint::FIXED:
{
// In case of fixed join:
// -add the inertia of the link to his parent in the model
// -let all the children become children of parent
// -inform the parser of the offset to apply
// -add fixed body in model to display it in gepetto-viewer
model.mergeFixedBody(parent, jointPlacement, Y); //Modify the parent inertia in the model
SE3 ptjot_se3 = convertFromUrdf(link->parent_joint->parent_to_joint_origin_transform);
//transformation of the current placement offset
nextPlacementOffset = placementOffset*ptjot_se3;
//add the fixed Body in the model for the viewer
model.addFixedBody(parent,nextPlacementOffset,link->name,visual);
BOOST_FOREACH(::urdf::LinkPtr child_link,link->child_links)
{
child_link->setParent(link->getParent() ); //skip the fixed generation
}
break;
}
default:
{
std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
assert(false && "Only revolute, prismatic and fixed joints are accepted." );
break;
}
}
}
::urdf::JointConstPtr joint = link->parent_joint;
SE3 nextPlacementOffset = SE3::Identity(); // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the length of its attached body to next joint.
// std::cout << " *** " << link->name << " < attached by joint ";
// if(joint)
// std::cout << "#" << link->parent_joint->name << std::endl;
// else std::cout << "###ROOT" << std::endl;
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
parseTree(child, model, nextPlacementOffset);
}
}
//assert(link->inertial && "The parser cannot accept trivial mass");
template <typename D>
void parseTree( ::urdf::LinkConstPtr link, Model & model,SE3 placementOffset , const JointModelBase<D> & root_joint )
{
const Inertia & Y = (link->inertial) ?
convertFromUrdf(*link->inertial)
: Inertia::Identity();
// std::cout << "placementOffset: " << placementOffset << std::endl;
bool visual = (link->visual) ? true : false;
if(joint!=NULL)
{
assert(link->getParent()!=NULL);
Model::Index parent
= (link->getParent()->parent_joint==NULL) ?
(freeFlyer ? 1 : 0)
: model.getJointId( link->getParent()->parent_joint->name );
//std::cout << joint->name << " === " << parent << std::endl;
const SE3 & jointPlacement = placementOffset*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
{
// const double maxEffort = (joint->limits) ?
// (joint->limits->effort)
// : std::numeric_limits<double>::infinity();
// const double lowerPosition = (joint->limits) ?
// (joint->limits->lower != 0. ?
// joint->limits->lower
// : -std::numeric_limits<double>::infinity()
// )
// : -std::numeric_limits<double>::infinity();
Eigen::VectorXd maxEffort;
Eigen::VectorXd velocity;
Eigen::VectorXd lowerPosition;
Eigen::VectorXd upperPosition;
if (joint->limits)
{
maxEffort.resize(1); maxEffort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper;
}
Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelRX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Y:
model.addBody( parent, JointModelRY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Z:
model.addBody( parent, JointModelRZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_UNALIGNED:
jointAxis= Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
jointAxis.normalize();
model.addBody( parent, JointModelRevoluteUnaligned(jointAxis),
jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
default:
assert( false && "Fatal Error while extracting revolute joint axis");
break;
}
break;
}
case ::urdf::Joint::PRISMATIC:
{
Eigen::VectorXd maxEffort = Eigen::VectorXd(0.);
Eigen::VectorXd velocity = Eigen::VectorXd(0.);
Eigen::VectorXd lowerPosition = Eigen::VectorXd(0.);
Eigen::VectorXd upperPosition = Eigen::VectorXd(0.);
if (joint->limits)
{
maxEffort.resize(1); maxEffort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper;
}
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelPX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Y:
model.addBody( parent, JointModelPY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Z:
model.addBody( parent, JointModelPZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_UNALIGNED:
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;
default:
assert( false && "Fatal Error while extracting prismatic joint axis");
break;
}
break;
}
case ::urdf::Joint::FIXED:
{
// In case of fixed join:
// -add the inertia of the link to his parent in the model
// -let all the children become children of parent
// -inform the parser of the offset to apply
// -add fixed body in model to display it in gepetto-viewer
model.mergeFixedBody(parent, jointPlacement, Y); //Modify the parent inertia in the model
SE3 ptjot_se3 = convertFromUrdf(link->parent_joint->parent_to_joint_origin_transform);
//transformation of the current placement offset
nextPlacementOffset = placementOffset*ptjot_se3;
//add the fixed Body in the model for the viewer
model.addFixedBody(parent,nextPlacementOffset,link->name,visual);
BOOST_FOREACH(::urdf::LinkPtr child_link,link->child_links)
{
child_link->setParent(link->getParent() ); //skip the fixed generation
}
break;
}
default:
{
std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
assert(false && "Only revolute, prismatic and fixed joints are accepted." );
break;
}
}
}
else if(freeFlyer)
{ // The link is the root of the body.
model.addBody( 0, JointModelFreeFlyer(), SE3::Identity(), Y, "root", link->name, true );
}
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
parseTree( child,model,freeFlyer,nextPlacementOffset );
}
}
Model buildModel( const std::string & filename, bool freeFlyer = false )
convertFromUrdf(*link->inertial)
: Inertia::Identity();
model.addBody( 0, root_joint, placementOffset, Y , "root_joint", link->name, true );
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
parseTree(child, model, SE3::Identity());
}
}
template <typename D>
Model buildModel( const std::string & filename, const JointModelBase<D> & root_joint )
{
Model model;
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
parseTree(urdfTree->getRoot(), model, SE3::Identity(), root_joint);
return model;
}
Model buildModel( const std::string & filename)
{
Model model;
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
parseTree(urdfTree->getRoot(),model,freeFlyer);
parseTree(urdfTree->getRoot(), model, SE3::Identity());
return model;
}
......
src/python/parsers.hpp
+ 32
− 7
View file @ 66c4b95c
......@@ -38,13 +38,34 @@ namespace se3
{
struct ParsersPythonVisitor
{
#ifdef WITH_URDFDOM
static ModelHandler buildModelFromUrdf( const std::string & filename,
bool ff )
struct build_model_visitor : public boost::static_visitor<ModelHandler>
{
const std::string& _filename;
build_model_visitor(const std::string& filename): _filename(filename){}
template <typename T> ModelHandler operator()( T & operand ) const
{
Model * model = new Model();
*model = se3::urdf::buildModel(_filename, operand);
return ModelHandler(model,true);
}
};
static ModelHandler buildModelFromUrdfWithRoot( const std::string & filename,
bp::object o
)
{
JointModelVariant variant = bp::extract<JointModelVariant> (o);
return boost::apply_visitor(build_model_visitor(filename), variant);
}
static ModelHandler buildModelFromUrdf( const std::string & filename)
{
Model * model = new Model();
*model = se3::urdf::buildModel(filename,ff);
*model = se3::urdf::buildModel(filename);
return ModelHandler(model,true);
}
#endif
......@@ -64,11 +85,15 @@ namespace se3
/* --- Expose --------------------------------------------------------- */
static void expose()
{
bp::def("buildModelFromUrdfWithRoot",buildModelFromUrdfWithRoot,
bp::args("Filename (string)",
"Root Joint Model"),
"Parse the urdf file given in input and return a proper pinocchio model "
"(remember to create the corresponding data structure).");
#ifdef WITH_URDFDOM
bp::def("buildModelFromUrdf",buildModelFromUrdf,
bp::args("Filename (string)",
"Free flyer (bool, false for a fixed robot)"),
bp::args("Filename (string)"),
"Parse the urdf file given in input and return a proper pinocchio model "
"(remember to create the corresponding data structure).");
#endif
......
src/python/robot_wrapper.py
+ 7
− 2
View file @ 66c4b95c
......@@ -21,9 +21,12 @@ from explog import exp
import time
class RobotWrapper:
def __init__(self,filename):
def __init__(self,filename, root_joint = None):
self.modelFileName = filename
self.model = se3.buildModelFromUrdf(filename,True)
if(root_joint is None):
self.model = se3.buildModelFromUrdf(filename)
else:
self.model = se3.buildModelFromUrdfWithRoot(filename, root_joint)
self.data = self.model.createData()
self.v0 = utils.zero(self.nv)
self.q0 = utils.zero(self.nq)
......@@ -169,4 +172,6 @@ class RobotWrapper:
if elapsed_time < dt:
time.sleep(dt - elapsed_time)
__all__ = [ 'RobotWrapper' ]
src/python/romeo_wrapper.py
+ 2
− 2
View file @ 66c4b95c
......@@ -19,8 +19,8 @@ import numpy as np
class RomeoWrapper(RobotWrapper):
def __init__(self,filename):
RobotWrapper.__init__(self,filename)
def __init__(self, filename, root_joint = None):
RobotWrapper.__init__(self, filename, root_joint)
self.q0 = np.matrix( [
0, 0, 0.840252, 0, 0, 0, 1, # Free flyer
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # left leg
......
unittest/value.cpp
+ 5
− 5
View file @ 66c4b95c
......@@ -48,7 +48,7 @@ BOOST_AUTO_TEST_CASE ( test_000 )
{
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
se3::Model model = se3::urdf::buildModel(filename,true);
se3::Model model = se3::urdf::buildModel(filename,se3::JointModelFreeFlyer());
model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
se3::Data data(model);
......@@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE( test_0V0 )
{
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
se3::Model model = se3::urdf::buildModel(filename,true);
se3::Model model = se3::urdf::buildModel(filename,se3::JointModelFreeFlyer());
model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
se3::Data data(model);
......@@ -98,7 +98,7 @@ BOOST_AUTO_TEST_CASE( test_0VA )
{
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
se3::Model model = se3::urdf::buildModel(filename,true);
se3::Model model = se3::urdf::buildModel(filename,se3::JointModelFreeFlyer());
model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
se3::Data data(model);
......@@ -121,7 +121,7 @@ BOOST_AUTO_TEST_CASE( test_Q00 )
{
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
se3::Model model = se3::urdf::buildModel(filename,true);
se3::Model model = se3::urdf::buildModel(filename,se3::JointModelFreeFlyer());
model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
se3::Data data(model);
......@@ -149,7 +149,7 @@ BOOST_AUTO_TEST_CASE( test_QVA )
{
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
se3::Model model = se3::urdf::buildModel(filename,true);
se3::Model model = se3::urdf::buildModel(filename,se3::JointModelFreeFlyer());
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