Skip to content
GitLab
Commit b35c281c authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add algo to compute the inverse of the joint space inertia matrix

parent 6db2169c
Hide whitespace changes
Inline Side-by-side
src/algorithm/aba.hpp
View file @ b35c281c
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -60,6 +60,20 @@ namespace se3
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
const container::aligned_vector<Force> & fext);
///
/// \brief Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
///
/// \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).
///
/// \return The inverse of the joint space inertia matrix stored in data.ddq.
///
inline const Data::RowMatrixXd &
computeMinverse(const Model & model,
Data & data,
const Eigen::VectorXd & q);
DEFINE_ALGO_CHECKER(ABA);
......
src/algorithm/aba.hxx
View file @ b35c281c
......@@ -18,6 +18,7 @@
#ifndef __se3_aba_hxx__
#define __se3_aba_hxx__
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
......@@ -257,8 +258,174 @@ namespace se3
return data.ddq;
}
struct computeMinverseForwardStep1 : public fusion::JointVisitor<computeMinverseForwardStep1>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(computeMinverseForwardStep1);
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 Model::JointIndex & i = jmodel.id();
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];
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock J_cols = jmodel.jointCols(data.J);
J_cols = data.oMi[i].act(jdata.S());
data.Yaba[i] = model.inertias[i].matrix();
}
};
struct computeMinverseBackwardStep : public fusion::JointVisitor<computeMinverseBackwardStep>
{
typedef boost::fusion::vector<const Model &,
Data &> ArgsType;
JOINT_VISITOR_INIT(computeMinverseBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointDataDerived> & jdata,
const Model & model,
Data & data)
{
const Model::JointIndex & i = jmodel.id();
const Model::Index & parent = model.parents[i];
Inertia::Matrix6 & Ia = data.Yaba[i];
Data::RowMatrixXd & Minv = data.Minv;
Data::Matrix6x & Fcrb = data.Fcrb[0];
Data::Matrix6x & FcrbTmp = data.Fcrb.back();
Data::RowMatrix6 & M6tmpR = data.M6tmpR;
jmodel.calc_aba(jdata.derived(), Ia, parent > 0);
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::RowMatrix6>::Type RowColsBlock;
// RowColsBlock U_cols = M6tmpR.leftCols<JointModel::NV>(jmodel.nv());
ColsBlock U_cols = jmodel.jointCols(data.IS);
forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame
Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),jmodel.nv()) = jdata.Dinv();
const int nv_children = data.nvSubtree[i] - jmodel.nv();
if(nv_children > 0)
{
ColsBlock J_cols = jmodel.jointCols(data.J);
ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
SDinv_cols.noalias() = J_cols * jdata.Dinv();
Minv.block(jmodel.idx_v(),jmodel.idx_v()+jmodel.nv(),jmodel.nv(),nv_children).noalias()
= -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v()+jmodel.nv(),nv_children);
if(parent > 0)
{
FcrbTmp.leftCols(data.nvSubtree[i]).noalias()
= U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);
Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) += FcrbTmp.leftCols(data.nvSubtree[i]);
// Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i])
// += U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);
}
}
else
{
Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias()
= U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);
}
if(parent > 0)
data.Yaba[parent] += AbaBackwardStep::SE3actOn(data.liMi[i], Ia);
}
};
struct computeMinverseForwardStep2 : public fusion::JointVisitor<computeMinverseForwardStep2>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &
> ArgsType;
JOINT_VISITOR_INIT(computeMinverseForwardStep2);
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 Model::JointIndex & i = jmodel.id();
const Model::Index & parent = model.parents[i];
Data::RowMatrixXd & Minv = data.Minv;
Data::Matrix6x & Fcrb = data.Fcrb[0];
Data::Matrix6x & FcrbTmp = data.Fcrb.back();
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv);
forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame
ColsBlock J_cols = jmodel.jointCols(data.J);
if(parent > 0)
{
FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias()
= UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v())
-= FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v());
// Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v())
// -= UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
}
data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() = J_cols * Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v());
if(parent > 0)
data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
}
};
inline const Data::RowMatrixXd &
computeMinverse(const Model & model,
Data & data,
const Eigen::VectorXd & q)
{
assert(model.check(data) && "data is not consistent with model.");
for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
{
computeMinverseForwardStep1::run(model.joints[i],data.joints[i],
computeMinverseForwardStep1::ArgsType(model,data,q));
}
data.Fcrb[0].setZero();
for( Model::Index i=(Model::Index)model.njoints-1;i>0;--i )
{
computeMinverseBackwardStep::run(model.joints[i],data.joints[i],
computeMinverseBackwardStep::ArgsType(model,data));
}
for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
{
computeMinverseForwardStep2::run(model.joints[i],data.joints[i],
computeMinverseForwardStep2::ArgsType(model,data));
}
return data.Minv;
}
// --- CHECKER ---------------------------------------------------------------
......
src/multibody/model.hpp
View file @ b35c281c
......@@ -360,6 +360,8 @@ namespace se3
typedef SE3::Vector3 Vector3;
typedef Eigen::Matrix<double,6,6,Eigen::RowMajor> RowMatrix6;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> RowMatrixXd;
/// \brief Vector of se3::JointData associated to the se3::JointModel stored in model,
/// encapsulated in JointDataAccessor.
JointDataVector joints;
......@@ -425,6 +427,9 @@ namespace se3
/// \brief The joint space inertia matrix (a square matrix of dim model.nv).
Eigen::MatrixXd M;
/// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
RowMatrixXd Minv;
/// \brief The Coriolis matrix (a square matrix of dim model.nv).
Eigen::MatrixXd C;
......@@ -437,6 +442,15 @@ namespace se3
/// \brief Variation of the forceset with respect to the joint acceleration.
Matrix6x dFda;
/// \brief Used in computeMinverse
Matrix6x SDinv;
/// \brief Used in computeMinverse
Matrix6x UDinv;
/// \brief Used in computeMinverse
Matrix6x IS;
/// \brief Right variation of the inertia matrix
container::aligned_vector<Inertia::Matrix6> vxI;
......
src/multibody/model.hxx
View file @ b35c281c
......@@ -252,10 +252,14 @@ namespace se3
,Ycrb((std::size_t)model.njoints)
,dYcrb((std::size_t)model.njoints)
,M(model.nv,model.nv)
,Minv(model.nv,model.nv)
,C(model.nv,model.nv)
,dFdq(6,model.nv)
,dFdv(6,model.nv)
,dFda(6,model.nv)
,SDinv(6,model.nv)
,UDinv(6,model.nv)
,IS(6,model.nv)
,vxI((std::size_t)model.njoints)
,Ivx((std::size_t)model.njoints)
,oYcrb((std::size_t)model.njoints)
......@@ -301,7 +305,7 @@ namespace se3
joints.push_back(CreateJointData::run(model.joints[i]));
/* Init for CRBA */
M.fill(0);
M.fill(0); Minv.setZero();
for(Model::Index i=0;i<(Model::Index)(model.njoints);++i ) { Fcrb[i].resize(6,model.nv); }
computeLastChild(model);
......
unittest/aba.cpp
View file @ b35c281c
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -22,6 +22,7 @@
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"
......@@ -125,5 +126,45 @@ BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea )
BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_computeMinverse )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
buildModels::humanoidSimple(model);
model.gravity.setZero();
se3::Data data(model);
se3::Data data_ref(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill(1.);
VectorXd q = randomConfiguration(model);
VectorXd v = VectorXd::Random(model.nv);
crba(model, data_ref, q);
data_ref.M.triangularView<Eigen::StrictlyLower>()
= data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
MatrixXd Minv_ref(data_ref.M.inverse());
computeMinverse(model, data, q);
BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));
data.Minv.triangularView<Eigen::StrictlyLower>()
= data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
BOOST_CHECK(data.Minv.isApprox(Minv_ref));
// std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
// std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
//
// std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
// std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;
}
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