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

Remove the duplicata crba crba2.

parent 6f01148e
#ifndef __se3_crba_hpp__
#ifndef __se3_crba2_hpp__
#define __se3_crba_hpp__
#include "pinocchio/multibody/visitor.hpp"
......@@ -7,11 +7,9 @@
namespace se3
{
inline const Eigen::VectorXd&
inline const Eigen::MatrixXd&
crba(const Model & model, Data& data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a);
const Eigen::VectorXd & q);
} // namespace se3
......@@ -49,37 +47,50 @@ namespace se3
namespace internal
{
template<typename D>
static Eigen::Matrix<double,6,D::ColsAtCompileTime>
act( const SE3 & m, const Eigen::MatrixBase<D> & iF )
template<typename D,typename Dret>
static void actVec( const SE3 & m,
const Eigen::MatrixBase<D> & iF,
Eigen::MatrixBase<Dret> & jF )
{
std::cout << "This never happens." << std::endl;
Eigen::Matrix<double,6,D::ColsAtCompileTime> jF(iF.rows(),iF.cols());
// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,6);
// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Dret,6);
typename D::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
typename D::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
Eigen::VectorBlock<const D,3> linear = iF.template head<3>();
Eigen::VectorBlock<const D,3> angular = iF.template tail<3>();
jF.template topRows <3>() = m.rotation()*linear;
jF.template bottomRows<3>() = skew(m.translation())*jF.template topRows<3>()
+m.rotation()*angular;
return jF;
jF.template head <3>() = m.rotation()*linear;
jF.template tail <3>() = m.translation().cross(jF.template head<3>()) + m.rotation()*angular;
}
int crbacounter = 0;
typedef Eigen::Matrix<double,6,1> Vector6;
static Vector6 act( const SE3 & m, const Eigen::MatrixBase<Vector6> & iF )
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)
{
crbacounter += 1;
Vector6 jF;
typename Dret::ColXpr jFc = jF.col(col);
actVec(m,iF.col(col),jFc);
}
}
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;
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>
......@@ -124,53 +135,37 @@ namespace se3
int i)
{
/*
* 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
* 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]
*/
Model::Index parent = model.parents[i];
data.Fcrb[i].block<6,JointModel::nv>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
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,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]);
jdata.F() = internal::act(data.liMi[i],jdata.F());
Eigen::Block<typename Data::Matrix6x> jF
= data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
internal::act_alt(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;
data.M(8,0) += 1;
while(parent>0)
{
// 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];
// 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;
}
};
......
#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_alt(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__
......@@ -80,7 +80,7 @@ namespace se3
= (link->getParent()->parent_joint==NULL) ?
(freeFlyer ? 1 : 0)
: model.getBodyId( link->getParent()->parent_joint->name );
std::cout << joint->name << " === " << parent << std::endl;
// std::cout << joint->name << " === " << parent << std::endl;
const SE3 & jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
......
......@@ -4,7 +4,6 @@
#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"
......@@ -35,7 +34,7 @@ int main(int argc, const char ** argv)
std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
if(argc>1) filename = argv[1];
model = se3::buildModel(filename);
model = se3::buildModel(filename,argc>1);
// SIMPLE no FF
......@@ -67,13 +66,10 @@ int main(int argc, const char ** argv)
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(1000)
{
crba2(model,data,q);
crba(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;
......
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