Commit a50e7131 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add algo to compute the time derivative of the Centroidal Momentum Matrix

parent 31562102
......@@ -62,23 +62,19 @@ namespace se3
const Eigen::VectorXd & v);
///
/// \brief Computes the upper triangular part of the joint space inertia matrix M by
/// using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008).
/// The result is accessible through data.M.
/// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.
///
/// \note You can easly get data.M symetric by copying the stricly upper trinangular part
/// in the stricly lower tringular part with
/// data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
/// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$ \dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\f$.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] v The joint configuration vector (dim model.nv).
///
/// \return The joint space inertia matrix with only the upper triangular part computed.
/// \return The Centroidal Momentum Matrix time derivative dAg
///
inline const Data::Matrix6x &
ccrba(const Model & model,
dccrba(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
......
......@@ -179,25 +179,25 @@ namespace se3
forceSet::se3Action(data.oMi[i],jdata.U(),jF);
}
static void algo(const se3::JointModelBase<JointModelComposite> & jmodel,
se3::JointDataBase<JointDataComposite> & jdata,
const se3::Model & model,
se3::Data & data)
{
typedef SizeDepType<JointModel::NV>::ColsReturn<Data::Matrix6x>::Type ColsBlock;
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
jdata.U() = data.Ycrb[i] * jdata.S();
ColsBlock jF
= data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
forceSet::se3Action(data.oMi[i],jdata.U(),jF);
}
// static void algo(const se3::JointModelBase<JointModelComposite> & jmodel,
// se3::JointDataBase<JointDataComposite> & jdata,
// const se3::Model & model,
// se3::Data & data)
// {
// typedef SizeDepType<JointModel::NV>::ColsReturn<Data::Matrix6x>::Type ColsBlock;
//
// const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
// const Model::Index & parent = model.parents[i];
//
// data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
//
// jdata.U() = data.Ycrb[i] * jdata.S();
//
// ColsBlock jF
// = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
//
// forceSet::se3Action(data.oMi[i],jdata.U(),jF);
// }
}; // struct CcrbaBackwardStep
......@@ -235,6 +235,174 @@ namespace se3
return data.Ag;
}
struct DCcrbaForwardStep : public fusion::JointVisitor<DCcrbaForwardStep>
{
typedef boost::fusion::vector< const se3::Model &,
se3::Data &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(DCcrbaForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::Model & model,
se3::Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
jmodel.calc(jdata.derived(),q,v);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.Ycrb[i] = model.inertias[i];
data.v[i] = jdata.v();
if (parent>0)
{
data.oMi[i] = data.oMi[parent]*data.liMi[i];
data.v[i] += data.liMi[i].actInv(data.v[parent]);
}
else data.oMi[i] = data.liMi[i];
}
}; // struct DCcrbaForwardStep
struct DCcrbaBackwardStep : public fusion::JointVisitor<DCcrbaBackwardStep>
{
typedef boost::fusion::vector< const se3::Model &,
se3::Data &
> ArgsType;
JOINT_VISITOR_INIT(DCcrbaBackwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::Model & model,
se3::Data & data)
{
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
typedef typename JointModel::JointDataDerived JointData;
typedef typename JointModel::Constraint_t Constraint_t;
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
const Motion & v = data.v[i];
const Inertia & Y = data.Ycrb[i];
const Inertia::Matrix6 & dY = data.dYcrb[i];
data.Ycrb[parent] += data.liMi[i].act(Y);
data.dYcrb[parent] += SE3actOn(data.liMi[i],dY);
// Calc Ag
ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
jdata.U() = Y * jdata.S();
forceSet::se3Action(data.oMi[i],jdata.U(),Ag_cols);
// Calc dAg = oXi* dU
typename JointData::U_t dU(dY * jdata.S()); // TODO: add dU inside jdata.
typename Constraint_t::DenseBase dS = jdata.S().variation(v);
dU.noalias() += Y.matrix()*dS;
ColsBlock dAg_cols = jmodel.jointCols(data.dAg);
forceSet::se3Action(data.oMi[i],dU,dAg_cols);
}
inline static Inertia::Matrix6 SE3actOn(const SE3 & M, const Inertia::Matrix6 & I)
{
typedef Inertia::Matrix6 Matrix6;
typedef SE3::Matrix3 Matrix3;
typedef SE3::Vector3 Vector3;
typedef Eigen::Block<const Matrix6,3,3> constBlock3;
typedef Eigen::Block<Matrix6,3,3> Block3;
const constBlock3 & Ai = I.block<3,3> (Inertia::LINEAR, Inertia::LINEAR);
const constBlock3 & Bi = I.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR);
const constBlock3 & Di = I.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR);
const Matrix3 & R = M.rotation();
const Vector3 & t = M.translation();
Matrix6 res;
Block3 Ao = res.block<3,3> (Inertia::LINEAR, Inertia::LINEAR);
Block3 Bo = res.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR);
Block3 Co = res.block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
Block3 Do = res.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR);
Ao = R*Ai*R.transpose();
Bo = R*Bi*R.transpose();
Do.row(0) = t.cross(Bo.col(0));
Do.row(1) = t.cross(Bo.col(1));
Do.row(2) = t.cross(Bo.col(2));
Co.col(0) = t.cross(Ao.col(0));
Co.col(1) = t.cross(Ao.col(1));
Co.col(2) = t.cross(Ao.col(2));
Co += Bo.transpose();
Bo = Co.transpose();
Do.col(0) += t.cross(Bo.col(0));
Do.col(1) += t.cross(Bo.col(1));
Do.col(2) += t.cross(Bo.col(2));
Do += R*Di*R.transpose();
return res;
}
}; // struct DCcrbaBackwardStep
inline const Data::Matrix6x &
dccrba(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
assert(model.check(data) && "data is not consistent with model.");
typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x;
forwardKinematics(model,data,q,v);
data.Ycrb[0].setZero();
for(Model::Index i=1;i<(Model::Index)(model.njoints);++i)
{
data.Ycrb[i] = model.inertias[i];
data.dYcrb[i] = model.inertias[i].variation(data.v[i]); // (vx* I - Ivx)
}
for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
{
DCcrbaBackwardStep::run(model.joints[i],data.joints[i],
DCcrbaBackwardStep::ArgsType(model,data));
}
data.com[0] = data.Ycrb[0].lever();
const Block3x Ag_lin = data.Ag.middleRows<3> (Force::LINEAR);
Block3x Ag_ang = data.Ag.middleRows<3> (Force::ANGULAR);
for (long i = 0; i<model.nv; ++i)
Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
data.hg = data.Ag*v;
data.vcom[0] = data.hg.linear()/data.Ycrb[0].mass();
const Block3x dAg_lin = data.dAg.middleRows<3>(Force::LINEAR);
Block3x dAg_ang = data.dAg.middleRows<3>(Force::ANGULAR);
for (long i = 0; i<model.nv; ++i)
{
dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]);// + Ag_lin.col(i).cross(data.vcom[0]);
}
data.Ig.mass() = data.Ycrb[0].mass();
data.Ig.lever().setZero();
data.Ig.inertia() = data.Ycrb[0].inertia();
return data.dAg;
}
// --- CHECKER ---------------------------------------------------------------
// --- CHECKER ---------------------------------------------------------------
......
......@@ -402,6 +402,9 @@ namespace se3
/// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.
container::aligned_vector<Inertia> Ycrb;
/// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}$\f. See Data::Ycrb for more details.
container::aligned_vector<Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
/// \brief The joint space inertia matrix (a square matrix of dim model.nv).
Eigen::MatrixXd M;
......@@ -410,7 +413,7 @@ namespace se3
// ABA internal data
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
container::aligned_vector<Inertia::Matrix6> Yaba;
container::aligned_vector<Inertia::Matrix6> Yaba; // TODO: change with dense symmetric matrix6
/// \brief Intermediate quantity corresponding to apparent torque [ABA]
Eigen::VectorXd u; // Joint Inertia
......@@ -420,6 +423,11 @@ namespace se3
/// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
Matrix6x Ag;
// dCCRBA return quantities
/// \brief Centroidal Momentum Matrix Time Variation
/// \note \f$ \dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}f$ maps the joint velocity and acceleration vectors to the time variation of the centroidal momentum.
Matrix6x dAg;
/// \brief Centroidal momentum quantity.
/// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
///
......
......@@ -238,11 +238,13 @@ namespace se3
,nle(model.nv)
,oMf((std::size_t)model.nframes)
,Ycrb((std::size_t)model.njoints)
,dYcrb((std::size_t)model.njoints)
,M(model.nv,model.nv)
,ddq(model.nv)
,Yaba((std::size_t)model.njoints)
,u(model.nv)
,Ag(6,model.nv)
,dAg(6,model.nv)
,Fcrb((std::size_t)model.njoints)
,lastChild((std::size_t)model.njoints)
,nvSubtree((std::size_t)model.njoints)
......
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -127,6 +127,56 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
se3::Data::Matrix6x Ag_ref (cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows <6> ());
BOOST_CHECK(data.Ag.isApprox(Ag_ref,1e-12));
}
BOOST_AUTO_TEST_CASE (test_dccrb)
{
using namespace se3;
Model model;
buildModels::humanoidSimple(model);
Data data(model), data_ref(model);
Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
q.segment <4> (3).normalize();
Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv);
Eigen::VectorXd a = Eigen::VectorXd::Ones(model.nv);
const Eigen::VectorXd g = rnea(model,data_ref,q,0*v,0*a);
rnea(model,data_ref,q,v,a);
crba(model,data_ref,q);
data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
SE3::Vector3 com = data_ref.Ycrb[1].lever();
SE3 cMo(SE3::Identity());
cMo.translation() = -getComFromCrba(model, data_ref);
SE3 oM1 (data_ref.liMi[1]);
SE3 cM1 (cMo * oM1);
Data::Matrix6x Ag_ref (cM1.toDualActionMatrix() * data_ref.M.topRows <6> ());
Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>())));
ccrba(model,data_ref,q,v);
dccrba(model,data,q,0*v);
BOOST_CHECK(data.Ag.isApprox(Ag_ref));
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
BOOST_CHECK(data.dAg.isZero());
centerOfMass(model,data_ref,q,v,a);
BOOST_CHECK(data_ref.vcom[0].isApprox(data_ref.hg.linear()/data_ref.M(0,0)));
BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear()/data_ref.M(0,0)));
dccrba(model,data,q,v);
Force::Vector6 test1(data.Ag * a);
Force::Vector6 test2(data.dAg * v);
Force hdot(data.Ag * a + data.dAg * v);
BOOST_CHECK(hdot.isApprox(hdot_ref));
}
BOOST_AUTO_TEST_SUITE_END ()
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