Skip to content
GitLab
Commit fe8407cd authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Remove the duplicata crba crba2.

parent 6f01148e
Hide whitespace changes
Inline Side-by-side
src/algorithm/crba.hpp
View file @ fe8407cd
#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());
typename D::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
typename D::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
// 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 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;
}
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);
}
}
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( const SE3 & m,
const Eigen::MatrixBase<D> & iF,
Eigen::MatrixBase<Dret> & jF )
{
crbacounter += 1;
Vector6 jF;
Eigen::VectorBlock<const Vector6,3> linear = iF.head<3>();
Eigen::VectorBlock<const Vector6,3> angular = iF.tail<3>();
typename D::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
typename D::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
jF.head <3>() = m.rotation()*linear;
jF.tail <3>() = m.translation().cross(jF.head<3>()) + m.rotation()*angular;
return jF;
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;
}
};
......
src/algorithm/crba2.hpp deleted 100644 → 0
View file @ 6f01148e
#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__
src/multibody/parser/urdf.hpp
View file @ fe8407cd
......@@ -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);
......
unittest/crba.cpp
View file @ fe8407cd
......@@ -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