Skip to content
Snippets Groups Projects
Verified Commit 13f6ab70 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

parsers: make sample-model header only

parent 048445be
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
+ 0
− 4
View file @ 13f6ab70
......@@ -11,10 +11,6 @@ ENDMACRO(ADD_TARGET_CFLAGS)
# --- C++ --------------------------------------------
# ----------------------------------------------------
SET(${PROJECT_NAME}_PARSERS_SOURCES
parsers/sample-models.cpp
)
IF(LUA5_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
parsers/lua/lua_tables.cpp
......
src/parsers/sample-models.cpp deleted 100644 → 0
+ 0
− 268
View file @ 048445be
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/parsers/sample-models.hpp"
namespace pinocchio
{
namespace buildModels
{
using details::addJointAndBody;
void humanoid2d(Model & model)
{
using details::addJointAndBody;
static const SE3 Id = SE3::Identity();
// root
addJointAndBody(model, JointModelRX(), "universe", "ff1", Id, false);
addJointAndBody(model, JointModelRY(), "ff1_joint", "root", Id, false);
// lleg
addJointAndBody(model, JointModelRZ(), "root_joint", "lleg1", Id, false);
addJointAndBody(model, JointModelRY(), "lleg1_joint", "lleg2", Id, false);
// rlgg
addJointAndBody(model, JointModelRZ(), "root_joint", "rleg1", Id, false);
addJointAndBody(model, JointModelRY(), "lleg1_joint", "rleg2", Id, false);
// torso
addJointAndBody(model, JointModelRY(), "root_joint", "torso1", Id, false);
addJointAndBody(model, JointModelRZ(), "torso1_joint", "chest", Id, false);
// rarm
addJointAndBody(model, JointModelRX(), "chest_joint", "rarm1", Id, false);
addJointAndBody(model, JointModelRZ(), "rarm1_joint", "rarm2", Id, false);
// larm
addJointAndBody(model, JointModelRX(), "root_joint", "larm1", Id, false);
addJointAndBody(model, JointModelRZ(), "larm1_joint", "larm2", Id, false);
}
static void addManipulator(Model & model,
Model::JointIndex rootJoint = 0,
const SE3 & Mroot = SE3::Identity(),
const std::string & pre = "")
{
typedef JointModelRX::ConfigVector_t CV;
typedef 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);
}
#ifdef PINOCCHIO_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.
* <pre> is the prefix (string) before every name in the model.
*/
static void addManipulatorGeometries(const Model & model,
GeometryModel & geom,
const std::string & pre = "")
{
FrameIndex parentFrame;
parentFrame = model.getBodyId(pre+"shoulder1_body");
GeometryObject shoulderBall(pre+"shoulder_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(shoulderBall);
parentFrame = model.getBodyId(pre+"elbow_body");
GeometryObject elbowBall(pre+"elbow_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(elbowBall);
parentFrame = model.getBodyId(pre+"wrist1_body");
GeometryObject wristBall(pre+"wrist_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(wristBall);
parentFrame = model.getBodyId(pre+"upperarm_body");
GeometryObject upperArm(pre+"upperarm_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(upperArm);
parentFrame = model.getBodyId(pre+"lowerarm_body");
GeometryObject lowerArm(pre+"lowerarm_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(lowerArm);
parentFrame = model.getBodyId(pre+"effector_body");
GeometryObject effectorArm(pre+"effector_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .2)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.1)) );
geom.addGeometryObject(effectorArm);
}
#endif
void manipulator(Model & model) { addManipulator(model); }
#ifdef PINOCCHIO_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(); }
void humanoid(Model & model, bool usingFF)
{
using namespace Eigen;
typedef JointModelRX::ConfigVector_t CV;
typedef 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");
model.lowerPositionLimit.segment<4>(3).fill(-1.);
model.upperPositionLimit.segment<4>(3).fill( 1.);
}
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");
}
#ifdef PINOCCHIO_WITH_HPP_FCL
void humanoidGeometries(const Model & model, GeometryModel & geom)
{
addManipulatorGeometries(model,geom,"rleg");
addManipulatorGeometries(model,geom,"lleg");
addManipulatorGeometries(model,geom,"rarm");
addManipulatorGeometries(model,geom,"larm");
FrameIndex parentFrame;
parentFrame = model.getBodyId("chest1_body");
GeometryObject chestBall("chest_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(chestBall);
parentFrame = model.getBodyId("head2_body");
GeometryObject headBall("head_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.25)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(headBall);
parentFrame = model.getBodyId("chest2_body");
GeometryObject chestArm("chest2_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(chestArm);
}
#endif
} // namespace buildModels
} // namespace pinocchio
src/parsers/sample-models.hpp
+ 18
− 7
View file @ 13f6ab70
......@@ -17,7 +17,9 @@ namespace pinocchio
*
* \param model: model, typically given empty, where the kinematic chain is added.
*/
void manipulator(Model & model);
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void manipulator(ModelTpl<Scalar,Options,JointCollectionTpl> & model);
#ifdef PINOCCHIO_WITH_HPP_FCL
/** \brief Create the geometries on top of the kinematic model created by manipulator function.
......@@ -26,7 +28,10 @@ namespace pinocchio
* \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);
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void manipulatorGeometries(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel & geom);
#endif
/** \brief Create a 28-DOF kinematic chain of a floating humanoid robot.
......@@ -41,7 +46,10 @@ namespace pinocchio
* \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);
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void humanoid(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
bool usingFF=true);
#ifdef PINOCCHIO_WITH_HPP_FCL
/** \brief Create the geometries on top of the kinematic model created by humanoid function.
......@@ -50,7 +58,10 @@ namespace pinocchio
* \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);
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void humanoidGeometries(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel & geom);
#endif
/** \brief Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
......@@ -71,14 +82,14 @@ namespace pinocchio
* \ deprecated This function has been replaced by the non-random pinocchio::humanoid function.
*/
PINOCCHIO_DEPRECATED
void humanoid2d(Model & model);
inline void humanoid2d(Model & model);
/** \brief Alias of humanoidRandom, for compatibility reasons.
* \deprecated use pinocchio::humanoid or pinocchio::humanoidRandom instead.
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void humanoidSimple(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool usingFF = true)
PINOCCHIO_DEPRECATED;
PINOCCHIO_DEPRECATED
inline void humanoidSimple(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool usingFF = true);
} // namespace buildModels
} // namespace pinocchio
......
src/parsers/sample-models.hxx
+ 331
− 7
View file @ 13f6ab70
......@@ -19,7 +19,7 @@ namespace pinocchio
const JointModelBase<JointModel> & joint,
const std::string & parent_name,
const std::string & name,
const SE3 & placement = SE3::Random(),
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & placement = ModelTpl<Scalar,Options,JointCollectionTpl>::SE3::Random(),
bool setRandomLimits = true)
{
typedef typename JointModel::ConfigVector_t CV;
......@@ -44,21 +44,212 @@ namespace pinocchio
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
model.addBodyFrame(name + "_body", idx);
}
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
static void addManipulator(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex rootJoint = 0,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & Mroot
= ModelTpl<Scalar,Options,JointCollectionTpl>::SE3::Identity(),
const std::string & pre = "")
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef typename Model::SE3 SE3;
typedef typename Model::Inertia Inertia;
typedef JointCollectionTpl<Scalar,Options> JC;
typedef typename JC::JointModelRX::ConfigVector_t CV;
typedef typename JC::JointModelRX::TangentVector_t TV;
JointIndex idx = rootJoint;
SE3 Marm(SE3::Matrix3::Identity(),SE3::Vector3::UnitZ());
SE3 I4 = SE3::Identity();
Inertia Ijoint(.1,Inertia::Vector3::Zero(),Inertia::Matrix3::Identity()*.01);
Inertia Iarm(1.,typename Inertia::Vector3(0,0,.5),Inertia::Matrix3::Identity());
CV qmin = CV::Constant(-3.14), qmax = CV::Constant(3.14);
TV vmax = TV::Constant(-10), taumax = TV::Constant(10);
idx = model.addJoint(idx,typename JC::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,typename JC::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,typename JC::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,typename JC::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,typename JC::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,typename JC::JointModelRY(),I4,
pre+"wrist2_joint",vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Iarm);
model.addJointFrame(idx);
model.addBodyFrame(pre+"effector_body",idx);
}
#ifdef PINOCCHIO_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.
* <pre> is the prefix (string) before every name in the model.
*/
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
static void addManipulatorGeometries(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel & geom,
const std::string & pre = "")
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::FrameIndex FrameIndex;
typedef typename Model::SE3 SE3;
FrameIndex parentFrame;
parentFrame = model.getBodyId(pre+"shoulder1_body");
GeometryObject shoulderBall(pre+"shoulder_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(shoulderBall);
parentFrame = model.getBodyId(pre+"elbow_body");
GeometryObject elbowBall(pre+"elbow_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(elbowBall);
parentFrame = model.getBodyId(pre+"wrist1_body");
GeometryObject wristBall(pre+"wrist_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(wristBall);
parentFrame = model.getBodyId(pre+"upperarm_body");
GeometryObject upperArm(pre+"upperarm_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0,0,0.5)) );
geom.addGeometryObject(upperArm);
parentFrame = model.getBodyId(pre+"lowerarm_body");
GeometryObject lowerArm(pre+"lowerarm_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0,0,0.5)) );
geom.addGeometryObject(lowerArm);
parentFrame = model.getBodyId(pre+"effector_body");
GeometryObject effectorArm(pre+"effector_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .2)),
SE3(SE3::Matrix3::Identity(), typename SE3::Vector3(0,0,0.1)) );
geom.addGeometryObject(effectorArm);
}
#endif
template<typename Vector3Like>
static typename Eigen::AngleAxis<typename Vector3Like::Scalar>::Matrix3
rotate(const typename Vector3Like::Scalar angle,
const Eigen::MatrixBase<Vector3Like> & axis)
{
typedef typename Vector3Like::Scalar Scalar;
typedef Eigen::AngleAxis<Scalar> AngleAxis;
return AngleAxis(angle,axis).toRotationMatrix();
}
} // namespace details
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void manipulator(ModelTpl<Scalar,Options,JointCollectionTpl> & model)
{
details::addManipulator(model);
}
#ifdef PINOCCHIO_WITH_HPP_FCL
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void manipulatorGeometries(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel & geom)
{ details::addManipulatorGeometries(model,geom); }
#endif
// Deprecated
inline void humanoid2d(Model & model)
{
using details::addJointAndBody;
static const Model::SE3 Id = Model::SE3::Identity();
// root
addJointAndBody(model, JointModelRX(), "universe", "ff1", Id, false);
addJointAndBody(model, JointModelRY(), "ff1_joint", "root", Id, false);
// lleg
addJointAndBody(model, JointModelRZ(), "root_joint", "lleg1", Id, false);
addJointAndBody(model, JointModelRY(), "lleg1_joint", "lleg2", Id, false);
// rlgg
addJointAndBody(model, JointModelRZ(), "root_joint", "rleg1", Id, false);
addJointAndBody(model, JointModelRY(), "lleg1_joint", "rleg2", Id, false);
// torso
addJointAndBody(model, JointModelRY(), "root_joint", "torso1", Id, false);
addJointAndBody(model, JointModelRZ(), "torso1_joint", "chest", Id, false);
// rarm
addJointAndBody(model, JointModelRX(), "chest_joint", "rarm1", Id, false);
addJointAndBody(model, JointModelRZ(), "rarm1_joint", "rarm2", Id, false);
// larm
addJointAndBody(model, JointModelRX(), "root_joint", "larm1", Id, false);
addJointAndBody(model, JointModelRZ(), "larm1_joint", "larm2", Id, false);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void humanoidSimple(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool usingFF)
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
inline void humanoidSimple(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
bool usingFF)
{ humanoidRandom(model,usingFF); }
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void humanoidRandom(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool usingFF)
{
typedef JointCollectionTpl<Scalar, Options> JC;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::SE3 SE3;
using details::addJointAndBody;
static const SE3 Id = SE3::Identity();
typedef JointCollectionTpl<Scalar, Options> JC;
// root
if(! usingFF )
if(! usingFF)
{
typename JC::JointModelComposite jff((typename JC::JointModelTranslation()));
jff.addJoint(typename JC::JointModelSphericalZYX());
......@@ -66,7 +257,8 @@ namespace pinocchio
}
else
{
addJointAndBody(model, typename JC::JointModelFreeFlyer(), "universe", "root", Id);
addJointAndBody(model, typename JC::JointModelFreeFlyer(),
"universe", "root", Id);
model.lowerPositionLimit.template segment<4>(3).fill(-1.);
model.upperPositionLimit.template segment<4>(3).fill( 1.);
}
......@@ -108,6 +300,138 @@ namespace pinocchio
addJointAndBody(model,typename JC::JointModelRX(),"larm5_joint","larm6");
}
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void humanoid(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
bool usingFF)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef JointCollectionTpl<Scalar,Options> JC;
typedef typename Model::SE3 SE3;
typedef typename Model::Inertia Inertia;
typedef typename JC::JointModelRX::ConfigVector_t CV;
typedef typename JC::JointModelRX::TangentVector_t TV;
typename Model::JointIndex idx,chest,ffidx;
SE3 Marm(SE3::Matrix3::Identity(),SE3::Vector3::UnitZ());
SE3 I4 = SE3::Identity();
Inertia Ijoint(.1,Inertia::Vector3::Zero(),Inertia::Matrix3::Identity()*.01);
Inertia Iarm(1.,typename Inertia::Vector3(0,0,.5),Inertia::Matrix3::Identity());
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,typename JC::JointModelFreeFlyer(),
SE3::Identity(),"freeflyer_joint");
model.lowerPositionLimit.template segment<4>(3).fill(-1.);
model.upperPositionLimit.template segment<4>(3).fill( 1.);
}
else
{
typename JC::JointModelComposite jff((typename JC::JointModelTranslation()));
jff.addJoint(typename JC::JointModelSphericalZYX());
ffidx = model.addJoint(0,jff,SE3::Identity(),"freeflyer_joint");
}
model.addJointFrame(ffidx);
/* --- Lower limbs --- */
details::addManipulator(model,ffidx,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
typename SE3::Vector3(0,-0.2,-.1)),"rleg");
details::addManipulator(model,ffidx,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
typename SE3::Vector3(0, 0.2,-.1)),"lleg");
model.jointPlacements[7 ].rotation()
= details::rotate(M_PI/2,SE3::Vector3::UnitY()); // rotate right foot
model.jointPlacements[13].rotation()
= details::rotate(M_PI/2,SE3::Vector3::UnitY()); // rotate left foot
/* --- Chest --- */
idx = model.addJoint(ffidx,typename JC::JointModelRX(),I4 ,"chest1_joint",
vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Ijoint);
model.addJointFrame(idx);
model.addBodyFrame("chest1_body",idx);
idx = model.addJoint(idx,typename JC::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,typename JC::JointModelRX(),
SE3(SE3::Matrix3::Identity(),SE3::Vector3::UnitZ()),
"head1_joint", vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Ijoint);
model.addJointFrame(idx);
model.addBodyFrame("head1_body",idx);
idx = model.addJoint(idx,typename JC::JointModelRY(),I4 ,"head2_joint",
vmax,taumax,qmin,qmax);
model.appendBodyToJoint(idx,Iarm);
model.addJointFrame(idx);
model.addBodyFrame("head2_body",idx);
/* --- Upper Limbs --- */
details::addManipulator(model,chest,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
typename SE3::Vector3(0,-0.3, 1.)),"rarm");
details::addManipulator(model,chest,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
typename SE3::Vector3(0, 0.3, 1.)),"larm");
}
#ifdef PINOCCHIO_WITH_HPP_FCL
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl>
void humanoidGeometries(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel & geom)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::FrameIndex FrameIndex;
typedef typename Model::SE3 SE3;
details::addManipulatorGeometries(model,geom,"rleg");
details::addManipulatorGeometries(model,geom,"lleg");
details::addManipulatorGeometries(model,geom,"rarm");
details::addManipulatorGeometries(model,geom,"larm");
FrameIndex parentFrame;
parentFrame = model.getBodyId("chest1_body");
GeometryObject chestBall("chest_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.05)),
SE3::Identity());
geom.addGeometryObject(chestBall);
parentFrame = model.getBodyId("head2_body");
GeometryObject headBall("head_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Sphere>(new fcl::Sphere(0.25)),
SE3(SE3::Matrix3::Identity(),
typename SE3::Vector3(0,0,0.5)));
geom.addGeometryObject(headBall);
parentFrame = model.getBodyId("chest2_body");
GeometryObject chestArm("chest2_object",
parentFrame, model.frames[parentFrame].parent,
boost::shared_ptr<fcl::Capsule>(new fcl::Capsule(0.05, .8)),
SE3(SE3::Matrix3::Identity(),
typename SE3::Vector3(0,0,0.5)));
geom.addGeometryObject(chestArm);
}
#endif
} // namespace buildModels
......
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