Commit 79251600 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #405 from jcarpent/topic/ag-variation

Implement time derivative of the centroidal momemtum matrix
parents 7700dee4 5e4ca72a
......@@ -47,6 +47,13 @@ namespace se3
"Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite Rigid Body Inertia, puts the result in Data and returns the centroidal mapping.",
bp::return_value_policy<bp::return_by_value>());
bp::def("dccrba",dccrba,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)"),
"Computes the time derivative of the centroidal momentum matrix Ag in terms of q and v. It computes also the same information than ccrtba for the same price.",
bp::return_value_policy<bp::return_by_value>());
}
} // namespace python
......
......@@ -88,6 +88,8 @@ namespace se3
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag,
"Centroidal matrix which maps from joint velocity to the centroidal momentum.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,dAg,
"Time derivative of the centroidal momentum matrix Ag.")
.ADD_DATA_PROPERTY_READONLY(Force,hg,
"Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame).")
.ADD_DATA_PROPERTY_READONLY(Inertia,Ig,
......
......@@ -74,6 +74,9 @@ namespace se3
.def("se3ActionInverse",&Motion::se3ActionInverse,
bp::args("M"),"Returns the result of the action of the inverse of M on *this.")
.add_property("action",&Motion::toActionMatrix,"Returns the action matrix of *this (acting on Motion).")
.add_property("dualAction",&Motion::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).")
.def("setZero",&MotionPythonVisitor::setZero,
"Set the linear and angular components of *this to zero.")
.def("setRandom",&MotionPythonVisitor::setRandom,
......
......@@ -44,26 +44,40 @@ namespace se3
const Eigen::VectorXd & q);
///
/// \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 Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta
/// according to the current joint configuration and velocity.
///
/// \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>();
///
/// \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).
/// \param[in] v The joint velocity vector (dim model.nv).
///
/// \return The joint space inertia matrix with only the upper triangular part computed.
/// \return The Centroidal Momentum Matrix Ag.
///
inline const Data::Matrix6x &
ccrba(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
///
/// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.
///
/// \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 Centroidal Momentum Matrix time derivative dAg
///
inline const Data::Matrix6x &
dccrba(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
DEFINE_ALGO_CHECKER(CRBA);
......
......@@ -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 ---------------------------------------------------------------
......
......@@ -189,6 +189,13 @@ namespace se3
{
return (m.inverse().toActionMatrix()*S).eval();
}
DenseBase variation(const Motion & v) const
{
DenseBase res(v.toActionMatrix() * S);
return res;
}
void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
......
......@@ -84,6 +84,8 @@ namespace se3
TransposeConst transpose() const { return TransposeConst(); }
operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); }
DenseBase variation(const Motion & m) const { return m.toActionMatrix(); }
}; // struct ConstraintIdentity
template<typename D>
......
......@@ -189,6 +189,20 @@ namespace se3
return X_subspace;
}
DenseBase variation(const Motion & m) const
{
const Motion::ConstLinear_t v = m.linear();
const Motion::ConstAngular_t w = m.angular();
DenseBase res(DenseBase::Zero());
res(0,1) = -w[2]; res(0,2) = v[1];
res(1,0) = w[2]; res(1,2) = -v[0];
res(2,0) = -w[1]; res(2,1) = w[0];
res(3,2) = w[1];
res(4,2) = -w[0];
return res;
}
}; // struct ConstraintPlanar
template<typename D>
......
......@@ -191,6 +191,14 @@ namespace se3
return ConstraintXd(S);
}
DenseBase variation(const Motion & m) const
{
DenseBase res;
res << m.angular().cross(axis), Vector3::Zero();
return res;
}
}; // struct ConstraintPrismaticUnaligned
......
......@@ -196,6 +196,14 @@ namespace se3
S << prismatic::CartesianVector3<axis>(1).vector(), Eigen::Vector3d::Zero();
return ConstraintXd(S);
}
DenseBase variation(const Motion & m) const
{
DenseBase res;
res << m.angular().cross(prismatic::CartesianVector3<axis>(1).vector()), Vector3::Zero();
return res;
}
}; // struct ConstraintPrismatic
......
......@@ -192,6 +192,17 @@ namespace se3
return ConstraintXd(S);
}
DenseBase variation(const Motion & m) const
{
const Motion::ConstLinear_t v = m.linear();
const Motion::ConstAngular_t w = m.angular();
DenseBase res;
res << v.cross(axis), w.cross(axis);
return res;
}
}; // struct ConstraintRevoluteUnaligned
......
......@@ -198,6 +198,18 @@ namespace se3
S << Eigen::Vector3d::Zero(), revolute::CartesianVector3<axis>(1).vector();
return ConstraintXd(S);
}
DenseBase variation(const Motion & m) const
{
const typename Motion::ConstLinear_t v = m.linear();
const typename Motion::ConstAngular_t w = m.angular();
const Vector3 a(revolute::CartesianVector3<axis>(1).vector());
DenseBase res;
res << v.cross(a), w.cross(a);
return res;
}
}; // struct ConstraintRevolute
template<int axis>
......
......@@ -97,6 +97,7 @@ namespace se3
typedef Eigen::Matrix <_Scalar,3,3,_Options> Matrix3;
typedef Eigen::Matrix <_Scalar,3,1,_Options> Vector3;
typedef Eigen::Matrix <_Scalar,6,3,_Options> ConstraintDense;
typedef Eigen::Matrix <_Scalar,6,3,_Options> DenseBase;
Matrix3 S_minimal;
......@@ -187,6 +188,18 @@ namespace se3
return result;
}
DenseBase variation(const Motion & m) const
{
const typename Motion::ConstLinear_t v = m.linear();
const typename Motion::ConstAngular_t w = m.angular();
DenseBase res;
res.template middleRows<3>(Motion::LINEAR) = cross(v,S_minimal);
res.template middleRows<3>(Motion::ANGULAR) = cross(w,S_minimal);
return res;
}
}; // struct ConstraintRotationalSubspace
......@@ -246,6 +259,13 @@ namespace se3
{
return Y.template block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal;
}
inline Eigen::Matrix<double,6,3>
operator*(const Inertia::Matrix6 & Y,
const JointSphericalZYX::ConstraintRotationalSubspace & S)
{
return Y.block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal;
}
namespace internal
{
......
......@@ -156,11 +156,23 @@ namespace se3
Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
{
Eigen::Matrix <double,6,3> X_subspace;
X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
X_subspace.block <3,3> (Motion::LINEAR, 0) = cross(m.translation(),m.rotation ());
X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
return X_subspace;
}
DenseBase variation(const Motion & m) const
{
const Motion::ConstLinear_t v = m.linear();
const Motion::ConstAngular_t w = m.angular();
DenseBase res;
res.middleRows<3>(LINEAR) = skew(v);
res.middleRows<3>(ANGULAR) = skew(w);
return res;
}
}; // struct ConstraintRotationalSubspace
......
......@@ -172,6 +172,17 @@ namespace se3
return M;
}
DenseBase variation(const Motion & m) const
{
const Motion::ConstAngular_t w = m.angular();
DenseBase res;
res.middleRows<3>(LINEAR) = skew(w);
res.middleRows<3>(ANGULAR).setZero();
return res;
}
}; // struct ConstraintTranslationSubspace
......
......@@ -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,18 +413,23 @@ 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
// CCRBA return quantities
/// \brief Centroidal Momentum Matrix
/// \note \f$ hg = Ag \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
/// \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.
/// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
///
Force hg;
......
......@@ -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
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -61,6 +61,7 @@ namespace se3
Force operator*(const Motion & v) const { return derived().__mult__(v); }
Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); }
Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); }
void setZero() { derived().setZero(); }
void setIdentity() { derived().setIdentity(); }
......@@ -119,6 +120,8 @@ namespace se3
SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
public:
// Constructors
InertiaTpl() : m(), c(), I() {}
......@@ -230,12 +233,12 @@ namespace se3
Matrix6 matrix_impl() const
{
Matrix6 M;
const Matrix3 & c_cross = (skew(c));
M.template block<3,3>(LINEAR, LINEAR ).setZero ();
M.template block<3,3>(LINEAR, LINEAR ).diagonal ().fill (m);