From dd4a37ae82350ed2bc8dc4ac29956dbdabc3f74e Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 26 Aug 2014 12:01:09 +0200
Subject: [PATCH] First working vesrion of CRBA (checked VS RBDL for revolute
joints).
---
CMakeLists.txt | 7 ++
doc/latex/se3.tex | 11 +-
src/algorithm/crba.hpp | 146 +++++++++++++++++++++++
src/algorithm/rnea.hpp | 6 +-
src/exception.hpp | 28 +++++
src/multibody/constraint.hpp | 8 +-
src/multibody/joint/joint-base.hpp | 36 +++++-
src/multibody/joint/joint-free-flyer.hpp | 17 ++-
src/multibody/joint/joint-generic.hpp | 132 ++++++++++++++++++++
src/multibody/joint/joint-revolute.hpp | 21 +++-
src/multibody/model.hpp | 28 ++++-
src/multibody/parser/urdf.hpp | 10 +-
src/spatial/inertia.hpp | 23 +++-
unittest/crba.cpp | 94 +++++++++++++++
unittest/tspatial.cpp | 11 ++
unittest/variant.cpp | 8 +-
16 files changed, 558 insertions(+), 28 deletions(-)
create mode 100644 src/algorithm/crba.hpp
create mode 100644 src/exception.hpp
create mode 100644 src/multibody/joint/joint-generic.hpp
create mode 100644 unittest/crba.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c8fca6b1..3146c6f50 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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)
diff --git a/doc/latex/se3.tex b/doc/latex/se3.tex
index 470a9aa2d..382e9850e 100644
--- a/doc/latex/se3.tex
+++ b/doc/latex/se3.tex
@@ -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:
diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
new file mode 100644
index 000000000..cc49ad2cd
--- /dev/null
+++ b/src/algorithm/crba.hpp
@@ -0,0 +1,146 @@
+#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__
+
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
index bee46880b..5e10b6670 100644
--- a/src/algorithm/rnea.hpp
+++ b/src/algorithm/rnea.hpp
@@ -1,5 +1,5 @@
-#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__
diff --git a/src/exception.hpp b/src/exception.hpp
new file mode 100644
index 000000000..b7f28d299
--- /dev/null
+++ b/src/exception.hpp
@@ -0,0 +1,28 @@
+#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__
diff --git a/src/multibody/constraint.hpp b/src/multibody/constraint.hpp
index f82bd9ede..18eddb33c 100644
--- a/src/multibody/constraint.hpp
+++ b/src/multibody/constraint.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__
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index 94dcc3727..3b8c4e177 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.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
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 21e4beef3..bae28b67f 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -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
diff --git a/src/multibody/joint/joint-generic.hpp b/src/multibody/joint/joint-generic.hpp
new file mode 100644
index 000000000..533b87c56
--- /dev/null
+++ b/src/multibody/joint/joint-generic.hpp
@@ -0,0 +1,132 @@
+#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__
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 71db05102..a9f9d25eb 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.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
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index cf53f9363..eda2c96f1 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -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);
}
+ void Data::computeLastChild(const Model& model)
+ {
+ typedef Model::Index Index;
+ //lastChild.fill(-1); TODO use fill algorithm
+ for( int i=0;i<model.nbody;++i ) lastChild[i] = -1;
+
+
+ for( int i=model.nbody-1;i>=0;--i )
+ {
+ if(lastChild[i] == -1) lastChild[i] = i;
+ const Index & parent = model.parents[i];
+ lastChild[parent] = std::max(lastChild[i],lastChild[parent]);
+ }
+ }
} // namespace se3
diff --git a/src/multibody/parser/urdf.hpp b/src/multibody/parser/urdf.hpp
index 17ab1fc95..b43daae96 100644
--- a/src/multibody/parser/urdf.hpp
+++ b/src/multibody/parser/urdf.hpp
@@ -66,8 +66,11 @@ namespace se3
// std::cout << "#" << link->parent_joint->name << std::endl;
// else std::cout << "###ROOT" << std::endl;
- assert(link->inertial && "The parser cannot accept trivial mass");
- const Inertia & Y = convertFromUrdf(*link->inertial);
+ // assert(link->inertial && "The parser cannot accept trivial mass");
+ const Inertia & Y = (link->inertial) ?
+ convertFromUrdf(*link->inertial)
+ : Inertia::Identity();
+
//std::cout << "Inertia: " << Y << std::endl;
if(joint!=NULL)
@@ -118,7 +121,8 @@ namespace se3
else /* (joint==NULL) */
{ /* The link is the root of the body. */
//std::cout << "Parent = 0 (universe)" << std::endl;
- model.addBody( 0, JointModelFreeFlyer(), SE3::Identity(), Y, "root" );
+ //model.addBody( 0, JointModelFreeFlyer(), SE3::Identity(), Y, "root" ); // TODO replace RX by FF
+ model.addBody( 0, JointModelRX(), SE3::Identity(), Y, "root" ); // TODO replace RX by FF
}
BOOST_FOREACH(urdf::LinkConstPtr child,link->child_links)
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index 87291b894..263a67c62 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -67,9 +67,16 @@ namespace se3
}
static Inertia Random()
{
+ /* We have to shoot "I" definite positive and not only symmetric. */
+ Matrix3 M3 = Matrix3::Random(); // TODO clean that mess
+ Matrix3 D = Vector3::Random().cwiseAbs().asDiagonal();
+ Matrix3 M3D2 = M3+2*D;
+ Matrix3 posdiag
+ = M3D2.template selfadjointView<Eigen::Upper>();
return InertiaTpl(Eigen::internal::random<Scalar>(),
Vector3::Random(),
- Matrix3::Random().template selfadjointView<Eigen::Upper>());
+ //Matrix3::Random().template selfadjointView<Eigen::Upper>());
+ posdiag);
}
// Getters
@@ -91,7 +98,19 @@ namespace se3
// Arithmetic operators
Inertia operator+(const InertiaTpl &other) const
{
- return InertiaTpl(m+other.m, c+other.c, I+other.I);
+ const double & mm = m+other.m;
+ const Matrix3 & X = skew((c-other.c).eval());
+ Matrix3 mmmXX = m*other.m/mm*X*X; // TODO clean this mess
+ return InertiaTpl(mm, (m*c+other.m*other.c)/mm, dense_I+other.dense_I-mmmXX); // TODO: += in Eigen::Symmetric?
+ }
+ Inertia& operator+=(const InertiaTpl &other)
+ {
+ const Matrix3 & X = skew((c-other.c).eval());
+ Matrix3 mmXX = m*other.m*X*X; // TODO: clean this mess
+ c *=m; c+=other.m*other.c;
+ m+=other.m; c/=m;
+ dense_I+=other.dense_I-mmXX/m; // TODO: += in Eigen::Symmetric?
+ return *this;
}
Force operator*(const Motion &v) const
diff --git a/unittest/crba.cpp b/unittest/crba.cpp
new file mode 100644
index 000000000..410e269ae
--- /dev/null
+++ b/unittest/crba.cpp
@@ -0,0 +1,94 @@
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/multibody/parser/urdf.hpp"
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+
+//#define __SSE3__
+#include <fenv.h>
+#ifdef __SSE3__
+#include <pmmintrin.h>
+#endif
+
+int main(int argc, const char ** argv)
+{
+#ifdef __SSE3__
+ _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
+#endif
+
+
+ using namespace Eigen;
+ using namespace se3;
+
+ SE3::Matrix3 I3 = SE3::Matrix3::Identity();
+
+ se3::Model model;
+
+ // std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
+ // if(argc>1) filename = argv[1];
+ // model = se3::buildModel(filename);
+
+
+ // SIMPLE no FF
+ // model.addBody(model.getBodyId("universe"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg1");
+ // model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg2");
+ // model.addBody(model.getBodyId("rleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg3");
+
+ // model.addBody(model.getBodyId("universe"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg1");
+ // model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg2");
+ // model.addBody(model.getBodyId("lleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg3");
+
+
+ //SIMPLE with FF
+ model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"root");
+
+ model.addBody(model.getBodyId("root"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg1");
+ model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg2");
+ model.addBody(model.getBodyId("rleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg3");
+
+ model.addBody(model.getBodyId("root"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg1");
+ model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg2");
+ model.addBody(model.getBodyId("lleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg3");
+
+
+ se3::Data data(model);
+
+ VectorXd q = VectorXd::Zero(model.nq);
+
+ StackTicToc timer(StackTicToc::US); timer.tic();
+ SMOOTH(1)
+ {
+ crba(model,data,q);
+ }
+ timer.toc(std::cout,1000);
+
+ std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
+
+#ifdef __se3_rnea_hpp__
+ /* Joint inertia from iterative crba. */
+ {
+ Eigen::MatrixXd M(model.nv,model.nv);
+ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
+ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
+ const Eigen::VectorXd bias = rnea(model,data,q,v,a);
+
+ for(int i=0;i<model.nv;++i)
+ {
+
+ 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__
+
+
+ return 0;
+}
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
index d771a42d8..554b7e3c8 100644
--- a/unittest/tspatial.cpp
+++ b/unittest/tspatial.cpp
@@ -221,6 +221,17 @@ bool testInertia()
Force vxIv = aI.vxiv(v);
assert( vxf.toVector().isApprox(vxIv.toVector()) );
+ // Test operator+
+ I1 = Inertia::Random();
+ Inertia I2 = Inertia::Random();
+ assert( (I1.toMatrix()+I2.toMatrix()).isApprox((I1+I2).toMatrix()) );
+
+ Inertia I12 = I1;
+ I12 += I2;
+ assert( (I1.toMatrix()+I2.toMatrix()).isApprox(I12.toMatrix()) );
+
+
+
return true;
}
diff --git a/unittest/variant.cpp b/unittest/variant.cpp
index 42e9aef1b..ee65a3146 100644
--- a/unittest/variant.cpp
+++ b/unittest/variant.cpp
@@ -2,6 +2,7 @@
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/joint/joint-generic.hpp"
#include <iostream>
@@ -16,12 +17,11 @@ int main()
using namespace se3;
- JointModelVariant jmodel = JointModelRX(0,0);
+ JointModelVariant jmodel = JointModelRX();
const JointDataVariant & jdata = CreateJointData::run(jmodel);
-
-
-
+ JointDataGeneric jdatagen(jdata);
+ JointModelGeneric jmodelgen(jmodel);
se3::Model model;
}
--
GitLab