From 13f6ab70cfcdb10dd886729d6988332f13dc3192 Mon Sep 17 00:00:00 2001
From: Justin Carpentier <justin.carpentier@inria.fr>
Date: Thu, 15 Nov 2018 19:27:10 +0100
Subject: [PATCH] parsers: make sample-model header only
---
src/CMakeLists.txt | 4 -
src/parsers/sample-models.cpp | 268 ---------------------------
src/parsers/sample-models.hpp | 25 ++-
src/parsers/sample-models.hxx | 338 +++++++++++++++++++++++++++++++++-
4 files changed, 349 insertions(+), 286 deletions(-)
delete mode 100644 src/parsers/sample-models.cpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4ca812c43..f56b5c57d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -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
diff --git a/src/parsers/sample-models.cpp b/src/parsers/sample-models.cpp
deleted file mode 100644
index bb3e6dfa7..000000000
--- a/src/parsers/sample-models.cpp
+++ /dev/null
@@ -1,268 +0,0 @@
-//
-// 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
diff --git a/src/parsers/sample-models.hpp b/src/parsers/sample-models.hpp
index 6b8c73a84..638712864 100644
--- a/src/parsers/sample-models.hpp
+++ b/src/parsers/sample-models.hpp
@@ -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
diff --git a/src/parsers/sample-models.hxx b/src/parsers/sample-models.hxx
index 2879477bd..d1878fe0a 100644
--- a/src/parsers/sample-models.hxx
+++ b/src/parsers/sample-models.hxx
@@ -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
--
GitLab