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

Working CRBA with simple bwd loop.

parent dd4a37ae
......@@ -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)
......
......@@ -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);
}
}
};
......
......@@ -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
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;
......
......@@ -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);
......
......@@ -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]);
}
}
......
......@@ -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?
......
......@@ -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(); }
......
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