Commit 9ecf12e6 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Fast version of the two-loop crba (10us on the simple h).

parent db58a88d
......@@ -3,7 +3,7 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include <iostream>
namespace se3
{
......@@ -47,6 +47,63 @@ namespace se3
};
namespace internal
{
template<typename D>
static Eigen::Matrix<double,6,D::ColsAtCompileTime>
act( const SE3 & m, const Eigen::MatrixBase<D> & iF )
{
Eigen::Matrix<double,6,D::ColsAtCompileTime> jF(iF.rows(),iF.cols());
typename D::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
typename D::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
jF.template topRows <3>() = m.rotation()*linear;
jF.template bottomRows<3>() = skew(m.translation())*jF.template topRows<3>()
+m.rotation()*angular;
return jF;
}
typedef Eigen::Matrix<double,6,1> Vector6;
static Vector6 act( const SE3 & m, const Eigen::MatrixBase<Vector6> & iF )
{
Vector6 jF;
Eigen::VectorBlock<const Vector6,3> linear = iF.head<3>();
Eigen::VectorBlock<const Vector6,3> angular = iF.tail<3>();
jF.head <3>() = m.rotation()*linear;
jF.tail <3>() = m.translation().cross(jF.head<3>()) + m.rotation()*angular;
return jF;
}
} // namespace internal
template<typename D>
struct CrbaInternalBackwardStep : public fusion::JointVisitor<CrbaInternalBackwardStep<D> >
{
typedef boost::fusion::vector<const Model&,
Data&,
const Eigen::MatrixBase<D>&,
const int &,
const int &> ArgsType;
CrbaInternalBackwardStep( JointDataVariant & jdata,ArgsType args ) : jdata(jdata),args(args) {}
using fusion::JointVisitor< CrbaInternalBackwardStep<D> >::run;
JointDataVariant & jdata;
ArgsType args;
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointData> & jdata,
const Model&,
Data& data,
const Eigen::MatrixBase<D> & F,
const int & idx_v, const int & nv)
{
data.M.block(jmodel.idx_v(),idx_v,JointModel::nv,nv) = jdata.S().transpose()*F;
}
};
struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
{
typedef boost::fusion::vector<const Model&,
......@@ -63,33 +120,51 @@ namespace se3
int i)
{
/*
* F[:,i] = Yi*Si
* M[i,i:subtree] = Si'*F[:,i:substree]
* if li>0:
* Yli += liMi.act(Yi)
* F[:,i:subtree] = liMi.act(F[:,i:subtree])
* F = Y*S
* M[i,i] = S'*F
* for j<i
* F = jXi*F
* M[j,i] = S'*F
* if li>0 Yli += liXi Yi
*/
const Model::Index & parent = model.parents[i];
const int & nsubtree = data.nvSubtree[i];
data.Fcrb.block(jmodel.idx_v(),JointModel::nv) = data.Ycrb[i] * jdata.S();
Model::Index parent = model.parents[i];
data.M.block(jmodel.idx_v(),jmodel.idx_v(),JointModel::nv,nsubtree)
= jdata.S().transpose() * data.Fcrb.block(jmodel.idx_v(),nsubtree);
jdata.F() = data.Ycrb[i] * jdata.S();
data.M.block(jmodel.idx_v(),jmodel.idx_v(),JointModel::nv,JointModel::nv)
= jdata.S().transpose()*jdata.F();
// std::cout << "*** joint " << i << std::endl;
// std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
// std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
// std::cout << "iF = " << data.Fcrb.matrix() << std::endl;
// std::cout << "M = " << data.M << std::endl;
// std::cout << "iFi = " << jdata.F() << std::endl;
if( parent>0 )
if(parent>0)
{
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
data.Fcrb.block(jmodel.idx_v(), nsubtree) =
data.liMi[i].act( data.Fcrb.block(jmodel.idx_v(), nsubtree) );
jdata.F() = internal::act(data.liMi[i],jdata.F());
}
// std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
// std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
// std::cout << "liFi = " << jdata.F() << std::endl;
while(parent>0)
{
CrbaInternalBackwardStep<typename JointModel::F_t>
::run( model.joints[parent],
data.joints[parent],
typename CrbaInternalBackwardStep<typename JointModel::F_t>
::ArgsType(model,data,jdata.F(),jmodel.idx_v(),JointModel::nv) );
jdata.F() = internal::act(data.liMi[parent],jdata.F());
parent = model.parents[parent];
// std::cout << "\tj =" << parent << std::endl;
// std::cout << "\tjFi = " << jdata.F() << std::endl;
// std::cout << "\tM = " << data.M.block(0,jmodel.idx_v(),model.nv,JointModel::nv) << std::endl;
}
// std::cout << "M = " << data.M << std::endl;
}
};
......
......@@ -47,6 +47,7 @@ namespace se3
typedef typename traits<Joint>::Transformation_t Transformation_t; \
typedef typename traits<Joint>::Motion_t Motion_t; \
typedef typename traits<Joint>::Bias_t Bias_t; \
typedef typename traits<Joint>::F_t F_t; \
enum { \
nq = traits<Joint>::nq, \
nv = traits<Joint>::nv \
......@@ -70,6 +71,7 @@ namespace se3
const Transformation_t & M() const { return static_cast<const JointData*>(this)->M; }
const Motion_t & v() const { return static_cast<const JointData*>(this)->v; }
const Bias_t & c() const { return static_cast<const JointData*>(this)->c; }
F_t& F() { return static_cast< JointData*>(this)->F; }
};
template<typename _JointModel>
......
......@@ -21,7 +21,12 @@ namespace se3
struct ConstraintIdentity
{
const ConstraintIdentity& transpose() const { return *this; }
struct TransposeConst
{
Force::Vector6 operator* (const Force & phi)
{ return phi.toVector(); }
};
TransposeConst transpose() const { return TransposeConst(); }
operator ConstraintXd () const { return ConstraintXd(Eigen::MatrixXd::Identity(6,6)); }
};
template<typename D>
......@@ -31,23 +36,21 @@ namespace se3
return Motion(v);
}
friend Force::Vector6 operator* (const ConstraintIdentity&, const Force & phi)
{ return phi.toVector(); }
};
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointFreeFlyer::ConstraintIdentity & )
Inertia::Matrix6 operator*( const Inertia& Y,const JointFreeFlyer::ConstraintIdentity & )
{
const Inertia::Matrix6 matY = Y;
return ForceSet(matY.topRows<3>(), matY.bottomRows<3>() );
return Y.toMatrix();
}
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
ForceSet::Matrix6x
operator*( const JointFreeFlyer::ConstraintIdentity &, const ForceSet::Block & F )
template<typename D>
const Eigen::MatrixBase<D> &
operator*( const JointFreeFlyer::ConstraintIdentity::TransposeConst &, const Eigen::MatrixBase<D> & F )
{
return F.matrix(); // TODO try to avoid creation of a new MatrixXd
return F;
}
......@@ -61,6 +64,7 @@ namespace se3
typedef SE3 Transformation_t;
typedef Motion Motion_t;
typedef JointFreeFlyer::BiasZero Bias_t;
typedef Eigen::Matrix<double,6,6> F_t;
enum {
nq = 7,
nv = 6
......@@ -83,6 +87,7 @@ namespace se3
Motion_t v;
Bias_t c;
F_t F; // TODO if not used anymore, clean F_t
JointDataFreeFlyer() : M(1)
{
}
......
......@@ -74,10 +74,12 @@ namespace se3
{ return f.angular().segment<1>(axis); }
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
Eigen::Block<const typename ForceSet::Matrix3x>
operator*( const typename ForceSet::Block & F )
template<typename D>
friend typename Eigen::MatrixBase<D>::ConstRowXpr
operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F )
{
return F.ref.angular().block(axis,F.idx,1,F.len);
assert(F.rows()==6);
return F.row(3+axis);
}
};
......@@ -148,9 +150,9 @@ namespace se3
Eigen::Matrix3d R3;
double ca,sa; sincos(angle,&sa,&ca);
R3 <<
1,0,0,
0,ca,-sa,
0,sa,ca;
1,0,0,
0,ca,-sa,
0,sa,ca;
return R3;
}
template<>
......@@ -159,8 +161,8 @@ namespace se3
Eigen::Matrix3d R3;
double ca,sa; sincos(angle,&sa,&ca);
R3 <<
ca, 0, sa,
0 , 1, 0,
ca, 0, sa,
0, 1, 0,
-sa, 0, ca;
return R3;
}
......@@ -177,7 +179,8 @@ namespace se3
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointRevolute<0>::ConstraintRevolute & )
Eigen::Matrix<double,6,1>
operator*( const Inertia& Y,const JointRevolute<0>::ConstraintRevolute & )
{
/* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
const double
......@@ -186,13 +189,15 @@ namespace se3
&y = Y.lever()[1],
&z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(0,-m*z,m*y),
Eigen::Vector3d(I(0,0)+m*(y*y+z*z),
Eigen::Matrix<double,6,1> res; res << 0.0,-m*z,m*y,
I(0,0)+m*(y*y+z*z),
I(0,1)-m*x*y,
I(0,2)-m*x*z) );
I(0,2)-m*x*z ;
return res;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointRevolute<1>::ConstraintRevolute & )
Eigen::Matrix<double,6,1>
operator*( const Inertia& Y,const JointRevolute<1>::ConstraintRevolute & )
{
/* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
const double
......@@ -201,13 +206,15 @@ namespace se3
&y = Y.lever()[1],
&z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(m*z,0,-m*x),
Eigen::Vector3d(I(1,0)-m*x*y,
Eigen::Matrix<double,6,1> res; res << m*z,0,-m*x,
I(1,0)-m*x*y,
I(1,1)+m*(x*x+z*z),
I(1,2)-m*y*z) );
I(1,2)-m*y*z ;
return res;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointRevolute<2>::ConstraintRevolute & )
Eigen::Matrix<double,6,1>
operator*( const Inertia& Y,const JointRevolute<2>::ConstraintRevolute & )
{
/* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */
const double
......@@ -216,10 +223,11 @@ namespace se3
&y = Y.lever()[1],
&z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(-m*y,m*x,0),
Eigen::Vector3d(I(2,0)-m*x*z,
I(2,1)-m*y*z,
I(2,2)+m*(x*x+y*y)) );
Eigen::Matrix<double,6,1> res; res << -m*y,m*x,0,
I(2,0)-m*x*z,
I(2,1)-m*y*z,
I(2,2)+m*(x*x+y*y) ;
return res;
}
......@@ -232,6 +240,7 @@ namespace se3
typedef SE3 Transformation_t;
typedef typename JointRevolute<axis>::MotionRevolute Motion_t;
typedef typename JointRevolute<axis>::BiasZero Bias_t;
typedef Eigen::Matrix<double,6,1> F_t;
enum {
nq = 1,
nv = 1
......@@ -252,6 +261,8 @@ namespace se3
Motion_t v;
Bias_t c;
F_t F;
JointDataRevolute() : M(1)
{
M.translation(SE3::Vector3::Zero());
......
......@@ -53,7 +53,7 @@ namespace se3
#define JOINT_VISITOR_INIT(VISITOR) \
VISITOR( JointDataVariant & jdata,ArgsType args ) : jdata(jdata),args(args) {} \
using JointVisitor<VISITOR>::run; \
using JointVisitor< VISITOR >::run; \
JointDataVariant & jdata; \
ArgsType args
......
......@@ -128,6 +128,7 @@ namespace se3
* 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
return Inertia( m,
M.translation()+M.rotation()*c,
//dense_I);
M.rotation()*I*M.rotation().transpose());
}
/// bI = aXb.actInv(aI)
......
......@@ -70,6 +70,7 @@ int main(int argc, const char ** argv)
}
timer.toc(std::cout,1000);
#ifndef NDEBUG
std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
#ifdef __se3_rnea_hpp__
......@@ -82,11 +83,12 @@ int main(int argc, const char ** argv)
for(int i=0;i<model.nv;++i)
{
M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
//M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
}
std::cout << "Mrne = [ " << M << " ]; " << std::endl;
}
#endif // ifdef __se3_rnea_hpp__
#endif // ifndef NDEBUG
return 0;
......
Supports Markdown
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