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

CRBA single loop working and efficient: 8us for the simple h.

parent 9ecf12e6
......@@ -52,6 +52,7 @@ SET(${PROJECT_NAME}_HEADERS
tools/timer.hpp
algorithm/rnea.hpp
algorithm/crba.hpp
algorithm/crba2.hpp
algorithm/kinematics.hpp
)
......
......@@ -53,6 +53,7 @@ namespace se3
static Eigen::Matrix<double,6,D::ColsAtCompileTime>
act( const SE3 & m, const Eigen::MatrixBase<D> & iF )
{
std::cout << "This never happens." << std::endl;
Eigen::Matrix<double,6,D::ColsAtCompileTime> jF(iF.rows(),iF.cols());
typename D::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
......@@ -64,9 +65,12 @@ namespace se3
return jF;
}
int crbacounter = 0;
typedef Eigen::Matrix<double,6,1> Vector6;
static Vector6 act( const SE3 & m, const Eigen::MatrixBase<Vector6> & iF )
{
crbacounter += 1;
Vector6 jF;
Eigen::VectorBlock<const Vector6,3> linear = iF.head<3>();
......@@ -131,15 +135,15 @@ namespace se3
Model::Index parent = model.parents[i];
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();
// 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 << "iFi = " << jdata.F() << std::endl;
if(parent>0)
{
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
// data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
jdata.F() = internal::act(data.liMi[i],jdata.F());
}
......@@ -147,13 +151,16 @@ namespace se3
// std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
// std::cout << "liFi = " << jdata.F() << std::endl;
data.M(8,0) += 1;
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) );
// std::cout << "i,parent =" << i<<","<<parent <<std::endl;
// 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];
......
#ifndef __se3_crba2_hpp__
#define __se3_crba2_hpp__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include <iostream>
namespace se3
{
inline const Eigen::VectorXd&
crba2(const Model & model, Data& data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
struct Crba2ForwardStep : public fusion::JointVisitor<Crba2ForwardStep>
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
const int&,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(Crba2ForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model& model,
se3::Data& data,
const int &i,
const Eigen::VectorXd & q)
{
using namespace Eigen;
using namespace se3;
jmodel.calc(jdata.derived(),q);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.Ycrb[i] = model.inertias[i];
}
};
namespace internal
{
int crba2counter = 0;
template<typename D,typename Dret>
static void actVec( const SE3 & m,
const Eigen::MatrixBase<D> & iF,
Eigen::MatrixBase<Dret> & jF )
{
crba2counter += 1;
// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,6);
// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Dret,6);
Eigen::VectorBlock<const D,3> linear = iF.template head<3>();
Eigen::VectorBlock<const D,3> angular = iF.template tail<3>();
jF.template head <3>() = m.rotation()*linear;
jF.template tail <3>() = m.translation().cross(jF.template head<3>()) + m.rotation()*angular;
}
template<typename D,typename Dret>
static void
act_alt( const SE3 & m,
const Eigen::MatrixBase<D> & iF,
Eigen::MatrixBase<Dret> & jF )
{
for(int col=0;col<jF.cols();++col)
{
typename Dret::ColXpr jFc = jF.col(col);
actVec(m,iF.col(col),jFc);
}
}
template<typename D,typename Dret>
static void
act( const SE3 & m,
const Eigen::MatrixBase<D> & iF,
Eigen::MatrixBase<Dret> & jF )
{
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>().noalias() = m.rotation()*linear;
jF.template bottomRows<3>().noalias()
= skew(m.translation())*jF.template topRows<3>() +
m.rotation()*angular;
}
} // namespace internal
template<typename D>
struct Crba2InternalBackwardStep : public fusion::JointVisitor<Crba2InternalBackwardStep<D> >
{
typedef boost::fusion::vector<const Model&,
Data&,
const Eigen::MatrixBase<D>&,
const int &,
const int &> ArgsType;
Crba2InternalBackwardStep( JointDataVariant & jdata,ArgsType args ) : jdata(jdata),args(args) {}
using fusion::JointVisitor< Crba2InternalBackwardStep<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 Crba2BackwardStep : public fusion::JointVisitor<Crba2BackwardStep>
{
typedef boost::fusion::vector<const Model&,
Data&,
const int &> ArgsType;
JOINT_VISITOR_INIT(Crba2BackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointData> & jdata,
const Model& model,
Data& data,
int i)
{
/*
* F[1:6,i] = Y*S
* M[i,SUBTREE] = S'*F[1:6,SUBTREE]
* if li>0
* Yli += liXi Yi
* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
*/
data.Fcrb[i].block<6,JointModel::nv>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
data.M.block(jmodel.idx_v(),jmodel.idx_v(),JointModel::nv,data.nvSubtree[i])
= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
// std::cout << "*** joint " << i << std::endl;
// std::cout << "iFi = " << jdata.F() << std::endl;
const Model::Index & parent = model.parents[i];
if(parent>0)
{
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
Eigen::Block<typename Data::Matrix6x> jF
= data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
internal::act(data.liMi[i],
data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
jF);
}
// 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;
// std::cout << "M = " << data.M << std::endl;
}
};
inline const Eigen::MatrixXd&
crba2(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
for( int i=1;i<model.nbody;++i )
{
Crba2ForwardStep::run(model.joints[i],data.joints[i],
Crba2ForwardStep::ArgsType(model,data,i,q));
}
for( int i=model.nbody-1;i>0;--i )
{
Crba2BackwardStep::run(model.joints[i],data.joints[i],
Crba2BackwardStep::ArgsType(model,data,i));
}
return data.M;
}
} // namespace se3
#endif // ifndef __se3_crba2_hpp__
......@@ -74,7 +74,9 @@ namespace se3
std::vector<Inertia> Ycrb; // Inertia of the sub-tree composit rigid body
Eigen::MatrixXd M; // Joint Inertia
ForceSet Fcrb; // Spatial forces set, used in CRBA
typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
std::vector<Matrix6x> Fcrb; // Spatial forces set, used in CRBA
std::vector<Model::Index> lastChild; // Index of the last child (for CRBA)
std::vector<int> nvSubtree; // Dimension of the subtree motion space (for CRBA)
......@@ -161,6 +163,8 @@ namespace se3
for(int i=0;i<model.nbody;++i)
joints.push_back(CreateJointData::run(model.joints[i]));
M.fill(NAN);
for(int i=0;i<ref.nbody;++i ) { Fcrb[i].resize(6,model.nv); Fcrb[i].fill(NAN); }
//for(int i=0;i<ref.nbody;++i ) { Fcrb[i].resize(6,12); Fcrb[i].fill(0); }
computeLastChild(ref);
}
......
......@@ -11,4 +11,18 @@ namespace se3
m(2,0) = -v[1]; m(2,1) = v[0]; m(2,2) = 0 ;
return m;
}
template <typename V,typename M>
inline Eigen::Matrix<typename M::Scalar,3,M::ColsAtCompileTime,M::Options>
cross(const Eigen::MatrixBase<V> & v,
const Eigen::MatrixBase<M> & m)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V,3);
Eigen::Matrix<typename M::Scalar,3,M::ColsAtCompileTime,M::Options> res (3,m.cols());
res.row(0) = v[1]*m.row(2) - v[2]*m.row(1);
res.row(1) = v[2]*m.row(0) - v[0]*m.row(2);
res.row(2) = v[0]*m.row(1) - v[1]*m.row(0);
return res;
}
} // namespace se3
......@@ -4,6 +4,7 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/crba2.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/multibody/parser/urdf.hpp"
......@@ -66,10 +67,13 @@ int main(int argc, const char ** argv)
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(1000)
{
crba(model,data,q);
crba2(model,data,q);
}
timer.toc(std::cout,1000);
//std::cout << se3::internal::crbacounter << std::endl;
//std::cout << se3::internal::crba2counter << std::endl;
#ifndef NDEBUG
std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
......@@ -83,7 +87,7 @@ 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;
}
......
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