Verified Commit 21857cf3 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

parsers/sample-models: handle the case where FCL is missing

parent f2faa5ff
......@@ -21,7 +21,6 @@
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/bindings/python/multibody/data.hpp"
#include "pinocchio/bindings/python/multibody/geometry-model.hpp"
#include "pinocchio/bindings/python/multibody/geometry-data.hpp"
namespace se3
{
......@@ -51,12 +50,14 @@ namespace se3
return model;
}
#ifdef WITH_HPP_FCL
static GeometryModel buildSampleGeometryModelManipulator(const Model& model)
{
GeometryModel geom;
buildModels::manipulatorGeometries(model,geom);
return geom;
}
#endif
static Model buildSampleModelHumanoid()
{
......@@ -72,12 +73,14 @@ namespace se3
return model;
}
#ifdef WITH_HPP_FCL
static GeometryModel buildSampleGeometryModelHumanoid(const Model& model)
{
GeometryModel geom;
buildModels::humanoidGeometries(model,geom);
return geom;
}
#endif
/* --- Expose --------------------------------------------------------- */
static void expose();
......@@ -101,11 +104,13 @@ namespace se3
"Generate a (hard-coded) model of a simple manipulator."
);
#ifdef WITH_HPP_FCL
bp::def("buildSampleGeometryModelManipulator",
static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelManipulator),
bp::args("Model (model)"),
"Generate a (hard-coded) geometry model of a simple manipulator."
);
#endif
bp::def("buildSampleModelHumanoid",
static_cast <Model (*) ()> (&SampleModelsPythonVisitor::buildSampleModelHumanoid),
......@@ -118,11 +123,13 @@ namespace se3
"Generate a (hard-coded) model of a simple humanoid."
);
#ifdef WITH_HPP_FCL
bp::def("buildSampleGeometryModelHumanoid",
static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelHumanoid),
bp::args("Model (model)"),
"Generate a (hard-coded) geometry model of a simple humanoid."
);
#endif
}
......
......@@ -24,7 +24,6 @@ namespace se3
{
const SE3 Id = SE3::Identity();
template<typename JointModel>
static void addJointAndBody(Model & model,
const JointModelBase<JointModel> & joint,
......@@ -184,6 +183,7 @@ namespace se3
}
#ifdef WITH_HPP_FCL
/* Add a 6DOF manipulator shoulder-elbow-wrist geometries to an existing model.
* <model> is the the kinematic chain, constant.
* <geom> is the geometry model where the new geoms are added.
......@@ -229,11 +229,14 @@ namespace se3
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.1)) );
geom.addGeometryObject(effectorArm,model,true);
}
#endif
void manipulator(Model & model) { addManipulator(model); }
#ifdef WITH_HPP_FCL
void manipulatorGeometries(const Model & model, GeometryModel & geom)
{ addManipulatorGeometries(model,geom); }
#endif
static Eigen::Matrix3d rotate(const double angle, const Eigen::Vector3d & axis)
{ return Eigen::AngleAxisd(angle,axis).toRotationMatrix(); }
......@@ -305,6 +308,7 @@ namespace se3
addManipulator(model,chest,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0, 0.3, 1.)),"larm");
}
#ifdef WITH_HPP_FCL
void humanoidGeometries(const Model & model, GeometryModel & geom)
{
addManipulatorGeometries(model,geom,"rleg");
......@@ -330,6 +334,7 @@ namespace se3
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(chestArm,model,true);
}
#endif
} // namespace buildModels
......
......@@ -32,6 +32,7 @@ namespace se3
*/
void manipulator(Model & model);
#ifdef WITH_HPP_FCL
/** \brief Create the geometries on top of the kinematic model created by manipulator function.
*
* \param model, const, kinematic chain typically produced by the function manipulator(model).
......@@ -39,6 +40,7 @@ namespace se3
* not using after manipulator(model).
*/
void manipulatorGeometries(const Model & model, GeometryModel & geom);
#endif
/** \brief Create a 28-DOF kinematic chain of a floating humanoid robot.
*
......@@ -53,6 +55,8 @@ namespace se3
* uses a composite joint. This changes the size of the configuration space (35 vs 34).
*/
void humanoid(Model & model, bool usingFF=true);
#ifdef WITH_HPP_FCL
/** \brief Create the geometries on top of the kinematic model created by humanoid function.
*
* \param model, const, kinematic chain typically produced by the function humanoid(model).
......@@ -60,6 +64,7 @@ namespace se3
* not using after humanoid(model).
*/
void humanoidGeometries(const Model & model, GeometryModel & geom);
#endif
/** \brief Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
*
......
......@@ -54,9 +54,11 @@ BOOST_AUTO_TEST_CASE ( build_model_sample_manipulator )
BOOST_CHECK(model.nq == 6);
BOOST_CHECK(model.nv == 6);
#ifdef WITH_HPP_FCL
se3::Data data(model);
se3::GeometryModel geom;
se3::buildModels::manipulatorGeometries(model,geom);
#endif
}
BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
......@@ -68,6 +70,7 @@ BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
BOOST_CHECK(model.nq == 35);
BOOST_CHECK(model.nv == 34);
#ifdef WITH_HPP_FCL
se3::GeometryModel geom;
se3::buildModels::humanoidGeometries(model,geom);
se3::GeometryData geomdata(geom);
......@@ -75,6 +78,7 @@ BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
Eigen::VectorXd q = se3::neutral(model);
se3::forwardKinematics(model,data,q);
se3::updateGeometryPlacements(model,data,geom,geomdata,q);
#endif
/* We might want to check here the joint namings, and validate the
* direct geometry with respect to reference values. */
......
Markdown is supported
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