diff --git a/benchmark/timings.cpp b/benchmark/timings.cpp
index 81c108ed579a7fb2006191147781ca0541193c27..4acf2a2b401486f7b763d0050e6554ced3a036f1 100644
--- a/benchmark/timings.cpp
+++ b/benchmark/timings.cpp
@@ -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);
diff --git a/src/multibody/parser/urdf.hpp b/src/multibody/parser/urdf.hpp
index 1288794261b0dbd46473223600655833efd0c012..75b2b0f9bda0062ca45555dd7ff877904fc37c48 100644
--- a/src/multibody/parser/urdf.hpp
+++ b/src/multibody/parser/urdf.hpp
@@ -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;
}
diff --git a/src/python/model.hpp b/src/python/model.hpp
index eb89ed4a2e5632c9c90f06bd773df49a73ba7058..6afb55f1ef50bcbffc6ff29baa3c4fb3d0191d46 100644
--- a/src/python/model.hpp
+++ b/src/python/model.hpp
@@ -24,6 +24,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/python/se3.hpp"
#include "pinocchio/python/eigen_container.hpp"
#include "pinocchio/python/handler.hpp"
@@ -42,6 +43,37 @@ namespace se3
public:
typedef Model::Index Index;
typedef eigenpy::UnalignedEquivalent<Motion>::type Motion_fx;
+ typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+ typedef eigenpy::UnalignedEquivalent<Inertia>::type Inertia_fx;
+
+ struct add_body_visitor : public boost::static_visitor<Model::Index>
+ {
+ ModelHandler & _model;
+ Model::Index & _index;
+ const SE3_fx & _placement;
+ const Inertia_fx & _inertia;
+ const std::string & _jName;
+ const std::string & _bName;
+ bool _visual;
+
+ add_body_visitor( ModelHandler & model,
+ Model::Index & idx, const SE3_fx & placement,
+ const Inertia_fx & Y, const std::string & jointName,
+ const std::string & bodyName, bool visual)
+ : _model(model)
+ , _index(idx)
+ , _placement(placement)
+ , _inertia(Y)
+ , _jName(jointName)
+ , _bName(bodyName)
+ , _visual(visual)
+ {}
+
+ template <typename T> Model::Index operator()( T & operand ) const
+ {
+ return _model->addBody(_index, operand, _placement, _inertia, _jName, _bName, _visual);
+ }
+ };
public:
@@ -75,6 +107,9 @@ namespace se3
.add_property("jointPlacements",
bp::make_function(&ModelPythonVisitor::jointPlacements,
bp::return_internal_reference<>()) )
+ .add_property("joints",
+ bp::make_function(&ModelPythonVisitor::joints,
+ bp::return_internal_reference<>()) )
.add_property("parents",
bp::make_function(&ModelPythonVisitor::parents,
bp::return_internal_reference<>()) )
@@ -88,6 +123,8 @@ namespace se3
bp::make_function(&ModelPythonVisitor::hasVisual,
bp::return_internal_reference<>()) )
+ .def("addBody",&ModelPythonVisitor::addJointToModel)
+
.add_property("nFixBody", &ModelPythonVisitor::nFixBody)
.add_property("fix_lmpMi", bp::make_function(&ModelPythonVisitor::fix_lmpMi, bp::return_internal_reference<>()) )
.add_property("fix_lastMovingParent",bp::make_function(&ModelPythonVisitor::fix_lastMovingParent,bp::return_internal_reference<>()) )
@@ -119,6 +156,18 @@ namespace se3
static std::vector<std::string> & bodyNames ( ModelHandler & m ) { return m->bodyNames; }
static std::vector<bool> & hasVisual ( ModelHandler & m ) { return m->hasVisual; }
+ static Model::Index addJointToModel( ModelHandler & modelPtr,
+ Model::Index idx, bp::object joint,
+ const SE3_fx & placement,
+ const Inertia_fx & Y,
+ const std::string & jointName,
+ const std::string & bodyName,
+ bool visual=false)
+ {
+ JointModelVariant variant = bp::extract<JointModelVariant> (joint);
+ return boost::apply_visitor(add_body_visitor(modelPtr, idx, placement, Y, jointName, bodyName, visual), variant);
+ }
+
static int nFixBody( ModelHandler & m ) { return m->nFixBody; }
static std::vector<SE3> & fix_lmpMi ( ModelHandler & m ) { return m->fix_lmpMi; }
static std::vector<Model::Index> & fix_lastMovingParent( ModelHandler & m ) { return m->fix_lastMovingParent; }
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index b5bbe33de0edba5cbf91b108542dfd7ae0c9f5b5..b408a54b644972cf125600fa2ab4671a01509a1a 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -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
diff --git a/src/python/robot_wrapper.py b/src/python/robot_wrapper.py
index 4e8dde9167cda66ba9638ae0fe91024b43984f98..739daf94c00dc26c1ba9c8bde9731c7fcc97b8ce 100644
--- a/src/python/robot_wrapper.py
+++ b/src/python/robot_wrapper.py
@@ -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' ]
diff --git a/src/python/romeo_wrapper.py b/src/python/romeo_wrapper.py
index 426d18cd9cdc273ed67fa99a7f62b8804d091c67..ce40e5919a5a451c6ac503a4f9d98f9bd957d764 100644
--- a/src/python/romeo_wrapper.py
+++ b/src/python/romeo_wrapper.py
@@ -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
diff --git a/unittest/value.cpp b/unittest/value.cpp
index 24652226f44994ed0244d49a01d8bf6d464d3757..dfa746199e0e94f526f711628526ceb7a1859b3e 100644
--- a/unittest/value.cpp
+++ b/unittest/value.cpp
@@ -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);