Skip to content
Snippets Groups Projects
Commit d451a70d authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Working with CRTP.

parent a632858d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/joint.hpp
+ 50
− 9
View file @ d451a70d
......@@ -5,6 +5,8 @@
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/motion.hpp"
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <boost/variant.hpp>
namespace se3
{
......@@ -21,7 +23,7 @@ namespace se3
typedef typename traits<JointData>::JointMotion_t JointMotion_t;
JointData& derived() { return *static_cast<JointData*>(this); }
const JointData& derived() const { return *static_cast<JointData*>(this); }
const JointData& derived() const { return *static_cast<const JointData*>(this); }
const Constraint_t & S() { return static_cast<JointData*>(this)->S; }
const Transformation_t & M() { return static_cast<JointData*>(this)->M; }
......@@ -32,22 +34,30 @@ namespace se3
template<typename Joint>
template<typename JointModel>
struct JointModelBase
{
typedef typename traits<Joint>::JointData JointData;
typedef typename traits<JointModel>::JointData JointData;
JointData createData() const { return static_cast<const Joint*>(this)->createData(); }
JointModel& derived() { return *static_cast<JointModel*>(this); }
const JointModel& derived() const { return *static_cast<const JointModel*>(this); }
JointData createData() const { return static_cast<const JointModel*>(this)->createData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs,
const Eigen::VectorXd & as ) const
{ return static_cast<const Joint*>(this)->calc(data,qs,vs,as); }
{ return static_cast<const JointModel*>(this)->calc(data,qs,vs,as); }
int idx_q() const { return static_cast<const Joint *>(this)->idx_q; }
int idx_v() const { return static_cast<const Joint *>(this)->idx_v; }
int nq() const { return static_cast<const Joint *>(this)->nq; }
int nv() const { return static_cast<const Joint *>(this)->nv; }
int idx_q() const { return static_cast<const JointModel *>(this)->idx_q; }
int idx_v() const { return static_cast<const JointModel *>(this)->idx_v; }
int nq() const { return static_cast<const JointModel *>(this)->nq; }
int nv() const { return static_cast<const JointModel *>(this)->nv; }
void setIndex(int q,int v)
{
derived().idx_q = q;
derived().idx_v = v;
}
};
......@@ -234,4 +244,35 @@ namespace se3
} // namespace se3
namespace se3
{
/* --- VARIANT ------------------------------------------------------------ */
/* --- VARIANT ------------------------------------------------------------ */
/* --- VARIANT ------------------------------------------------------------ */
typedef boost::variant< JointModelRX,JointModelFreeFlyer> JointModelVariant;
typedef boost::variant< JointDataRX,JointDataFreeFlyer> JointDataVariant;
typedef std::vector<JointModelVariant> JointModelVector;
typedef std::vector<JointDataVariant> JointDataVector;
class CreateJointData: public boost::static_visitor<JointDataVariant>
{
public:
template<typename D>
JointDataVariant operator()(const JointModelBase<D> & jmodel) const
{ return JointDataVariant(jmodel.createData()); }
static JointDataVariant run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( CreateJointData(), jmodel ); }
};
} // namespace se3
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModelRX);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointDataRX);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModelVariant);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointDataVariant);
#endif // ifndef __se3_joint_hpp__
src/multibody/model.hpp
+ 18
− 12
View file @ d451a70d
......@@ -8,14 +8,11 @@
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/multibody/joint.hpp"
#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModelRX);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointDataRX);
namespace se3
{
......@@ -33,7 +30,8 @@ namespace se3
std::vector<Inertia> inertias;
std::vector<SE3> jointPlacements;
std::vector<JointModelRX> joints;
JointModelVector joints;
//std::vector<JointModelRX> joints;
std::vector<Index> parents;
std::vector<std::string> names;
......@@ -53,7 +51,8 @@ namespace se3
{
names[0] = "universe";
}
Index addBody( Index parent,const JointModelRX & j,const SE3 & placement,
template<typename D>
Index addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,const std::string & name = "" );
Index getBodyId( const std::string & name ) const;
const std::string& getBodyName( Index index ) const;
......@@ -65,7 +64,8 @@ namespace se3
public:
const Model& model;
std::vector<JointDataRX> joints;
JointDataVector joints;
//std::vector<JointDataRX> joints;
std::vector<Motion> a; // Body acceleration
std::vector<Motion> v; // Body velocity
std::vector<Force> f; // Body force
......@@ -102,23 +102,27 @@ namespace se3
return res;
}
Model::Index Model::addBody( Index parent,const JointModelRX & j,const SE3 & placement,
template<typename D>
Model::Index Model::addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,const std::string & name )
{
assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
&&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
assert( (j.nq>=0)&&(j.nv>=0) );
assert( (j.nq()>=0)&&(j.nv()>=0) );
Index idx = nbody ++;
joints .push_back(JointModelRX(nq,nv));
joints .push_back(j.derived());
boost::get<D&>(joints.back()).setIndex(nq,nv);
//joints.back().setIndex(nq,nv);
inertias .push_back(Y);
parents .push_back(parent);
jointPlacements.push_back(placement);
names .push_back( (name!="")?name:random(8) );
nq += j.nq;
nv += j.nv;
nq += j.nq();
nv += j.nv();
return idx;
}
Model::Index Model::getBodyId( const std::string & name ) const
......@@ -144,7 +148,9 @@ namespace se3
,liMi(ref.nbody)
,tau(ref.nbody)
{
for(int i=0;i<model.nbody;++i) joints.push_back(model.joints[i].createData());
for(int i=0;i<model.nbody;++i)
//joints.push_back(model.joints[i].createData());
joints.push_back(CreateJointData::run(model.joints[i]));
}
......
unittest/rnea.cpp
+ 79
− 4
View file @ d451a70d
......@@ -41,18 +41,86 @@ template<typename JointModel>
void rneaBackwardStep(const se3::Model& model,
se3::Data& data,
const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & /*jdata*/,
se3::JointDataBase<typename JointModel::JointData> & jdata,
int i)
{
using namespace Eigen;
using namespace se3;
const Model::Index & parent = model.parents[i];
data.tau.segment(jmodel.idx_v(),jmodel.nv()) = data.joints[i].S.transpose()*data.f[i].toVector();
data.tau.segment(jmodel.idx_v(),jmodel.nv()) = jdata.S().transpose()*data.f[i].toVector();
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
}
struct RneaForwardStepVisitor : public boost::static_visitor<>
{
const se3::Model& model;
se3::Data& data;
se3::JointDataVariant & jdata;
int i;
const Eigen::VectorXd & q;
const Eigen::VectorXd & v;
const Eigen::VectorXd & a;
RneaForwardStepVisitor( const se3::Model& model,
se3::Data& data,
se3::JointDataVariant & jdata,
int i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
: model(model),data(data),jdata(jdata),i(i),q(q),v(v),a(a) {}
template <typename D>
void operator()(const se3::JointModelBase<D> & jmodel) const
{
rneaForwardStep(model,data,jmodel,boost::get<typename D::JointData&>(jdata),i,q,v,a);
}
static void run( const se3::Model& model,
se3::Data& data,
const se3::JointModelVariant & jmodel,
se3::JointDataVariant & jdata,
int i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{ boost::apply_visitor( RneaForwardStepVisitor(model,data,jdata,i,q,v,a), jmodel ); }
};
struct RneaBackwardStepVisitor : public boost::static_visitor<>
{
const se3::Model& model;
se3::Data& data;
se3::JointDataVariant & jdata;
int i;
RneaBackwardStepVisitor( const se3::Model& model,
se3::Data& data,
se3::JointDataVariant & jdata,
int i)
: model(model),data(data),jdata(jdata),i(i) {}
template <typename D>
void operator()(const se3::JointModelBase<D> & jmodel) const
{
rneaBackwardStep(model,data,jmodel,boost::get<typename D::JointData&>(jdata),i);
}
static void run( const se3::Model& model,
se3::Data& data,
const se3::JointModelVariant & jmodel,
se3::JointDataVariant & jdata,
int i)
{ boost::apply_visitor( RneaBackwardStepVisitor(model,data,jdata,i), jmodel ); }
};
int main()
{
using namespace Eigen;
......@@ -114,12 +182,19 @@ int main()
{
for( int i=1;i<model.nbody;++i )
{
rneaForwardStep(model,data,model.joints[i],data.joints[i],i,q,v,a);
//rneaForwardStep(model,data,model.joints[i],data.joints[i],i,q,v,a);
//rneaForwardStep(model,data,
// boost::get<const se3::JointModelRX&>(model.joints[i]),
// boost::get< se3::JointDataRX&>(data.joints[i]),
// i,q,v,a);
RneaForwardStepVisitor::run(model,data,model.joints[i],data.joints[i],i,q,v,a);
}
for( int i=model.nbody-1;i>0;--i )
{
rneaBackwardStep(model,data,model.joints[i],data.joints[i],i);
//rneaBackwardStep(model,data,model.joints[i],data.joints[i],i);
//RneaBackwardStepVisitor::run(model,data,model.joints[i],data.joints[i],i);
}
}
timer.toc(std::cout,1000);
......
unittest/variant.cpp
+ 6
− 19
View file @ d451a70d
......@@ -7,33 +7,20 @@
#include "pinocchio/tools/timer.hpp"
#include <boost/variant.hpp>
using namespace se3;
typedef boost::variant< JointModelRX,JointModelFreeFlyer> JointModel;
typedef boost::variant< JointDataRX,JointDataFreeFlyer> JointData;
class CreateJointData: public boost::static_visitor<JointData>
{
public:
template<typename D>
JointData operator()(const se3::JointModelBase<D> & jmodel) const
{ return JointData(jmodel.createData()); }
static JointData run( const JointModel & jmodel)
{ return boost::apply_visitor( CreateJointData(), jmodel ); }
};
int main()
{
using namespace Eigen;
using namespace se3;
JointModelVariant jmodel = JointModelRX(0,0);
const JointDataVariant & jdata = CreateJointData::run(jmodel);
JointModel jmodel = JointModelRX(0,0);
const JointData & jdata = CreateJointData::run(jmodel); //x boost::apply_visitor( CreateJointData(), jmodel );
se3::Model model;
......
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