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

First working vesrion of CRBA (checked VS RBDL for revolute joints).

parent 087a5fbe
......@@ -31,6 +31,7 @@ ADD_REQUIRED_DEPENDENCY("urdfdom >= v0.3.0")
# --- INCLUDE ----------------------------------------
# ----------------------------------------------------
SET(${PROJECT_NAME}_HEADERS
exception.hpp
spatial/se3.hpp
spatial/motion.hpp
spatial/force.hpp
......@@ -43,11 +44,13 @@ SET(${PROJECT_NAME}_HEADERS
multibody/joint/joint-revolute.hpp
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
multibody/joint/joint-generic.hpp
multibody/model.hpp
multibody/visitor.hpp
multibody/parser/urdf.hpp
tools/timer.hpp
algorithm/rnea.hpp
algorithm/crba.hpp
algorithm/kinematics.hpp
)
......@@ -84,6 +87,10 @@ PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL unittest/rnea.cpp)
PKG_CONFIG_USE_DEPENDENCY(rnea eigenpy)
ADD_EXECUTABLE(crba EXCLUDE_FROM_ALL unittest/crba.cpp)
PKG_CONFIG_USE_DEPENDENCY(crba eigenpy)
PKG_CONFIG_USE_DEPENDENCY(crba urdfdom)
ADD_EXECUTABLE(dg EXCLUDE_FROM_ALL unittest/dg.cpp)
PKG_CONFIG_USE_DEPENDENCY(dg eigenpy)
......
......@@ -103,7 +103,8 @@ For Featherstone, $E = \bRa =\aRb^T$ and $r = \apb$. Then:
$$\bXa^* = \BIN \bRa & 0 \\ -\bRa \apb_\times & \bRa \BOUT = \BIN E & 0 \\ - E r_\times & E \BOUT $$
$$\aXb^* = \BIN \bRa^T & 0 \\ \apb_\times \bRa^T & \bRa^T \BOUT = \BIN E^T & 0 \\ r_\times E^T & E^T \BOUT $$
\section{Inertia application}
\section{Inertia}
\subsection{Inertia application}
$$\aY: \avs \rightarrow \afs = \aY \avs$$
......@@ -131,6 +132,14 @@ Nota: the square of the cross product is:
$$\BIN x\\y\\z\BOUT_ \times^2 = \BIN 0&-z&y \\ z&0&-x \\ -y&x&0 \BOUT^2 = \BIN -y^2-z^2&xy&xz \\ xy&-x^2-z^2&yz \\ xz&yz&-x^2-y^2 \BOUT$$
There is no computational interest in using it.
\subsection{Inertia addition}
$$ Y_p = \BIN m_p & m_p p_\times^T \\ m_p p_\times & I_p + m_p p_\times p_\times^T \BOUT$$
$$ Y_q = \BIN m_q & m_q q_\times^T \\ m_q q_\times & I_q + m_q q_\times q_\times^T \BOUT$$
\section{Cross products}
Motion-motion product:
......
#ifndef __se3_crba_hpp__
#define __se3_crba_hpp__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
namespace se3
{
inline const Eigen::VectorXd&
crba(const Model & model, Data& data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
struct CrbaForwardStep : public fusion::JointVisitor<CrbaForwardStep>
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
const int&,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(CrbaForwardStep);
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);
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.Ycrb[i] = model.inertias[i];
}
};
struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
{
typedef boost::fusion::vector<const Model&,
Data&,
const int &> ArgsType;
JOINT_VISITOR_INIT(CrbaBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointData> & jdata,
const Model& model,
Data& data,
int i)
{
/*
* F = Yi*Si
* for j<i
* F = ljXj.act(F)
* M.block(j,i,nvj,nvi) = Sj'*F
*/
std::cout << "*** joint " << i << std::endl;
Model::Index parent = model.parents[i];
if( parent>0 )
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
//std::cout << "liMi = " << (SE3::Matrix4)data.liMi[i] << std::endl;
Eigen::Matrix<double,6,JointModel::nv> F;
ConstraintXd S = jdata.S();
F = data.Ycrb[i].toMatrix() * S.matrix() ;
std::cout << "*** joint " << i << std::endl;
std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
std::cout << "iSi = " << S.matrix() << std::endl;
std::cout << "iFi = " << F << std::endl;
data.M.block<JointModel::nv,JointModel::nv>(jmodel.idx_v(),jmodel.idx_v())
= S.matrix().transpose() * F;
SE3::Matrix6 ljXj = data.liMi[i];
while(parent>0)
{
JointDataGeneric jdataparent( data.joints[parent] );
JointModelGeneric jmodelparent( model.joints[parent] );
F = ljXj.inverse().transpose()*F; // equivalent to ljF = ljMj.act(jF)
const ConstraintXd::DenseBase & S = jdataparent.S.matrix();
std::cout << "jFi = " << F <<std::endl;
std::cout << "jS = " << S <<std::endl;
data.M.block(jmodelparent.idx_v(),jmodel.idx_v(),S.cols(),JointModel::nv)
= S.transpose() * F;
std::cout << "\t\t on parent #i,j = " << i<<","<<parent << " ... compute block "
<< jmodelparent.idx_v() << ":"
<< jmodelparent.idx_v() + S.cols() << " x "
<< jmodel.idx_v() << ":"
<< jmodel.idx_v() + JointModel::nv << std::endl;
std::cout << "jFi = " << F << std::endl;
std::cout << "jSj = " << S << std::endl;
ljXj = data.liMi[parent];
parent = model.parents[parent];
}
}
};
inline const Eigen::MatrixXd&
crba(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
for( int i=1;i<model.nbody;++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
CrbaForwardStep::ArgsType(model,data,i,q));
}
for( int i=model.nbody-1;i>0;--i )
{
CrbaBackwardStep::run(model.joints[i],data.joints[i],
CrbaBackwardStep::ArgsType(model,data,i));
}
return data.M;
}
} // namespace se3
#endif // ifndef __se3_crba_hpp__
#ifndef __se3_rne_hpp__
#define __se3_rne_hpp__
#ifndef __se3_rnea_hpp__
#define __se3_rnea_hpp__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
......@@ -109,5 +109,5 @@ namespace se3
}
} // namespace se3
#endif // ifndef __se3_rne_hpp__
#endif // ifndef __se3_rnea_hpp__
#ifndef __se3_exception_hpp__
#define __se3_exception_hpp__
#include <exception>
#include <string>
namespace se3
{
class Exception : public std::exception
{
public:
Exception() : message() {}
Exception(std::string msg) : message(msg) {}
const char *what() const throw()
{
return this->getMessage().c_str();
}
~Exception() throw() {}
virtual const std::string & getMessage() const { return message; }
std::string copyMessage() const { return getMessage(); }
protected:
std::string message;
};
} // namespace
#endif // ifndef __se3_exception_hpp__
......@@ -11,7 +11,7 @@
namespace se3
{
template<int _Dim, typename _Scalar, int _Options>
template<int _Dim, typename _Scalar, int _Options=0>
class ConstraintTpl
{
public:
......@@ -51,8 +51,10 @@ namespace se3
DenseBase S;
};
};
typedef ConstraintTpl<1,double,0> Constraint1d;
typedef ConstraintTpl<6,double,0> Constraint6d;
typedef ConstraintTpl<Eigen::Dynamic,double,0> ConstraintXd;
} // namespace se3
#endif // ifndef __se3_constraint_hpp__
......@@ -12,9 +12,10 @@ namespace se3
{
template<class C> struct traits {};
/*
/* RNEA operations
*
* *** FORWARD ***
* J::calc()
* J::calc(q,vq)
* SE3 = SE3 * J::SE3
* Motion = J::Motion
* Motion = J::Constraint*J::JointMotion + J::Bias + Motion^J::Motion
......@@ -24,6 +25,21 @@ namespace se3
* J::JointForce = J::Constraint::Transpose*J::Force
*/
/* CRBA operations
*
* *** FORWARD ***
* J::calc(q)
* Inertia = Inertia
*
* *** BACKWARD ***
* Inertia += SE3::act(Inertia)
* F = Inertia*J::Constraint
* JointInertia.block = J::Constraint::Transpose*F
* *** *** INNER ***
* F = SE3::act(f)
* JointInertia::block = J::Constraint::Transpose*F
*/
#define SE3_JOINT_TYPEDEF \
typedef typename traits<Joint>::JointData JointData; \
typedef typename traits<Joint>::JointModel JointModel; \
......@@ -36,6 +52,11 @@ namespace se3
nv = traits<Joint>::nv \
}
#define SE3_JOINT_USE_INDEXES \
typedef JointModelBase<JointModel> Base; \
using Base::idx_q; \
using Base::idx_v
template<typename _JointData>
struct JointDataBase
{
......@@ -45,10 +66,10 @@ namespace se3
JointData& derived() { return *static_cast<JointData*>(this); }
const JointData& derived() const { return *static_cast<const JointData*>(this); }
const Constraint_t & S() { return static_cast<JointData*>(this)->S; }
const Transformation_t & M() { return static_cast<JointData*>(this)->M; }
const Motion_t & v() { return static_cast<JointData*>(this)->v; }
const Bias_t & c() { return static_cast<JointData*>(this)->c; }
const Constraint_t & S() const { return static_cast<const JointData*>(this)->S; }
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; }
};
template<typename _JointModel>
......@@ -61,6 +82,9 @@ namespace se3
const JointModel& derived() const { return *static_cast<const JointModel*>(this); }
JointData createData() const { return static_cast<const JointModel*>(this)->createData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs ) const
{ return static_cast<const JointModel*>(this)->calc(data,qs); }
void calc( JointData& data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
......
......@@ -2,6 +2,7 @@
#define __se3_joint_free_flyer_hpp__
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
namespace se3
{
......@@ -11,13 +12,17 @@ namespace se3
struct JointFreeFlyer
{
struct BiasZero {};
struct BiasZero
{
operator Motion () const { return Motion::Zero(); }
};
friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
struct ConstraintIdentity
{
const ConstraintIdentity& transpose() const { return *this; }
operator ConstraintXd () const { return ConstraintXd(Eigen::MatrixXd::Identity(6,6)); }
};
template<typename D>
friend Motion operator* (const ConstraintIdentity&, const Eigen::MatrixBase<D>& v)
......@@ -30,9 +35,6 @@ namespace se3
{ return phi.toVector(); }
};
struct JointModelFreeFlyer;
struct JointDataFreeFlyer;
template<>
struct traits<JointFreeFlyer>
{
......@@ -75,6 +77,13 @@ namespace se3
SE3_JOINT_TYPEDEF;
JointData createData() const { return JointData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<nq>::Type q = qs.segment<nq>(idx_q());
JointData::Quaternion quat(Eigen::Matrix<double,4,1>(q.tail(4))); // TODO
data.M = SE3(quat.matrix(),q.head<3>());
}
void calc( JointData& data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
......
#ifndef __se3_joint_generic_hpp__
#define __se3_joint_generic_hpp__
#include "pinocchio/exception.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
namespace se3
{
struct JointGeneric;
struct JointModelGeneric;
struct JointDataGeneric;
template<>
struct traits<JointGeneric>
{
typedef JointDataGeneric JointData;
typedef JointModelGeneric JointModel;
typedef ConstraintXd Constraint_t;
typedef SE3 Transformation_t;
typedef Motion Motion_t;
typedef Motion Bias_t;
enum {
nq = -1,
nv = -1
};
};
template<> struct traits<JointDataGeneric> { typedef JointGeneric Joint; };
template<> struct traits<JointModelGeneric> { typedef JointGeneric Joint; };
struct JointDataGeneric : public JointDataBase<JointDataGeneric>
{
typedef JointGeneric Joint;
SE3_JOINT_TYPEDEF;
Constraint_t S;
Transformation_t M;
Motion_t v;
Bias_t c;
JointDataGeneric() {}
JointDataGeneric( const JointDataVariant & jvar );
};
struct JointModelGeneric : public JointModelBase<JointModelGeneric>
{
typedef JointGeneric Joint;
SE3_JOINT_TYPEDEF;
SE3_JOINT_USE_INDEXES;
JointData createData() const { return JointData(); }
virtual void jcalc(JointData& /*data*/,
const Eigen::VectorXd & /*qs*/) const
{
throw se3::Exception("Virtual function calc not implemented in the generic class. Derive it.");
}
virtual void jcalc(JointData& /*data*/,
const Eigen::VectorXd & /*qs*/,
const Eigen::VectorXd & /*vs*/ ) const
{
throw se3::Exception("Virtual function calc not implemented in the generic class. Derive it.");
}
JointModelGeneric() {}
JointModelGeneric( const JointModelVariant & jvar );
JointModelGeneric & operator= (const JointModelGeneric& clone)
{
setIndexes(clone.idx_q(),clone.idx_v()); return *this;
}
};
/* --- Convert Data ------------------------------------------------------ */
namespace internal
{
struct JointDataVariantToGeneric : public boost::static_visitor< JointDataGeneric >
{
template <typename D>
JointDataGeneric operator() ( const JointDataBase<D> & jdata ) const
{
JointDataGeneric jgen;
jgen.S = jdata.S();
jgen.M = jdata.M();
jgen.v = jdata.v();
jgen.c = jdata.c();
return jgen;
}
static JointDataGeneric run ( const JointDataVariant & jdata )
{
return boost::apply_visitor( JointDataVariantToGeneric(),jdata );
}
};
}// namespace internal
JointDataGeneric::JointDataGeneric( const JointDataVariant & jvar )
{
const JointDataGeneric & clone = internal::JointDataVariantToGeneric::run(jvar);
(*this) = clone;
}
/* --- Convert Model ----------------------------------------------------- */
namespace internal
{
struct JointModelVariantToGeneric : public boost::static_visitor< JointModelGeneric >
{
template <typename D>
JointModelGeneric operator() ( const JointModelBase<D> & jmodel ) const
{
JointModelGeneric jgen;
jgen.setIndexes(jmodel.idx_q(),jmodel.idx_v());
return jgen;
}
static JointModelGeneric run ( const JointModelVariant & jmodel )
{
return boost::apply_visitor( JointModelVariantToGeneric(),jmodel );
}
};
}// namespace internal
JointModelGeneric::JointModelGeneric( const JointModelVariant & jvar )
{
const JointModelGeneric & clone = internal::JointModelVariantToGeneric::run(jvar);
(*this) = clone;
}
} // namespace se3
#endif // ifndef __se3_joint_generic_hpp__
......@@ -2,6 +2,7 @@
#define __se3_joint_revolute_hpp__
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
namespace se3
{
......@@ -16,6 +17,7 @@ namespace se3
{
double w;
CartesianVector3(const double & w) : w(w) {}
CartesianVector3() : w(1) {}
operator Eigen::Vector3d (); // { return Eigen::Vector3d(w,0,0); }
};
template<>CartesianVector3<0>::operator Eigen::Vector3d () { return Eigen::Vector3d(w,0,0); }
......@@ -31,7 +33,10 @@ namespace se3
template<int axis>
struct JointRevolute {
struct BiasZero {};
struct BiasZero
{
operator Motion () const { return Motion::Zero(); }
};
friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
......@@ -62,6 +67,13 @@ namespace se3
Force::Vector3::ConstFixedSegmentReturnType<1>::Type
operator*( const Force& f ) const
{ return f.angular().segment<1>(axis); }
operator ConstraintXd () const
{
Eigen::Matrix<double,6,1> S;
S << Eigen::Vector3d::Zero(), (Eigen::Vector3d)revolute::CartesianVector3<axis>();
return ConstraintXd(S);
}
}; // struct ConstraintRevolute
static Eigen::Matrix3d cartesianRotation(const double & angle);
......@@ -191,6 +203,13 @@ namespace se3
using JointModelBase<JointModelRevolute>::setIndexes;
JointData createData() const { return JointData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs ) const
{
const double & q = qs[idx_q()];
data.M.rotation(JointRevolute<axis>::cartesianRotation(q));
}
void calc( JointData& data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
......
......@@ -71,9 +71,15 @@ namespace se3
std::vector<SE3> liMi; // Body relative placement (wrt parent)
Eigen::VectorXd tau; // Joint forces
Data( const Model& ref );
std::vector<Inertia> Ycrb; // Inertia of the sub-tree composit rigid body
Eigen::MatrixXd M; // Joint Inertia
Eigen::MatrixXd Fcrb; // Spatial forces set, used in CRBA
std::vector<Model::Index> lastChild; // Index of the last child (for CRBA)
Data( const Model& ref );
void computeLastChild(const Model& model);
};
const Eigen::Vector3d Model::gravity981 (0,0,-9.81);
......@@ -144,11 +150,31 @@ namespace se3
,oMi(ref.nbody)
,liMi(ref.nbody)
,tau(ref.nv)
,Ycrb(ref.nbody)
,M(ref.nv,ref.nv)
,Fcrb(6,ref.nv)
,lastChild(ref.nbody)
{
for(int i=0;i<model.nbody;++i)
joints.push_back(CreateJointData::run(model.joints[i]));
M.fill(NAN);
computeLastChild(ref);