diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
index c21c101d20d68328f0d4a6bad5ad55f812c8e1ba..471ec71b9327afa6bc6b7657e7259c365d0f51bf 100644
--- a/src/algorithm/crba.hpp
+++ b/src/algorithm/crba.hpp
@@ -41,12 +41,7 @@ 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];
}
@@ -78,25 +73,22 @@ namespace se3
const Model::Index & parent = model.parents[i];
const int & nsubtree = data.nvSubtree[i];
- ConstraintXd S = jdata.S();
- data.Fcrb.block<6,JointModel::nv>(0,jmodel.idx_v()) = data.Ycrb[i].toMatrix() * S.matrix() ;
+ data.Fcrb.block(jmodel.idx_v(),JointModel::nv) = data.Ycrb[i] * jdata.S();
data.M.block(jmodel.idx_v(),jmodel.idx_v(),JointModel::nv,nsubtree)
- = S.matrix().transpose() * data.Fcrb.block(0,jmodel.idx_v(), 6,nsubtree);
+ = jdata.S().transpose() * data.Fcrb.block(jmodel.idx_v(),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 = " << 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;
+ // std::cout << "*** joint " << i << std::endl;
+ // std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
+ // std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
+ // std::cout << "iF = " << data.Fcrb.matrix() << std::endl;
+ // std::cout << "M = " << data.M << std::endl;
if( parent>0 )
{
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);
+ data.Fcrb.block(jmodel.idx_v(), nsubtree) =
+ data.liMi[i].act( data.Fcrb.block(jmodel.idx_v(), nsubtree) );
}
}
};
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
index 5e10b667047d9c91735c02e1457623de54bbf58b..7c9d5181fa4f5fab6f42fd36f4af449c991a0710 100644
--- a/src/algorithm/rnea.hpp
+++ b/src/algorithm/rnea.hpp
@@ -53,8 +53,7 @@ namespace se3
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
- data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
- // if(parent>0)
+ data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
@@ -102,7 +101,7 @@ namespace se3
for( int i=model.nbody-1;i>0;--i )
{
RneaBackwardStep::run(model.joints[i],data.joints[i],
- RneaBackwardStep::ArgsType(model,data,i));
+ RneaBackwardStep::ArgsType(model,data,i));
}
return data.tau;
diff --git a/src/multibody/force-set.hpp b/src/multibody/force-set.hpp
index 9d934f115ceece0c73bb06c74da2c155bc4bbe5b..0be8a2f2f9e0418254109b6dfa35c6991aabcdd7 100644
--- a/src/multibody/force-set.hpp
+++ b/src/multibody/force-set.hpp
@@ -74,38 +74,51 @@ namespace se3
Block( ForceSetTpl & ref, const int & idx, const int & len )
: ref(ref), idx(idx), len(len) {}
+ Eigen::Block<ForceSetTpl::Matrix3x> linear() { return ref.m_f.block(0,idx,3,len); }
+ Eigen::Block<ForceSetTpl::Matrix3x> angular() { return ref.m_n.block(0,idx,3,len); }
+ Eigen::Block<const ForceSetTpl::Matrix3x> linear() const
+ { return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0,idx,3,len); }
+ Eigen::Block<const ForceSetTpl::Matrix3x> angular() const
+ { return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0,idx,3,len); }
+
+ ForceSetTpl::Matrix6x matrix() const
+ {
+ ForceSetTpl::Matrix6x res(6,len); res << linear(),angular();
+ return res;
+ }
+
Block& operator= (const ForceSetTpl & copy)
{
assert(copy.size == len);
- ref.m_f.block(0,idx,3,len) = copy.m_f;
- ref.m_n.block(0,idx,3,len) = copy.m_n;
+ linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
+ angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
return *this;
}
Block& operator= (const ForceSetTpl::Block & copy)
{
assert(copy.len == len);
- ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
- ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
+ linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
+ angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
return *this;
}
/// af = aXb.act(bf)
ForceSetTpl se3Action(const SE3 & m) const
{
- const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
- const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
- Matrix3x Rf = (m.rotation()*linear).eval();
- return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular);
+ // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
+ // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
+ Matrix3x Rf = (m.rotation()*linear()).eval();
+ return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
// TODO check if nothing better than explicitely calling skew
}
/// bf = aXb.actInv(af)
ForceSetTpl se3ActionInverse(const SE3 & m) const
{
- const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
- const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
- return ForceSetTpl(m.rotation().transpose()*linear,
- m.rotation().transpose()*(angular - skew(m.translation())*linear) );
+ // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
+ // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
+ return ForceSetTpl(m.rotation().transpose()*linear(),
+ m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
// TODO check if nothing better than explicitely calling skew
}
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index a6a7b38b776fabef7500c9b70edd11aa4ff1d143..8ea6a0a3e9fd35ccce1988d545e6663bdd4e9271 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -44,10 +44,10 @@ namespace se3
}
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
- const ForceSet::Block &
+ ForceSet::Matrix6x
operator*( const JointFreeFlyer::ConstraintIdentity &, const ForceSet::Block & F )
{
- return F;
+ return F.matrix(); // TODO try to avoid creation of a new MatrixXd
}
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index acba73aac7d38cecdb43561dc2a9fc8169c7e134..90d092e28ae86d4dbbd3fbc862e1728aaebe458a 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/spatial/inertia.hpp"
#include "pinocchio/multibody/force-set.hpp"
namespace se3
@@ -186,9 +187,9 @@ namespace se3
&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) );
+ Eigen::Vector3d(I(0,0)+m*(y*y+z*z),
+ I(0,1)-m*x*y,
+ I(0,2)-m*x*z) );
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointRevolute<1>::ConstraintRevolute & )
@@ -201,9 +202,9 @@ namespace se3
&z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(m*z,0,-m*x),
- Eigen::Vector3d(I(1,0)-m*x*y,
- I(1,1)+m*(x*x+z*z),
- I(1,2)-m*y*z) );
+ Eigen::Vector3d(I(1,0)-m*x*y,
+ I(1,1)+m*(x*x+z*z),
+ I(1,2)-m*y*z) );
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointRevolute<2>::ConstraintRevolute & )
@@ -216,9 +217,9 @@ namespace se3
&z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(-m*y,m*x,0),
- Eigen::Vector3d(I(2,0)-m*x*z,
- I(2,1)-m*y*z,
- I(2,2)+m*(x*x+y*y)) );
+ Eigen::Vector3d(I(2,0)-m*x*z,
+ I(2,1)-m*y*z,
+ I(2,2)+m*(x*x+y*y)) );
}
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 34380b4b2edfb5c67af581989c10f401be6eae23..4f278e9db314bfd55d01297b192c594726b7e14c 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -8,6 +8,7 @@
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/force-set.hpp"
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia);
@@ -73,7 +74,7 @@ namespace se3
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
+ ForceSet 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)
@@ -153,7 +154,7 @@ namespace se3
,tau(ref.nv)
,Ycrb(ref.nbody)
,M(ref.nv,ref.nv)
- ,Fcrb(6,ref.nv)
+ ,Fcrb(ref.nv)
,lastChild(ref.nbody)
,nvSubtree(ref.nbody)
{
diff --git a/unittest/crba.cpp b/unittest/crba.cpp
index 410e269ae74fc5fb29ff53d899ea54c5efd6cf12..d2ad31f6028b5b6ead616338e84f1f12f8fa18a3 100644
--- a/unittest/crba.cpp
+++ b/unittest/crba.cpp
@@ -32,9 +32,9 @@ int main(int argc, const char ** argv)
se3::Model model;
- // std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
- // if(argc>1) filename = argv[1];
- // model = se3::buildModel(filename);
+ std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
+ if(argc>1) filename = argv[1];
+ model = se3::buildModel(filename);
// SIMPLE no FF
@@ -48,15 +48,15 @@ int main(int argc, const char ** argv)
//SIMPLE with FF
- model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"root");
+ // 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(),"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");
+ // 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);
@@ -64,7 +64,7 @@ int main(int argc, const char ** argv)
VectorXd q = VectorXd::Zero(model.nq);
StackTicToc timer(StackTicToc::US); timer.tic();
- SMOOTH(1)
+ SMOOTH(1000)
{
crba(model,data,q);
}
@@ -82,7 +82,6 @@ int main(int argc, const char ** argv)
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;