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

[C++][Cmake] Split ABA and CRBA in .hpp and .hxx

parent a0750255
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ c36ae895
......@@ -139,8 +139,10 @@ SET(${PROJECT_NAME}_MULTIBODY_HEADERS
SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/aba.hpp
algorithm/aba.hxx
algorithm/rnea.hpp
algorithm/crba.hpp
algorithm/crba.hxx
algorithm/jacobian.hpp
algorithm/jacobian.hxx
algorithm/cholesky.hpp
......
src/algorithm/aba.hpp
View file @ c36ae895
......@@ -18,7 +18,6 @@
#ifndef __se3_aba_hpp__
#define __se3_aba_hpp__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
namespace se3
......@@ -44,210 +43,6 @@ namespace se3
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
struct AbaForwardStep1 : public fusion::JointVisitor<AbaForwardStep1>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Model::Index,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(AbaForwardStep1);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
using namespace Eigen;
using namespace se3;
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i] * jdata.M();
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];
data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.Yaba[i] = model.inertias[i].matrix();
data.f[i] = model.inertias[i].vxiv(data.v[i]); // -f_ext
}
};
struct AbaBackwardStep : public fusion::JointVisitor<AbaBackwardStep>
{
typedef boost::fusion::vector<const Model &,
Data &,
const Model::Index> ArgsType;
JOINT_VISITOR_INIT(AbaBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointData> & jdata,
const Model & model,
Data & data,
const Model::Index i)
{
const Model::Index & parent = model.parents[i];
Inertia::Matrix6 & Ia = data.Yaba[i];
jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i];
jmodel.calc_aba(jdata.derived(), Ia, parent > 0);
jmodel.jointVelocitySelector(data.ddq) = jdata.Dinv() * jmodel.jointVelocitySelector(data.u);
if (parent > 0)
{
Force & pa = data.f[i];
pa.toVector() += Ia * data.a[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
data.Yaba[parent] += SE3actOn(data.liMi[i], Ia);
data.f[parent] += data.liMi[i].act(pa);
}
// Data::Matrix6x & U = data.U_aba[i];
// Eigen::MatrixXd & D_inv = data.D_inv_aba[i];
//
//// const ConstraintXd::DenseBase S = ((ConstraintXd)jdata.S()).matrix();
//
// U = data.Yaba[i] * ((ConstraintXd)jdata.S()).matrix();
//// U = Data::Matrix6x::Zero(6, JointModelBase<JointModel>::NV);
// D_inv = (jdata.S().transpose() * U.block(0,0,U.rows(), U.cols())).inverse();
//// D_inv = Eigen::MatrixXd::Zero(JointModelBase<JointModel>::NV, JointModelBase<JointModel>::NV);
// jmodel.jointVelocitySelector(data.tau) -= jdata.S().transpose()*data.f[i];
// if(parent>0)
// {
// Inertia::Matrix6 & Ia = data.Yaba[i];
// Force & pa = data.f[i];
//
// Ia -= U * D_inv * U.transpose();
//
// pa.toVector() += Ia * data.a[i].toVector() + U * D_inv * jmodel.jointVelocitySelector(data.tau);
//// Inertia::Matrix6 tmp = data.liMi[i].inverse().toActionMatrix();
//// data.Yaba[parent].triangularView<Eigen::Upper>() += tmp.transpose() * Ia.selfadjointView<Eigen::Upper>() * tmp;
// data.Yaba[parent] += SE3actOn(data.liMi[i], Ia);
// data.f[parent] += data.liMi[i].act(pa);
// }
}
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 AbaForwardStep2 : public fusion::JointVisitor<AbaForwardStep2>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Model::Index
> ArgsType;
JOINT_VISITOR_INIT(AbaForwardStep2);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i)
{
using namespace Eigen;
using namespace se3;
const Model::Index & parent = model.parents[i];
data.a[i] += data.liMi[i].actInv(data.a[parent]);
jmodel.jointVelocitySelector(data.ddq) -= jdata.UDinv().transpose() * data.a[i].toVector();
data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq);
}
};
inline const Eigen::VectorXd &
aba(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau)
{
data.v[0].setZero();
data.a[0] = -model.gravity;
data.u = tau;
for(Model::Index i=1;i<(Model::Index)model.nbody;++i)
{
AbaForwardStep1::run(model.joints[i],data.joints[i],
AbaForwardStep1::ArgsType(model,data,i,q,v));
}
for( Model::Index i=(Model::Index)model.nbody-1;i>0;--i )
{
AbaBackwardStep::run(model.joints[i],data.joints[i],
AbaBackwardStep::ArgsType(model,data,i));
}
for(Model::Index i=1;i<(Model::Index)model.nbody;++i)
{
AbaForwardStep2::run(model.joints[i],data.joints[i],
AbaForwardStep2::ArgsType(model,data,i));
}
return data.ddq;
}
} // namespace se3
#include "pinocchio/algorithm/aba.hxx"
#endif // ifndef __se3_aba_hpp__
src/algorithm/aba.hxx 0 → 100644
View file @ c36ae895
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_aba_hxx__
#define __se3_aba_hxx__
#include "pinocchio/multibody/visitor.hpp"
/// @cond DEV
namespace se3
{
struct AbaForwardStep1 : public fusion::JointVisitor<AbaForwardStep1>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Model::Index,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(AbaForwardStep1);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
using namespace Eigen;
using namespace se3;
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i] * jdata.M();
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];
data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.Yaba[i] = model.inertias[i].matrix();
data.f[i] = model.inertias[i].vxiv(data.v[i]); // -f_ext
}
};
struct AbaBackwardStep : public fusion::JointVisitor<AbaBackwardStep>
{
typedef boost::fusion::vector<const Model &,
Data &,
const Model::Index> ArgsType;
JOINT_VISITOR_INIT(AbaBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointData> & jdata,
const Model & model,
Data & data,
const Model::Index i)
{
const Model::Index & parent = model.parents[i];
Inertia::Matrix6 & Ia = data.Yaba[i];
jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i];
jmodel.calc_aba(jdata.derived(), Ia, parent > 0);
jmodel.jointVelocitySelector(data.ddq) = jdata.Dinv() * jmodel.jointVelocitySelector(data.u);
if (parent > 0)
{
Force & pa = data.f[i];
pa.toVector() += Ia * data.a[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
data.Yaba[parent] += SE3actOn(data.liMi[i], Ia);
data.f[parent] += data.liMi[i].act(pa);
}
// Data::Matrix6x & U = data.U_aba[i];
// Eigen::MatrixXd & D_inv = data.D_inv_aba[i];
//
//// const ConstraintXd::DenseBase S = ((ConstraintXd)jdata.S()).matrix();
//
// U = data.Yaba[i] * ((ConstraintXd)jdata.S()).matrix();
//// U = Data::Matrix6x::Zero(6, JointModelBase<JointModel>::NV);
// D_inv = (jdata.S().transpose() * U.block(0,0,U.rows(), U.cols())).inverse();
//// D_inv = Eigen::MatrixXd::Zero(JointModelBase<JointModel>::NV, JointModelBase<JointModel>::NV);
// jmodel.jointVelocitySelector(data.tau) -= jdata.S().transpose()*data.f[i];
// if(parent>0)
// {
// Inertia::Matrix6 & Ia = data.Yaba[i];
// Force & pa = data.f[i];
//
// Ia -= U * D_inv * U.transpose();
//
// pa.toVector() += Ia * data.a[i].toVector() + U * D_inv * jmodel.jointVelocitySelector(data.tau);
//// Inertia::Matrix6 tmp = data.liMi[i].inverse().toActionMatrix();
//// data.Yaba[parent].triangularView<Eigen::Upper>() += tmp.transpose() * Ia.selfadjointView<Eigen::Upper>() * tmp;
// data.Yaba[parent] += SE3actOn(data.liMi[i], Ia);
// data.f[parent] += data.liMi[i].act(pa);
// }
}
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 AbaForwardStep2 : public fusion::JointVisitor<AbaForwardStep2>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Model::Index
> ArgsType;
JOINT_VISITOR_INIT(AbaForwardStep2);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i)
{
using namespace Eigen;
using namespace se3;
const Model::Index & parent = model.parents[i];
data.a[i] += data.liMi[i].actInv(data.a[parent]);
jmodel.jointVelocitySelector(data.ddq) -= jdata.UDinv().transpose() * data.a[i].toVector();
data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq);
}
};
inline const Eigen::VectorXd &
aba(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau)
{
data.v[0].setZero();
data.a[0] = -model.gravity;
data.u = tau;
for(Model::Index i=1;i<(Model::Index)model.nbody;++i)
{
AbaForwardStep1::run(model.joints[i],data.joints[i],
AbaForwardStep1::ArgsType(model,data,i,q,v));
}
for( Model::Index i=(Model::Index)model.nbody-1;i>0;--i )
{
AbaBackwardStep::run(model.joints[i],data.joints[i],
AbaBackwardStep::ArgsType(model,data,i));
}
for(Model::Index i=1;i<(Model::Index)model.nbody;++i)
{
AbaForwardStep2::run(model.joints[i],data.joints[i],
AbaForwardStep2::ArgsType(model,data,i));
}
return data.ddq;
}
} // namespace se3
/// @endcond
#endif // ifndef __se3_aba_hxx__
src/algorithm/crba.hpp
View file @ c36ae895
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -18,12 +18,7 @@
#ifndef __se3_crba_hpp__
#define __se3_crba_hpp__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include <iostream>
namespace se3
{
......@@ -72,210 +67,6 @@ namespace se3
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
struct CrbaForwardStep : public fusion::JointVisitor<CrbaForwardStep>
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(CrbaForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model& model,
se3::Data& data,
const Eigen::VectorXd & q)
{
using namespace Eigen;
using namespace se3;
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
jmodel.calc(jdata.derived(),q);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.Ycrb[i] = model.inertias[i];
}
};
struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
{
typedef boost::fusion::vector<const Model&,
Data&> ArgsType;
JOINT_VISITOR_INIT(CrbaBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointData> & jdata,
const Model& model,
Data& data)
{
/*
* F[1:6,i] = Y*S
* M[i,SUBTREE] = S'*F[1:6,SUBTREE]
* if li>0
* Yli += liXi Yi
* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
*/
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
/* F[1:6,i] = Y*S */
//data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
/* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
const Model::JointIndex & parent = model.parents[i];
if(parent>0)
{
/* Yli += liXi Yi */
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
/* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
Eigen::Block<typename Data::Matrix6x> jF
= data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
Eigen::Block<typename Data::Matrix6x> iF
= data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
forceSet::se3Action(data.liMi[i], iF, jF);
}
// std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
// std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
// std::cout << "liFi = " << jdata.F() << std::endl;
// std::cout << "M = " << data.M << std::endl;
}
};
inline const Eigen::MatrixXd&
crba(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
for( Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
CrbaForwardStep::ArgsType(model,data,q));
}
for( Model::JointIndex i=(Model::JointIndex)(model.nbody-1);i>0;--i )
{
CrbaBackwardStep::run(model.joints[i],data.joints[i],
CrbaBackwardStep::ArgsType(model,data));
}
return data.M;
}
struct CcrbaForwardStep : public fusion::JointVisitor<CcrbaForwardStep>
{
typedef boost::fusion::vector< const se3::Model &,
se3::Data &,
const Model::Index,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(CcrbaForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,