Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
src/algorithm/crba.hpp
View file @ 8b9ce39f
......@@ -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) );
}
}
};
......
src/algorithm/rnea.hpp
View file @ 8b9ce39f
......@@ -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;
......
src/multibody/force-set.hpp
View file @ 8b9ce39f
......@@ -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
}
......
src/multibody/joint/joint-free-flyer.hpp
View file @ 8b9ce39f
......@@ -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
}
......
src/multibody/joint/joint-revolute.hpp
View file @ 8b9ce39f
......@@ -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)) );
}
......
src/multibody/model.hpp
View file @ 8b9ce39f
......@@ -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)
{
......
unittest/crba.cpp
View file @ 8b9ce39f
......@@ -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