Unverified Commit ac47735d authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #554 from nmansard/devel

[parser] Add two sample models 
parents 90241249 dad8db5e
......@@ -70,6 +70,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
multibody/joint/joint-derived.hpp
algorithm/algorithms.hpp
parsers/parsers.hpp
parsers/sample-models.hpp
)
SET(${PROJECT_NAME}_PYTHON_SOURCES
......
......@@ -17,6 +17,7 @@
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/parsers/parsers.hpp"
#include "pinocchio/bindings/python/parsers/sample-models.hpp"
namespace se3
{
......@@ -26,6 +27,7 @@ namespace se3
void exposeParsers()
{
ParsersPythonVisitor::expose();
SampleModelsPythonVisitor::expose();
}
} // namespace python
......
//
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_python_sample_models_hpp__
#define __se3_python_sample_models_hpp__
#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
{
namespace python
{
struct SampleModelsPythonVisitor
{
static Model buildSampleModelHumanoidSimple(bool usingFF)
{
Model model;
buildModels::humanoidSimple(model,usingFF);
return model;
}
static Model buildSampleModelManipulator()
{
Model model;
buildModels::manipulator(model);
return model;
}
static GeometryModel buildSampleGeometryModelManipulator(const Model& model)
{
GeometryModel geom;
buildModels::manipulatorGeometries(model,geom);
return geom;
}
static Model buildSampleModelHumanoid()
{
Model model;
buildModels::humanoid(model);
return model;
}
static GeometryModel buildSampleGeometryModelHumanoid(const Model& model)
{
GeometryModel geom;
buildModels::humanoidGeometries(model,geom);
return geom;
}
/* --- Expose --------------------------------------------------------- */
static void expose();
}; // struct SampleModelsPythonVisitor
inline void SampleModelsPythonVisitor::expose()
{
bp::def("buildSampleModelHumanoidSimple",
static_cast <Model (*) (bool)> (&SampleModelsPythonVisitor::buildSampleModelHumanoidSimple),
bp::args("bool (usingFreeFlyer)"),
"Generate a (hard-coded) model of a simple humanoid robot."
);
bp::def("buildSampleModelManipulator",
static_cast <Model (*) ()> (&SampleModelsPythonVisitor::buildSampleModelManipulator),
"Generate a (hard-coded) model of a simple manipulator."
);
bp::def("buildSampleGeometryModelManipulator",
static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelManipulator),
bp::args("Model (model)"),
"Generate a (hard-coded) geometry model of a simple manipulator."
);
bp::def("buildSampleModelHumanoid",
static_cast <Model (*) ()> (&SampleModelsPythonVisitor::buildSampleModelHumanoid),
"Generate a (hard-coded) model of a simple humanoid."
);
bp::def("buildSampleGeometryModelHumanoid",
static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelHumanoid),
bp::args("Model (model)"),
"Generate a (hard-coded) geometry model of a simple humanoid."
);
}
}
} // namespace se3::python
#endif // ifndef __se3_python_sample_models_hpp__
......@@ -84,7 +84,7 @@ namespace se3
}
void humanoidSimple(Model & model, bool usingFF)
void humanoidRandom(Model & model, bool usingFF)
{
// root
if(! usingFF )
......@@ -137,6 +137,203 @@ namespace se3
}
static void addManipulator(Model & model,
Model::JointIndex rootJoint = 0,
const SE3 & Mroot = SE3::Identity(),
const std::string& pre = "")
{
typedef typename JointModelRX::ConfigVector_t CV;
typedef typename JointModelRX::TangentVector_t TV;
Model::JointIndex idx = rootJoint;
SE3 Marm(Eigen::Matrix3d::Identity(),Eigen::Vector3d(0,0,1));
SE3 I4 = SE3::Identity();
Inertia Ijoint = Inertia(.1,Eigen::Vector3d::Zero(),Eigen::Matrix3d::Identity()*.01);
Inertia Iarm = Inertia(1.,Eigen::Vector3d(0,0,.5),Eigen::Matrix3d::Identity()*.1 );
CV qmin = CV::Constant(-3.14), qmax = CV::Constant(3.14);
TV vmax = TV::Constant(-10), taumax = TV::Constant(10);
idx = model.addJoint(idx,JointModelRX(),Mroot,pre+"shoulder1_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Ijoint);
model.addJointFrame(idx);
model.addBodyFrame(pre+"shoulder1_body",idx);
idx = model.addJoint(idx,JointModelRY(),I4 ,pre+"shoulder2_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Ijoint);
model.addJointFrame(idx);
model.addBodyFrame(pre+"shoulder2_body",idx);
idx = model.addJoint(idx,JointModelRZ(),I4 ,pre+"shoulder3_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Iarm);
model.addJointFrame(idx);
model.addBodyFrame(pre+"upperarm_body",idx);
idx = model.addJoint(idx,JointModelRY(),Marm ,pre+"elbow_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Iarm);
model.addJointFrame(idx);
model.addBodyFrame(pre+"lowerarm_body",idx);
model.addBodyFrame(pre+"elbow_body",idx);
idx = model.addJoint(idx,JointModelRX(),Marm ,pre+"wrist1_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Ijoint);
model.addJointFrame(idx);
model.addBodyFrame(pre+"wrist1_body",idx);
idx = model.addJoint(idx,JointModelRY(),I4 ,pre+"wrist2_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Iarm);
model.addJointFrame(idx);
model.addBodyFrame(pre+"effector_body",idx);
}
/* 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.
* <pre> is the prefix (string) before every name in the model.
*/
static void addManipulatorGeometries(const Model& model,
GeometryModel & geom,
const std::string & pre = "")
{
GeometryObject shoulderBall(pre+"shoulder_object",
model.getBodyId(pre+"shoulder1_body"),/*NR*/0,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(shoulderBall,model,true);
GeometryObject elbowBall(pre+"elbow_object",
model.getBodyId(pre+"elbow_body"),/*NR*/0,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(elbowBall,model,true);
GeometryObject wristBall(pre+"wrist_object",
model.getBodyId(pre+"wrist1_body"),/*NR*/0,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(wristBall,model,true);
GeometryObject upperArm(pre+"upperarm_object",
model.getBodyId(pre+"upperarm_body"),/*NR*/0,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(upperArm,model,true);
GeometryObject lowerArm(pre+"lowerarm_object",
model.getBodyId(pre+"lowerarm_body"),/*NR*/0,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(lowerArm,model,true);
GeometryObject effectorArm(pre+"effector_object",
model.getBodyId(pre+"effector_body"),/*NR*/0,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .2)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.1)) );
geom.addGeometryObject(effectorArm,model,true);
}
void manipulator(Model& model) { addManipulator(model); }
void manipulatorGeometries(const Model& model, GeometryModel & geom)
{ addManipulatorGeometries(model,geom); }
static Eigen::Matrix3d rotate(const double angle, const Eigen::Vector3d & axis)
{ return Eigen::AngleAxisd(angle,axis).toRotationMatrix(); }
void humanoid(Model & model, bool usingFF)
{
using namespace Eigen;
typedef typename JointModelRX::ConfigVector_t CV;
typedef typename JointModelRX::TangentVector_t TV;
Model::JointIndex idx,chest,ffidx;
SE3 Marm(Eigen::Matrix3d::Identity(),Eigen::Vector3d(0,0,1));
SE3 I4 = SE3::Identity();
Inertia Ijoint = Inertia(.1,Eigen::Vector3d::Zero(),Eigen::Matrix3d::Identity()*.01);
Inertia Iarm = Inertia(1.,Eigen::Vector3d(0,0,.5),Eigen::Matrix3d::Identity()*.1 );
CV qmin = CV::Constant(-3.14), qmax = CV::Constant(3.14);
TV vmax = TV::Constant(-10), taumax = TV::Constant(10);
/* --- Free flyer --- */
if(usingFF)
ffidx = model.addJoint(0,JointModelFreeFlyer(),SE3::Identity(),"freeflyer_joint");
else
{
JointModelComposite jff((JointModelTranslation()));
jff.addJoint(JointModelSphericalZYX());
ffidx = model.addJoint(0,jff,SE3::Identity(),"freeflyer_joint");
}
model.addJointFrame(ffidx);
/* --- Lower limbs --- */
AngleAxisd(M_PI,Vector3d(1,0,0)).toRotationMatrix();
addManipulator(model,ffidx,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0,-0.2,-.1)),"rleg");
addManipulator(model,ffidx,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0, 0.2,-.1)),"lleg");
model.jointPlacements[7 ].rotation() = rotate(M_PI/2,Vector3d::UnitY()); // rotate right foot
model.jointPlacements[13].rotation() = rotate(M_PI/2,Vector3d::UnitY()); // rotate left foot
/* --- Chest --- */
idx = model.addJoint(ffidx,JointModelRX(),I4 ,"chest1_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Ijoint);
model.addJointFrame(idx);
model.addBodyFrame("chest1_body",idx);
idx = model.addJoint(idx,JointModelRY(),I4 ,"chest2_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Iarm);
model.addJointFrame(idx);
model.addBodyFrame("chest2_body",idx);
chest = idx;
/* --- Head --- */
idx = model.addJoint(idx,JointModelRX(),
SE3(Matrix3d::Identity(),Vector3d(0.,0.,1.)),
"head1_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Ijoint);
model.addJointFrame(idx);
model.addBodyFrame("head1_body",idx);
idx = model.addJoint(idx,JointModelRY(),I4 ,"head2_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Iarm);
model.addJointFrame(idx);
model.addBodyFrame("head2_body",idx);
/* --- Upper Limbs --- */
addManipulator(model,chest,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0,-0.3, 1.)),"rarm");
addManipulator(model,chest,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0, 0.3, 1.)),"larm");
}
void humanoidGeometries(const Model& model, GeometryModel & geom)
{
addManipulatorGeometries(model,geom,"rleg");
addManipulatorGeometries(model,geom,"lleg");
addManipulatorGeometries(model,geom,"rarm");
addManipulatorGeometries(model,geom,"larm");
GeometryObject chestBall("chest_object",
model.getBodyId("chest1_body"),/*NR*/0,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(chestBall,model,true);
GeometryObject headBall("head_object",
model.getBodyId("head2_body"),/*NR*/0,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.25)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(headBall,model,true);
GeometryObject chestArm("chest2_object",
model.getBodyId("chest2_body"),/*NR*/0,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(chestArm,model,true);
}
} // namespace buildModels
} // namespace se3
......@@ -20,15 +20,69 @@
#define __se3_sample_models_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp"
namespace se3
{
namespace buildModels
{
{
/** \brief Create a 6-DOF kinematic chain shoulder-elbow-wrist.
*
* \param model: model, typically given empty, where the kinematic chain is added.
*/
void manipulator(Model& model);
/** \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).
* \warning this method is expecting specific namings of the kinematic chain, use it with care
* not using after manipulator(model).
*/
void manipulatorGeometries(const Model& model, GeometryModel & geom);
/** \brief Create a 28-DOF kinematic chain of a floating humanoid robot.
*
* The kinematic chain has 4 limbs shoulder-elbow-wrist, one 2-dof torso, one
* 2-dof neck. The float joint is either a free-float joint JointModelFreeFloating
* (with nq=7 and nv=6), or a composite joint with 3 prismatic and 1 roll-pitch-yaw.
* Using a free-floating or a composite joint is decided by the boolean usingFF.
*
* \param model: model, typically given empty, where the kinematic chain is added.
* \param usingFF: if True, implement the chain with a plain JointModelFreeFloating; if False,
* uses a composite joint. This changes the size of the configuration space (35 vs 34).
*/
void humanoid(Model& model,bool usingFF=true);
/** \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).
* \warning this method is expecting specific namings of the kinematic chain, use it with care
* not using after humanoid(model).
*/
void humanoidGeometries(const Model& model, GeometryModel & geom);
/** \brief Create a humanoid kinematic tree with 6D limbs and random joint placement.
*
* This method is only meant to be used in unittest. Due to random placement and masses,
* the resulting model is likely to not correspond to any physically-plausible model.
* You may want to consider se3::humanoid and se3::humanoidGeometries functions that
* rather define a plain and non-random model.
* \param model: model, typically given empty, where the kinematic chain is added.
* \param usingFF: if True, implement the chain with a plain JointModelFreeFloating; if False,
* uses a composite joint. This changes the size of the configuration space (35 vs 34).
*/
void humanoidRandom(Model& model, bool usingFF = true);
/** \brief Create a random humanoid tree with 2d limbs.
* \ deprecated This function has been replaced by the non-random se3::humanoid function.
*/
PINOCCHIO_DEPRECATED
void humanoid2d(Model& model);
void humanoidSimple(Model& model, bool usingFF = true);
/** \brief Alias of humanoidRandom, for compatibility reasons.
* \deprecated use se3::humanoid or se3::humanoidRandom instead.
*/
PINOCCHIO_DEPRECATED
inline void humanoidSimple(Model& model, bool usingFF = true) { humanoidRandom(model,usingFF); }
} // namespace buildModels
} // namespace se3
......
......@@ -59,6 +59,7 @@ ADD_PINOCCHIO_UNIT_TEST(com eigen3)
ADD_PINOCCHIO_UNIT_TEST(jacobian eigen3)
ADD_PINOCCHIO_UNIT_TEST(cholesky eigen3)
ADD_PINOCCHIO_UNIT_TEST(dynamics eigen3)
ADD_PINOCCHIO_UNIT_TEST(sample-models eigen3)
IF(URDFDOM_FOUND)
ADD_PINOCCHIO_UNIT_TEST(urdf "eigen3;urdfdom")
......
//
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include <iostream>
#include <fstream>
#include <streambuf>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include <boost/test/unit_test.hpp>
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid_simple )
{
se3::Model model;
se3::buildModels::humanoidSimple(model,true);
BOOST_CHECK(model.nq == 33);
BOOST_CHECK(model.nv == 32);
se3::Model modelff;
se3::buildModels::humanoidSimple(modelff,false);
BOOST_CHECK(modelff.nq == 32);
BOOST_CHECK(modelff.nv == 32);
}
BOOST_AUTO_TEST_CASE ( build_model_sample_manipulator )
{
se3::Model model;
se3::buildModels::manipulator(model);
BOOST_CHECK(model.nq == 6);
BOOST_CHECK(model.nv == 6);
se3::Data data(model);
se3::GeometryModel geom;
se3::buildModels::manipulatorGeometries(model,geom);
}
BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
{
se3::Model model;
se3::buildModels::humanoid(model);
se3::Data data(model);
BOOST_CHECK(model.nq == 35);
BOOST_CHECK(model.nv == 34);
se3::GeometryModel geom;
se3::buildModels::humanoidGeometries(model,geom);
se3::GeometryData geomdata(geom);
Eigen::VectorXd q = se3::neutral(model);
se3::forwardKinematics(model,data,q);
se3::updateGeometryPlacements(model,data,geom,geomdata,q);
/* We might want to check here the joint namings, and validate the
* direct geometry with respect to reference values. */
}
BOOST_AUTO_TEST_SUITE_END()
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