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
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) );
}
}
};
......
......@@ -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;
......
......@@ -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
}
......
......@@ -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
}
......
......@@ -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)) );
}
......
......@@ -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)
{
......
......@@ -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;
......
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