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

Working version of CRBA with ForceSet ... but 50us.

parent 5e0d90d4
...@@ -41,12 +41,7 @@ namespace se3 ...@@ -41,12 +41,7 @@ namespace se3
jmodel.calc(jdata.derived(),q); jmodel.calc(jdata.derived(),q);
const Model::Index & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i]*jdata.M(); 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]; data.Ycrb[i] = model.inertias[i];
} }
...@@ -78,25 +73,22 @@ namespace se3 ...@@ -78,25 +73,22 @@ namespace se3
const Model::Index & parent = model.parents[i]; const Model::Index & parent = model.parents[i];
const int & nsubtree = data.nvSubtree[i]; const int & nsubtree = data.nvSubtree[i];
ConstraintXd S = jdata.S(); data.Fcrb.block(jmodel.idx_v(),JointModel::nv) = data.Ycrb[i] * jdata.S();
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) 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 << "*** joint " << i << std::endl;
std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl; // std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
std::cout << "iSi = " << S.matrix() << std::endl; // std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
std::cout << "iFi = " << data.Fcrb.block<6,JointModel::nv>(0,jmodel.idx_v()) << std::endl; // std::cout << "iF = " << data.Fcrb.matrix() << std::endl;
std::cout << "F = " << data.Fcrb << std::endl; // std::cout << "M = " << data.M << std::endl;
std::cout << "M = " << data.M << std::endl;
if( parent>0 ) if( parent>0 )
{ {
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
SE3::Matrix6 ljXj = data.liMi[i]; data.Fcrb.block(jmodel.idx_v(), nsubtree) =
data.Fcrb.block(0,jmodel.idx_v(), 6,nsubtree) data.liMi[i].act( data.Fcrb.block(jmodel.idx_v(), nsubtree) );
= ljXj.transpose().inverse() * data.Fcrb.block(0,jmodel.idx_v(), 6,nsubtree);
} }
} }
}; };
......
...@@ -53,8 +53,7 @@ namespace se3 ...@@ -53,8 +53,7 @@ namespace se3
data.v[i] = jdata.v(); data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]); 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()) ; data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
// if(parent>0)
data.a[i] += data.liMi[i].actInv(data.a[parent]); 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 data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
...@@ -102,7 +101,7 @@ namespace se3 ...@@ -102,7 +101,7 @@ namespace se3
for( int i=model.nbody-1;i>0;--i ) for( int i=model.nbody-1;i>0;--i )
{ {
RneaBackwardStep::run(model.joints[i],data.joints[i], RneaBackwardStep::run(model.joints[i],data.joints[i],
RneaBackwardStep::ArgsType(model,data,i)); RneaBackwardStep::ArgsType(model,data,i));
} }
return data.tau; return data.tau;
......
...@@ -74,38 +74,51 @@ namespace se3 ...@@ -74,38 +74,51 @@ namespace se3
Block( ForceSetTpl & ref, const int & idx, const int & len ) Block( ForceSetTpl & ref, const int & idx, const int & len )
: ref(ref), idx(idx), len(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) Block& operator= (const ForceSetTpl & copy)
{ {
assert(copy.size == len); assert(copy.size == len);
ref.m_f.block(0,idx,3,len) = copy.m_f; linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
ref.m_n.block(0,idx,3,len) = copy.m_n; angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
return *this; return *this;
} }
Block& operator= (const ForceSetTpl::Block & copy) Block& operator= (const ForceSetTpl::Block & copy)
{ {
assert(copy.len == len); assert(copy.len == len);
ref.m_f.block(0,idx,3,len) = copy.ref.m_f.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);
ref.m_n.block(0,idx,3,len) = copy.ref.m_n.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; return *this;
} }
/// af = aXb.act(bf) /// af = aXb.act(bf)
ForceSetTpl se3Action(const SE3 & m) const ForceSetTpl se3Action(const SE3 & m) const
{ {
const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len); // 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); // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
Matrix3x Rf = (m.rotation()*linear).eval(); Matrix3x Rf = (m.rotation()*linear()).eval();
return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular); return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
// TODO check if nothing better than explicitely calling skew // TODO check if nothing better than explicitely calling skew
} }
/// bf = aXb.actInv(af) /// bf = aXb.actInv(af)
ForceSetTpl se3ActionInverse(const SE3 & m) const ForceSetTpl se3ActionInverse(const SE3 & m) const
{ {
const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len); // 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); // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
return ForceSetTpl(m.rotation().transpose()*linear, return ForceSetTpl(m.rotation().transpose()*linear(),
m.rotation().transpose()*(angular - skew(m.translation())*linear) ); m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
// TODO check if nothing better than explicitely calling skew // TODO check if nothing better than explicitely calling skew
} }
......
...@@ -44,10 +44,10 @@ namespace se3 ...@@ -44,10 +44,10 @@ namespace se3
} }
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
const ForceSet::Block & ForceSet::Matrix6x
operator*( const JointFreeFlyer::ConstraintIdentity &, const ForceSet::Block & F ) operator*( const JointFreeFlyer::ConstraintIdentity &, const ForceSet::Block & F )
{ {
return F; return F.matrix(); // TODO try to avoid creation of a new MatrixXd
} }
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include "pinocchio/multibody/joint/joint-base.hpp" #include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp" #include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/force-set.hpp" #include "pinocchio/multibody/force-set.hpp"
namespace se3 namespace se3
...@@ -186,9 +187,9 @@ namespace se3 ...@@ -186,9 +187,9 @@ namespace se3
&z = Y.lever()[2]; &z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia(); const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(0,-m*z,m*y), return ForceSet( Eigen::Vector3d(0,-m*z,m*y),
Eigen::Vector3d(I(0,0)+m*(y*y+z*z), Eigen::Vector3d(I(0,0)+m*(y*y+z*z),
I(0,1)-m*x*y, I(0,1)-m*x*y,
I(0,2)-m*x*z) ); I(0,2)-m*x*z) );
} }
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointRevolute<1>::ConstraintRevolute & ) ForceSet operator*( const Inertia& Y,const JointRevolute<1>::ConstraintRevolute & )
...@@ -201,9 +202,9 @@ namespace se3 ...@@ -201,9 +202,9 @@ namespace se3
&z = Y.lever()[2]; &z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia(); const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(m*z,0,-m*x), return ForceSet( Eigen::Vector3d(m*z,0,-m*x),
Eigen::Vector3d(I(1,0)-m*x*y, Eigen::Vector3d(I(1,0)-m*x*y,
I(1,1)+m*(x*x+z*z), I(1,1)+m*(x*x+z*z),
I(1,2)-m*y*z) ); I(1,2)-m*y*z) );
} }
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
ForceSet operator*( const Inertia& Y,const JointRevolute<2>::ConstraintRevolute & ) ForceSet operator*( const Inertia& Y,const JointRevolute<2>::ConstraintRevolute & )
...@@ -216,9 +217,9 @@ namespace se3 ...@@ -216,9 +217,9 @@ namespace se3
&z = Y.lever()[2]; &z = Y.lever()[2];
const Inertia::Symmetric3 & I = Y.inertia(); const Inertia::Symmetric3 & I = Y.inertia();
return ForceSet( Eigen::Vector3d(-m*y,m*x,0), return ForceSet( Eigen::Vector3d(-m*y,m*x,0),
Eigen::Vector3d(I(2,0)-m*x*z, Eigen::Vector3d(I(2,0)-m*x*z,
I(2,1)-m*y*z, I(2,1)-m*y*z,
I(2,2)+m*(x*x+y*y)) ); I(2,2)+m*(x*x+y*y)) );
} }
......
...@@ -8,6 +8,7 @@ ...@@ -8,6 +8,7 @@
#include "pinocchio/spatial/motion.hpp" #include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp" #include "pinocchio/spatial/force.hpp"
#include "pinocchio/multibody/joint.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::SE3);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia); EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia);
...@@ -73,7 +74,7 @@ namespace se3 ...@@ -73,7 +74,7 @@ namespace se3
std::vector<Inertia> Ycrb; // Inertia of the sub-tree composit rigid body std::vector<Inertia> Ycrb; // Inertia of the sub-tree composit rigid body
Eigen::MatrixXd M; // Joint Inertia 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<Model::Index> lastChild; // Index of the last child (for CRBA)
std::vector<int> nvSubtree; // Dimension of the subtree motion space (for CRBA) std::vector<int> nvSubtree; // Dimension of the subtree motion space (for CRBA)
...@@ -153,7 +154,7 @@ namespace se3 ...@@ -153,7 +154,7 @@ namespace se3
,tau(ref.nv) ,tau(ref.nv)
,Ycrb(ref.nbody) ,Ycrb(ref.nbody)
,M(ref.nv,ref.nv) ,M(ref.nv,ref.nv)
,Fcrb(6,ref.nv) ,Fcrb(ref.nv)
,lastChild(ref.nbody) ,lastChild(ref.nbody)
,nvSubtree(ref.nbody) ,nvSubtree(ref.nbody)
{ {
......
...@@ -32,9 +32,9 @@ int main(int argc, const char ** argv) ...@@ -32,9 +32,9 @@ int main(int argc, const char ** argv)
se3::Model model; se3::Model model;
// std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf"; std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
// if(argc>1) filename = argv[1]; if(argc>1) filename = argv[1];
// model = se3::buildModel(filename); model = se3::buildModel(filename);
// SIMPLE no FF // SIMPLE no FF
...@@ -48,15 +48,15 @@ int main(int argc, const char ** argv) ...@@ -48,15 +48,15 @@ int main(int argc, const char ** argv)
//SIMPLE with FF //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("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("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("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("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("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("lleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg3");
se3::Data data(model); se3::Data data(model);
...@@ -64,7 +64,7 @@ int main(int argc, const char ** argv) ...@@ -64,7 +64,7 @@ int main(int argc, const char ** argv)
VectorXd q = VectorXd::Zero(model.nq); VectorXd q = VectorXd::Zero(model.nq);
StackTicToc timer(StackTicToc::US); timer.tic(); StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(1) SMOOTH(1000)
{ {
crba(model,data,q); crba(model,data,q);
} }
...@@ -82,7 +82,6 @@ int main(int argc, const char ** argv) ...@@ -82,7 +82,6 @@ int main(int argc, const char ** argv)
for(int i=0;i<model.nv;++i) for(int i=0;i<model.nv;++i)
{ {
M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias; M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
} }
std::cout << "Mrne = [ " << M << " ]; " << std::endl; std::cout << "Mrne = [ " << 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