diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3146c6f506889fc7b262f0ff3b9826cdf86c9707..3cb7a5a3fa50b618318010a142954b2bc8c7b861 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,6 +39,7 @@ SET(${PROJECT_NAME}_HEADERS
spatial/fwd.hpp
spatial/skew.hpp
multibody/constraint.hpp
+ multibody/force-set.hpp
multibody/joint.hpp
multibody/joint/joint-base.hpp
multibody/joint/joint-revolute.hpp
@@ -84,6 +85,9 @@ ENDFOREACH(header)
ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL unittest/tspatial.cpp)
PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
+ADD_EXECUTABLE(constraint EXCLUDE_FROM_ALL unittest/constraint.cpp)
+PKG_CONFIG_USE_DEPENDENCY(constraint eigenpy)
+
ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL unittest/rnea.cpp)
PKG_CONFIG_USE_DEPENDENCY(rnea eigenpy)
diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
index cc49ad2cda42dcf55b1f1c954fee144fdadd1322..c21c101d20d68328f0d4a6bad5ad55f812c8e1ba 100644
--- a/src/algorithm/crba.hpp
+++ b/src/algorithm/crba.hpp
@@ -68,57 +68,36 @@ namespace se3
int i)
{
/*
- * F = Yi*Si
- * for j<i
- * F = ljXj.act(F)
- * M.block(j,i,nvj,nvi) = Sj'*F
+ * F[:,i] = Yi*Si
+ * M[i,i:subtree] = Si'*F[:,i:substree]
+ * if li>0:
+ * Yli += liMi.act(Yi)
+ * F[:,i:subtree] = liMi.act(F[:,i:subtree])
*/
- 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;
+ const Model::Index & parent = model.parents[i];
+ const int & nsubtree = data.nvSubtree[i];
- Eigen::Matrix<double,6,JointModel::nv> F;
ConstraintXd S = jdata.S();
- F = data.Ycrb[i].toMatrix() * S.matrix() ;
+ data.Fcrb.block<6,JointModel::nv>(0,jmodel.idx_v()) = data.Ycrb[i].toMatrix() * S.matrix() ;
+
+ data.M.block(jmodel.idx_v(),jmodel.idx_v(),JointModel::nv,nsubtree)
+ = S.matrix().transpose() * data.Fcrb.block(0,jmodel.idx_v(), 6,nsubtree);
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;
+ std::cout << "iFi = " << data.Fcrb.block<6,JointModel::nv>(0,jmodel.idx_v()) << std::endl;
+ std::cout << "F = " << data.Fcrb << std::endl;
+ std::cout << "M = " << data.M << 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)
+ if( 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];
+ data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
+ SE3::Matrix6 ljXj = data.liMi[i];
+ data.Fcrb.block(0,jmodel.idx_v(), 6,nsubtree)
+ = ljXj.transpose().inverse() * data.Fcrb.block(0,jmodel.idx_v(), 6,nsubtree);
}
-
}
};
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index a9f9d25ebaf96bde099dbb52014f83e2771ff62f..f67d2a6f7796029075a0beb9a0f9530ae35ed1c6 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -3,6 +3,7 @@
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
+#include "pinocchio/multibody/force-set.hpp"
namespace se3
{
@@ -62,12 +63,30 @@ namespace se3
template<typename D>
MotionRevolute operator*( const Eigen::MatrixBase<D> & v ) const { return MotionRevolute(v[0]); }
- const ConstraintRevolute & transpose() const { return *this; }
- //template<typename D> D operator*( const Force& f ) const
- Force::Vector3::ConstFixedSegmentReturnType<1>::Type
- operator*( const Force& f ) const
- { return f.angular().segment<1>(axis); }
-
+ struct TransposeConst
+ {
+ const ConstraintRevolute & ref;
+ TransposeConst(const ConstraintRevolute & ref) : ref(ref) {}
+
+ Force::Vector3::ConstFixedSegmentReturnType<1>::Type
+ operator*( const Force& f ) const
+ { return f.angular().segment<1>(axis); }
+
+ Eigen::Block<const typename ForceSet::Matrix3x>
+ operator*( const typename ForceSet::Block & F )
+ {
+ return F.ref.angular().block(axis,F.idx,1,F.len);
+ }
+
+ };
+ TransposeConst transpose() const { return TransposeConst(*this); }
+
+ /* CRBA joint operators
+ * - ForceSet::Block = ForceSet
+ * - ForceSet operator* (Inertia Y,Constraint S)
+ * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
+ * - SE3::act(ForceSet::Block)
+ */
operator ConstraintXd () const
{
Eigen::Matrix<double,6,1> S;
@@ -155,6 +174,25 @@ namespace se3
return R3;
}
+ /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
+ ForceSet operator*( const Inertia& Y,const JointRevolute<0>::ConstraintRevolute & )
+ {
+ /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
+ /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
+ /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */
+ const double
+ &m = Y.mass(),
+ &x = Y.lever()[0],
+ &y = Y.lever()[1],
+ &z = Y.lever()[2];
+ const Inertia::Symmetric3 & I = Y.inertia();
+ return ForceSet( Eigen::Vector3d(0,-m*z,m*y),
+ Eigen::Vector3d(I(0,0)+m*(y*y+z*z),
+ I(0,1)-m*x*y,
+ I(0,2)-m*x*z) );
+ }
+ /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
+ //template<int axis>
template<int axis>
@@ -224,12 +262,15 @@ namespace se3
};
+ typedef JointRevolute<0> JointRX;
typedef JointDataRevolute<0> JointDataRX;
typedef JointModelRevolute<0> JointModelRX;
+ typedef JointRevolute<1> JointRY;
typedef JointDataRevolute<1> JointDataRY;
typedef JointModelRevolute<1> JointModelRY;
+ typedef JointRevolute<2> JointRZ;
typedef JointDataRevolute<2> JointDataRZ;
typedef JointModelRevolute<2> JointModelRZ;
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index de332d43d39a4b7b4068e93a1264c5112812a220..3e7ae192f8588ec213c1f141e12c07458be0743d 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -23,6 +23,50 @@ namespace se3
{ return boost::apply_visitor( CreateJointData(), jmodel ); }
};
+ class Joint_nv: public boost::static_visitor<int>
+ {
+ public:
+ template<typename D>
+ int operator()(const JointModelBase<D> & ) const
+ { return D::nv; }
+
+ static int run( const JointModelVariant & jmodel)
+ { return boost::apply_visitor( Joint_nv(), jmodel ); }
+ };
+
+ class Joint_nq: public boost::static_visitor<int>
+ {
+ public:
+ template<typename D>
+ int operator()(const JointModelBase<D> & ) const
+ { return D::nq; }
+
+ static int run( const JointModelVariant & jmodel)
+ { return boost::apply_visitor( Joint_nq(), jmodel ); }
+ };
+
+ class Joint_idx_q: public boost::static_visitor<int>
+ {
+ public:
+ template<typename D>
+ int operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.idx_q(); }
+
+ static int run( const JointModelVariant & jmodel)
+ { return boost::apply_visitor( Joint_idx_q(), jmodel ); }
+ };
+
+ class Joint_idx_v: public boost::static_visitor<int>
+ {
+ public:
+ template<typename D>
+ int operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.idx_v(); }
+
+ static int run( const JointModelVariant & jmodel)
+ { return boost::apply_visitor( Joint_idx_v(), jmodel ); }
+ };
+
} // namespace se3
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModelVariant);
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index eda2c96f1c82cbb413f1b1e688fa46d1a78898a6..34380b4b2edfb5c67af581989c10f401be6eae23 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -76,6 +76,7 @@ namespace se3
Eigen::MatrixXd 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)
Data( const Model& ref );
@@ -154,6 +155,7 @@ namespace se3
,M(ref.nv,ref.nv)
,Fcrb(6,ref.nv)
,lastChild(ref.nbody)
+ ,nvSubtree(ref.nbody)
{
for(int i=0;i<model.nbody;++i)
joints.push_back(CreateJointData::run(model.joints[i]));
@@ -173,6 +175,10 @@ namespace se3
if(lastChild[i] == -1) lastChild[i] = i;
const Index & parent = model.parents[i];
lastChild[parent] = std::max(lastChild[i],lastChild[parent]);
+
+ nvSubtree[i]
+ = Joint_idx_v::run(model.joints[lastChild[i]]) + Joint_nv::run(model.joints[lastChild[i]])
+ - Joint_idx_v::run(model.joints[i]);
}
}
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index 263a67c629a6bf6171329325a591f543c0b9a5b1..5cadda30f45b7ae9ee1ad72f69ff3517802f2025 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -100,13 +100,13 @@ namespace se3
{
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
+ Matrix3 mmmXX = (m*other.m/mm*X*X).eval(); // 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
+ Matrix3 mmXX = (m*other.m*X*X).eval(); // 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?
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index a2515c9ff3dd2d10593eeda85ae25d721e2be5d5..63e109c47f40bee3c60cf24be12956cfef2239fd 100644
--- a/src/spatial/se3.hpp
+++ b/src/spatial/se3.hpp
@@ -8,6 +8,12 @@
namespace se3
{
+ namespace internal
+ {
+ template<typename D>
+ struct ActionReturn { typedef D Type; };
+ }
+
/** The rigid transform aMb can be seen in two ways:
*
* - given a point p expressed in frame B by its coordinate vector Bp, aMb
@@ -100,9 +106,11 @@ namespace se3
/* --- GROUP ACTIONS ON M6, F6 and I6 --- */
/// ay = aXb.act(by)
- template<typename D> D act (const D & d) const { return d.se3Action(*this); }
+ template<typename D> typename internal::ActionReturn<D>::Type act (const D & d) const
+ { return d.se3Action(*this); }
/// by = aXb.actInv(ay)
- template<typename D> D actInv(const D & d) const { return d.se3ActionInverse(*this); }
+ template<typename D> typename internal::ActionReturn<D>::Type actInv(const D & d) const
+ { return d.se3ActionInverse(*this); }
Vector3 act (const Vector3& p) const { return (rot*p+trans).eval(); }
Vector3 actInv(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }