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

Forward loop for rne.

parent 2e0cffc4
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/joint.hpp
+ 44
− 71
View file @ 36e57606
......@@ -18,76 +18,42 @@ namespace se3
typedef typename traits<JointData>::Transformation_t Transformation_t;
typedef typename traits<JointData>::Velocity_t Velocity_t;
typedef typename traits<JointData>::Bias_t Bias_t;
typedef typename traits<JointData>::JointMotion_t JointMotion_t;
const Constraint_t & S() { return static_cast<JointData*>(this)->S; }
const Transformation_t & M() { return static_cast<JointData*>(this)->M; };
const Velocity_t & v() { return static_cast<JointData*>(this)->v; };
const Bias_t & c() { return static_cast<JointData*>(this)->c; };
JointData& derived() { return *static_cast<JointData*>(this); }
const JointData& derived() const { return *static_cast<JointData*>(this); }
const Constraint_t & S() { return static_cast<JointData*>(this)->S; }
const Transformation_t & M() { return static_cast<JointData*>(this)->M; }
const Velocity_t & v() { return static_cast<JointData*>(this)->v; }
const Bias_t & c() { return static_cast<JointData*>(this)->c; }
const JointMotion_t & qdd() { return static_cast<JointData*>(this)->qdd; }
};
template<typename Joint>
struct JointBase
struct JointModelBase
{
typedef typename traits<Joint>::JointData JointData;
typedef typename traits<Joint>::Placement_t Placement_t;
typedef typename traits<Joint>::Configuration_t Configuration_t;
typedef typename traits<Joint>::Velocity_t Velocity;
JointData createData() { return static_cast<Joint*>(this)->createData(); }
JointData createData() const { return static_cast<Joint*>(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); }
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; }
};
// struct JointRXData;
// template<>
// struct traits<JointRXData>
// {
// typedef Eigen::Matrix<double,6,1> Constraint_t;
// typedef se3::SE3 Transformation_t;
// typedef Eigen::Matrix<double,6,1> Velocity_t;
// typedef BiasZero Bias_t;
// };
// struct JointRXData : public JointDataBase<JointRXData>
// {
// typedef typename traits<JointRXData>::Constraint_t Constraint_t;
// typedef typename traits<JointRXData>::Transformation_t Transformation_t;
// typedef typename traits<JointRXData>::Velocity_t Velocity_t;
// typedef typename traits<JointRXData>::Bias_t Bias_t;
// Constraint_t S;
// Transformation_t M;
// Velocity_t v;
// Bias_t c;
// JointRXData() { S << 0,0,0,1,0,0; }
// };
// struct JointRX;
// template<>
// struct traits<JointRX>
// {
// typedef JointRXData JointData;
// typedef SE3 Placement_t;
// typedef double Configuration_t;
// typedef double Velocity_t;
// };
// struct JointRX : public JointBase<JointRX>
// {
// typedef traits<JointRX>::JointData JointData;
// JointData createData() const { return JointData(); }
// void calc( JointData& data, Configuration_t q, Configuration_t v )
// {
// }
// };
/* --- REVOLUTE X --------------------------------------------------------- */
/* --- REVOLUTE X --------------------------------------------------------- */
/* --- REVOLUTE X --------------------------------------------------------- */
struct JointDataRX;
struct JointModelRX;
......@@ -106,21 +72,23 @@ namespace se3
typedef Motion Velocity_t;
typedef BiasZero Bias_t;
typedef JointModelRX JointModel;
typedef Eigen::Matrix<double,1,1> JointMotion_t;
};
struct JointDataRX
struct JointDataRX : public JointDataBase<JointDataRX>
{
typedef typename traits<JointDataRX>::Constraint_t Constraint_t;
typedef typename traits<JointDataRX>::Transformation_t Transformation_t;
typedef typename traits<JointDataRX>::Velocity_t Velocity_t;
typedef typename traits<JointDataRX>::Bias_t Bias_t;
typedef typename traits<JointDataRX>::JointMotion_t JointMotion_t;
typedef typename traits<JointDataRX>::JointModel JointModel;
//typedef typename traits<JointDataRX>::;
Constraint_t S;
Transformation_t M;
Velocity_t v;
Bias_t c;
JointMotion_t qdd;
JointDataRX() : M(1)
{
......@@ -135,7 +103,7 @@ namespace se3
typedef SE3 Placement_t;
};
struct JointModelRX
struct JointModelRX : public JointModelBase<JointModelRX>
{
typedef traits<JointModelRX>::JointData JointData;
......@@ -145,28 +113,33 @@ namespace se3
JointModelRX() : idx_q(-1),idx_v(-1) {} // Default constructor for std::vector
JointModelRX( int index_q,int index_v ) : idx_q(index_q),idx_v(index_v) {}
JointModelRX( int index_q,int index_v, const JointModelRX& ) : idx_q(index_q),idx_v(index_v) {}
JointData createData() const { return JointData(); }
void calc( JointData& data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs )
void calc( JointData& data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs,
const Eigen::VectorXd & as ) const
{
const double & q = qs[idx_q];
const double & v = vs[idx_v];
//data.M.rotation( Eigen::AngleAxis<double>(q, Eigen::Vector3d::UnitX()).matrix() );
double ca,sa; sincos(q,&sa,&ca);
data.qdd[0] = as[idx_q];
data.M.rotation(rotationX(q));
data.v.angular(Eigen::Vector3d(v,0,0));
}
static inline Eigen::Matrix3d rotationX(const double & angle)
{
Eigen::Matrix3d R3;
double ca,sa; sincos(angle,&sa,&ca);
R3 <<
1,0,0,
0,ca,sa,
0,-sa,ca;
data.M.rotation(R3);
Eigen::Vector3d v3; v3 << v,0,0;
data.v.angular(v3);
return R3;
}
};
};
} // namespace se3
......
src/multibody/model.hpp
+ 1
− 1
View file @ 36e57606
......@@ -111,7 +111,7 @@ namespace se3
Index idx = nbody ++;
joints .push_back(JointModelRX(nq,nv,j));
joints .push_back(JointModelRX(nq,nv));
inertias .push_back(Y);
parents .push_back(parent);
jointPlacements.push_back(placement);
......
unittest/rnea.cpp
+ 58
− 14
View file @ 36e57606
......@@ -7,6 +7,50 @@
#include "pinocchio/tools/timer.hpp"
template<typename JointModel>
void rneaForwardStep(const se3::Model& model,
se3::Data& data,
const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
int i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
using namespace Eigen;
using namespace se3;
jmodel.calc(jdata.derived(),q,v,a);
const Model::Index & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i]*jdata.M();
if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
else data.oMi[i] = data.liMi[i];
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
data.a[i] = Motion(jdata.S()*jdata.qdd()) + jdata.c() + data.v[i].cross(jdata.v());
if(parent>0) data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
}
template<typename JointModel>
void rneaBackwardStep(const se3::Model& model,
se3::Data& data,
const se3::JointModelBase<JointModel> & jmodel,
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();
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
}
int main()
......@@ -70,24 +114,24 @@ int main()
{
for( int i=1;i<model.nbody;++i )
{
JointModelRX & jmodel = model.joints[i];
JointDataRX & jdata = data.joints[i];
jmodel.calc(jdata,q,v);
VectorBlock<VectorXd> qdd = a.segment(jmodel.idx_v,jmodel.nv);
// JointModelRX & jmodel = model.joints[i];
// JointDataRX & jdata = data.joints[i];
// jmodel.calc(jdata,q,v,a);
const Model::Index & parent = model.parents[i];
const SE3 & liMi = data.liMi[i] = model.jointPlacements[i]*jdata.M;
// const Model::Index & parent = model.parents[i];
// const SE3 & liMi = data.liMi[i] = model.jointPlacements[i]*jdata.M;
if(parent>0) data.oMi[i] = data.oMi[parent]*liMi;
else data.oMi[i] = liMi;
// if(parent>0) data.oMi[i] = data.oMi[parent]*liMi;
// else data.oMi[i] = liMi;
data.v[i] = jdata.v;
if(parent>0) data.v[i] += liMi.actInv(data.v[parent]);
// data.v[i] = jdata.v;
// if(parent>0) data.v[i] += liMi.actInv(data.v[parent]);
data.a[i] = Motion(jdata.S*qdd) + jdata.c + data.v[i].cross(jdata.v);
if(parent>0) data.a[i] += liMi.actInv(data.a[parent]);
// data.a[i] = Motion(jdata.S*jdata.qdd) + jdata.c + data.v[i].cross(jdata.v);
// if(parent>0) data.a[i] += liMi.actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
// data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
rneaForwardStep(model,data,model.joints[i],data.joints[i],i,q,v,a);
}
for( int i=model.nbody-1;i>0;--i )
......@@ -97,7 +141,7 @@ int main()
data.tau.segment(jmodel.idx_v,jmodel.nv) = data.joints[i].S.transpose()*data.f[i].toVector();
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
//rneaBackwardStep(model,data,model.joints[i],data.joints[i],i);
}
}
timer.toc(std::cout,1000);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment