Skip to content
Snippets Groups Projects
Commit 1991ff9f authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[C++] Make humanoidRandom templated

parent 0b69d5bc
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/parsers/sample-models.cpp
+ 4
− 85
View file @ 1991ff9f
......@@ -9,41 +9,13 @@ namespace pinocchio
{
namespace buildModels
{
static const SE3 Id = SE3::Identity();
template<typename JointModel>
static void addJointAndBody(Model & model,
const JointModelBase<JointModel> & joint,
const std::string & parent_name,
const std::string & name,
const SE3 & placement = SE3::Random(),
bool setRandomLimits = true)
{
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
Model::JointIndex idx;
if(setRandomLimits)
idx = model.addJoint(model.getJointId(parent_name),joint,
placement, name + "_joint",
TV::Random(joint.nv(),1) + TV::Constant(joint.nv(),1,1), // effort
TV::Random(joint.nv(),1) + TV::Constant(joint.nv(),1,1), // vel
CV::Random(joint.nq(),1) - CV::Constant(joint.nq(),1,1), // qmin
CV::Random(joint.nq(),1) + CV::Constant(joint.nq(),1,1) // qmax
);
else
idx = model.addJoint(model.getJointId(parent_name),joint,
placement, name + "_joint");
model.addJointFrame(idx);
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
model.addBodyFrame(name + "_body", idx);
}
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);
......@@ -70,59 +42,6 @@ namespace pinocchio
}
void humanoidRandom(Model & model, bool usingFF)
{
// root
if(! usingFF )
{
JointModelComposite jff((JointModelTranslation()));
jff.addJoint(JointModelSphericalZYX());
addJointAndBody(model, jff, "universe", "root", Id);
}
else
{
addJointAndBody(model, JointModelFreeFlyer(), "universe", "root", Id);
model.lowerPositionLimit.segment<4>(3).fill(-1.);
model.upperPositionLimit.segment<4>(3).fill( 1.);
}
// lleg
addJointAndBody(model,JointModelRX(),"root_joint","lleg1");
addJointAndBody(model,JointModelRY(),"lleg1_joint","lleg2");
addJointAndBody(model,JointModelRZ(),"lleg2_joint","lleg3");
addJointAndBody(model,JointModelRY(),"lleg3_joint","lleg4");
addJointAndBody(model,JointModelRY(),"lleg4_joint","lleg5");
addJointAndBody(model,JointModelRX(),"lleg5_joint","lleg6");
// rleg
addJointAndBody(model,JointModelRX(),"root_joint","rleg1");
addJointAndBody(model,JointModelRY(),"rleg1_joint","rleg2");
addJointAndBody(model,JointModelRZ(),"rleg2_joint","rleg3");
addJointAndBody(model,JointModelRY(),"rleg3_joint","rleg4");
addJointAndBody(model,JointModelRY(),"rleg4_joint","rleg5");
addJointAndBody(model,JointModelRX(),"rleg5_joint","rleg6");
// trunc
addJointAndBody(model,JointModelRY(),"root_joint","torso1");
addJointAndBody(model,JointModelRZ(),"torso1_joint","chest");
// rarm
addJointAndBody(model,JointModelRX(),"chest_joint","rarm1");
addJointAndBody(model,JointModelRY(),"rarm1_joint","rarm2");
addJointAndBody(model,JointModelRZ(),"rarm2_joint","rarm3");
addJointAndBody(model,JointModelRY(),"rarm3_joint","rarm4");
addJointAndBody(model,JointModelRY(),"rarm4_joint","rarm5");
addJointAndBody(model,JointModelRX(),"rarm5_joint","rarm6");
// larm
addJointAndBody(model,JointModelRX(),"chest_joint","larm1");
addJointAndBody(model,JointModelRY(),"larm1_joint","larm2");
addJointAndBody(model,JointModelRZ(),"larm2_joint","larm3");
addJointAndBody(model,JointModelRY(),"larm3_joint","larm4");
addJointAndBody(model,JointModelRY(),"larm4_joint","larm5");
addJointAndBody(model,JointModelRX(),"larm5_joint","larm6");
}
static void addManipulator(Model & model,
Model::JointIndex rootJoint = 0,
......
src/parsers/sample-models.hpp
+ 7
− 4
View file @ 1991ff9f
......@@ -64,7 +64,8 @@ namespace pinocchio
* uses a composite joint translation + roll-pitch-yaw.
* This changes the size of the configuration space (33 vs 32).
*/
void humanoidRandom(Model & model, bool usingFF = true);
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void humanoidRandom(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool usingFF = true);
/** \brief Create a random humanoid tree with 2d limbs.
* \ deprecated This function has been replaced by the non-random pinocchio::humanoid function.
......@@ -75,11 +76,13 @@ namespace pinocchio
/** \brief Alias of humanoidRandom, for compatibility reasons.
* \deprecated use pinocchio::humanoid or pinocchio::humanoidRandom instead.
*/
PINOCCHIO_DEPRECATED
inline void humanoidSimple(Model & model, bool usingFF = true)
{ humanoidRandom(model,usingFF); }
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void humanoidSimple(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool usingFF = true)
PINOCCHIO_DEPRECATED;
} // namespace buildModels
} // namespace pinocchio
#include "pinocchio/parsers/sample-models.hxx"
#endif // ifndef __pinocchio_sample_models_hpp__
src/parsers/sample-models.hxx 0 → 100644
+ 116
− 0
View file @ 1991ff9f
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_sample_models_hxx__
#define __pinocchio_sample_models_hxx__
namespace pinocchio
{
namespace buildModels
{
namespace details
{
template<typename Scalar, int Options,
template<typename,int> class JointCollectionTpl,
typename JointModel>
static void addJointAndBody(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const JointModelBase<JointModel> & joint,
const std::string & parent_name,
const std::string & name,
const SE3 & placement = SE3::Random(),
bool setRandomLimits = true)
{
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
JointIndex idx;
if(setRandomLimits)
idx = model.addJoint(model.getJointId(parent_name),joint,
placement, name + "_joint",
TV::Random(joint.nv(),1) + TV::Constant(joint.nv(),1,1), // effort
TV::Random(joint.nv(),1) + TV::Constant(joint.nv(),1,1), // vel
CV::Random(joint.nq(),1) - CV::Constant(joint.nq(),1,1), // qmin
CV::Random(joint.nq(),1) + CV::Constant(joint.nq(),1,1) // qmax
);
else
idx = model.addJoint(model.getJointId(parent_name),joint,
placement, name + "_joint");
model.addJointFrame(idx);
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
model.addBodyFrame(name + "_body", idx);
}
} // namespace details
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>
void humanoidRandom(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool usingFF)
{
using details::addJointAndBody;
static const SE3 Id = SE3::Identity();
typedef JointCollectionTpl<Scalar, Options> JC;
// root
if(! usingFF )
{
typename JC::JointModelComposite jff((typename JC::JointModelTranslation()));
jff.addJoint(typename JC::JointModelSphericalZYX());
addJointAndBody(model, jff, "universe", "root", Id);
}
else
{
addJointAndBody(model, typename JC::JointModelFreeFlyer(), "universe", "root", Id);
model.lowerPositionLimit.template segment<4>(3).fill(-1.);
model.upperPositionLimit.template segment<4>(3).fill( 1.);
}
// lleg
addJointAndBody(model,typename JC::JointModelRX(),"root_joint","lleg1");
addJointAndBody(model,typename JC::JointModelRY(),"lleg1_joint","lleg2");
addJointAndBody(model,typename JC::JointModelRZ(),"lleg2_joint","lleg3");
addJointAndBody(model,typename JC::JointModelRY(),"lleg3_joint","lleg4");
addJointAndBody(model,typename JC::JointModelRY(),"lleg4_joint","lleg5");
addJointAndBody(model,typename JC::JointModelRX(),"lleg5_joint","lleg6");
// rleg
addJointAndBody(model,typename JC::JointModelRX(),"root_joint","rleg1");
addJointAndBody(model,typename JC::JointModelRY(),"rleg1_joint","rleg2");
addJointAndBody(model,typename JC::JointModelRZ(),"rleg2_joint","rleg3");
addJointAndBody(model,typename JC::JointModelRY(),"rleg3_joint","rleg4");
addJointAndBody(model,typename JC::JointModelRY(),"rleg4_joint","rleg5");
addJointAndBody(model,typename JC::JointModelRX(),"rleg5_joint","rleg6");
// trunc
addJointAndBody(model,typename JC::JointModelRY(),"root_joint","torso1");
addJointAndBody(model,typename JC::JointModelRZ(),"torso1_joint","chest");
// rarm
addJointAndBody(model,typename JC::JointModelRX(),"chest_joint","rarm1");
addJointAndBody(model,typename JC::JointModelRY(),"rarm1_joint","rarm2");
addJointAndBody(model,typename JC::JointModelRZ(),"rarm2_joint","rarm3");
addJointAndBody(model,typename JC::JointModelRY(),"rarm3_joint","rarm4");
addJointAndBody(model,typename JC::JointModelRY(),"rarm4_joint","rarm5");
addJointAndBody(model,typename JC::JointModelRX(),"rarm5_joint","rarm6");
// larm
addJointAndBody(model,typename JC::JointModelRX(),"chest_joint","larm1");
addJointAndBody(model,typename JC::JointModelRY(),"larm1_joint","larm2");
addJointAndBody(model,typename JC::JointModelRZ(),"larm2_joint","larm3");
addJointAndBody(model,typename JC::JointModelRY(),"larm3_joint","larm4");
addJointAndBody(model,typename JC::JointModelRY(),"larm4_joint","larm5");
addJointAndBody(model,typename JC::JointModelRX(),"larm5_joint","larm6");
}
} // namespace buildModels
} // namespace pinocchio
#endif // ifndef __pinocchio_sample_models_hxx__
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment