Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[core] Fix numerical instability of flexibility model by adding armature-like inertia parameter. #433

Merged
merged 3 commits into from
Oct 29, 2021
Merged

[core] Fix numerical instability of flexibility model by adding armature-like inertia parameter. #433

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
596448f
[core] Full support of armature for all joints but mimic.
Oct 29, 2021
0b59909
[core] Rename 'pncCollisionData' in 'collisionData' and 'pncModelRigi…
Oct 29, 2021
17675af
[core] Add armature-like flexibility inertia parameter to fix numeric…
Oct 29, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 12 additions & 7 deletions core/include/jiminy/core/Types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,23 +127,27 @@ namespace jiminy
struct flexibleJointData_t
{
std::string frameName;
vectorN_t stiffness;
vectorN_t damping;
vector3_t stiffness;
vector3_t damping;
vector3_t inertia;

flexibleJointData_t(void) :
frameName(),
stiffness(),
damping()
damping(),
inertia()
{
// Empty.
};

flexibleJointData_t(std::string const & frameNameIn,
vectorN_t const & stiffnessIn,
vectorN_t const & dampingIn) :
vector3_t const & stiffnessIn,
vector3_t const & dampingIn,
vector3_t const & inertiaIn) :
frameName(frameNameIn),
stiffness(stiffnessIn),
damping(dampingIn)
damping(dampingIn),
inertia(inertiaIn)
{
// Empty.
};
Expand All @@ -152,7 +156,8 @@ namespace jiminy
{
return (this->frameName == other.frameName
&& this->stiffness == other.stiffness
&& this->damping == other.damping);
&& this->damping == other.damping
&& this->inertia == other.inertia);
};
};

Expand Down
8 changes: 5 additions & 3 deletions core/include/jiminy/core/robot/Model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -392,20 +392,22 @@ namespace jiminy
hresult_t removeConstraints(std::vector<std::string> const & constraintsNames,
constraintsHolderType_t const & holderType);

hresult_t refreshCollisionsProxies(void);
hresult_t refreshGeometryProxies(void);
hresult_t refreshContactsProxies(void);
/// \brief Refresh the proxies of the kinematics constraints.
hresult_t refreshConstraintsProxies(void);
virtual hresult_t refreshProxies(void);

public:
pinocchio::Model pncModelRigidOrig_;
pinocchio::Model pncModelOrig_;
pinocchio::Model pncModel_;
pinocchio::GeometryModel collisionModelOrig_;
pinocchio::GeometryModel collisionModel_;
pinocchio::GeometryModel visualModelOrig_;
pinocchio::GeometryModel visualModel_;
pinocchio::Data pncDataRigidOrig_;
mutable pinocchio::Data pncData_;
mutable std::unique_ptr<pinocchio::GeometryData> pncCollisionData_; // Using smart ptr to avoid having to initialize it with an empty GeometryModel, which causes Pinocchio segfault at least up to v2.5.6
mutable std::unique_ptr<pinocchio::GeometryData> collisionData_; // Using smart ptr to avoid having to initialize it with an empty GeometryModel, which causes Pinocchio segfault at least up to v2.5.6
std::unique_ptr<modelOptions_t const> mdlOptions_;
forceVector_t contactForces_; ///< Buffer storing the contact forces

Expand Down
219 changes: 179 additions & 40 deletions core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include <functional>

#include "pinocchio/spatial/fwd.hpp" // `Pinocchio::Motion`
#include "pinocchio/spatial/fwd.hpp" // `Pinocchio::Inertia`
#include "pinocchio/multibody/visitor.hpp" // `pinocchio::fusion::JointUnaryVisitorBase`
#include "pinocchio/multibody/fwd.hpp" // `pinocchio::ModelTpl`, `pinocchio::DataTpl`
#include "pinocchio/multibody/joint/fwd.hpp" // `pinocchio::JointModelBase`, `pinocchio::JointDataBase`, ...
Expand Down Expand Up @@ -74,7 +74,7 @@ namespace pinocchio_overload
Eigen::MatrixBase<TangentVectorType2> const & a)
{
(void) pinocchio::rnea(model, data, q, v, a);
data.tau += model.rotorInertia.asDiagonal() * a;
data.tau.array() += model.rotorInertia.array() * a.array();
return data.tau;
}

Expand All @@ -86,9 +86,7 @@ namespace pinocchio_overload
Eigen::MatrixBase<ConfigVectorType> const & q)
{
(void) pinocchio::crba(model, data, q);
// data.M.triangularView<Eigen::StrictlyLower>() =
// data.M.transpose().triangularView<Eigen::StrictlyLower>();
data.M += model.rotorInertia.asDiagonal();
data.M.diagonal() += model.rotorInertia;
return data.M;
}

Expand Down Expand Up @@ -120,8 +118,8 @@ namespace pinocchio_overload
jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i];

// jmodel.calc_aba(jdata.derived(), Ia, parent > 0);
Scalar const & Im = model.rotorInertia[jmodel.idx_v()];
calc_aba(jmodel.derived(), jdata.derived(), Ia, Im, parent > 0);
auto const Im = model.rotorInertia.segment(jmodel.idx_v(), jmodel.nv());
calc_aba(jmodel.derived(), jdata.derived(), Im, Ia, parent > 0);

if (parent > 0)
{
Expand All @@ -132,102 +130,243 @@ namespace pinocchio_overload
}
}

template<typename JointModel, typename Matrix6Like>
template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_revolute_v<JointModel>
|| is_pinocchio_joint_revolute_unbounded_v<JointModel>, void>
calc_aba(JointModel const & model,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
Scalar const & Im,
bool const & update_I)
{
using Motion = pinocchio::Motion;
using Inertia = pinocchio::Inertia;

data.U.noalias() = Ia.col(Motion::ANGULAR + getAxis(model));
data.Dinv[0] = Scalar(1.0) / (data.U[Motion::ANGULAR + getAxis(model)] + Im);
data.U = Ia.col(Inertia::ANGULAR + getAxis(model));
data.Dinv[0] = Scalar(1) / (data.U[Inertia::ANGULAR + getAxis(model)] + Im[0]);
data.UDinv.noalias() = data.U * data.Dinv[0];

if (update_I)
{
Ia -= data.UDinv * data.U.transpose();
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename Matrix6Like>
template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_revolute_unaligned_v<JointModel>
|| is_pinocchio_joint_revolute_unbounded_unaligned_v<JointModel>, void>
calc_aba(JointModel const & model,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
Scalar const & Im,
bool const & update_I)
{
using Motion = pinocchio::Motion;
using Inertia = pinocchio::Inertia;

data.U.noalias() = Ia.template middleCols<3>(Motion::ANGULAR) * model.axis;
data.Dinv[0] = Scalar(1.0) / (model.axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + Im);
data.U.noalias() = Ia.template middleCols<3>(Inertia::ANGULAR) * model.axis;
data.Dinv[0] = Scalar(1) / (model.axis.dot(data.U.template segment<3>(Inertia::ANGULAR)) + Im[0]);
data.UDinv.noalias() = data.U * data.Dinv[0];

if (update_I)
{
Ia -= data.UDinv * data.U.transpose();
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename Matrix6Like>
template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_prismatic_v<JointModel>, void>
calc_aba(JointModel const & model,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
Scalar const & Im,
bool const & update_I)
{
using Motion = pinocchio::Motion;
using Inertia = pinocchio::Inertia;

data.U.noalias() = Ia.col(Motion::LINEAR + getAxis(model));
data.Dinv[0] = Scalar(1.0) / (data.U[Motion::LINEAR + getAxis(model)] + Im);
data.U = Ia.col(Inertia::LINEAR + getAxis(model));
data.Dinv[0] = Scalar(1) / (data.U[Inertia::LINEAR + getAxis(model)] + Im[0]);
data.UDinv.noalias() = data.U * data.Dinv[0];

if (update_I)
{
Ia -= data.UDinv * data.U.transpose();
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename Matrix6Like>
template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_prismatic_unaligned_v<JointModel>, void>
calc_aba(JointModel const & model,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
Scalar const & Im,
bool const & update_I)
{
using Motion = pinocchio::Motion;
using Inertia = pinocchio::Inertia;

data.U.noalias() = Ia.template middleCols<3>(Motion::LINEAR) * model.axis;
data.Dinv[0] = Scalar(1.0) / (model.axis.dot(data.U.template segment<3>(Motion::LINEAR)) + Im);
data.U.noalias() = Ia.template middleCols<3>(Inertia::LINEAR) * model.axis;
data.Dinv[0] = Scalar(1) / (model.axis.dot(data.U.template segment<3>(Inertia::LINEAR)) + Im[0]);
data.UDinv.noalias() = data.U * data.Dinv[0];

if (update_I)
{
Ia -= data.UDinv * data.U.transpose();
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_planar_v<JointModel>, void>
calc_aba(JointModel const & /* model */,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
bool const & update_I)
{
static Eigen::Matrix<Scalar, 3, 3> StU;

data.U.template leftCols<2>() = Ia.template leftCols<2>();
data.U.template rightCols<1>() = Ia.template rightCols<1>();
StU.template leftCols<2>() = data.U.template topRows<2>().transpose();
StU.template rightCols<1>() = data.U.template bottomRows<1>();

StU.diagonal() += Im;
pinocchio::internal::PerformStYSInversion<Scalar>::run(StU, data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;

if(update_I)
{
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_translation_v<JointModel>, void>
calc_aba(JointModel const & /* model */,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
bool const & update_I)
{
static Eigen::Matrix<Scalar, 3, 3> StU;

using Inertia = pinocchio::Inertia;

data.U = Ia.template middleCols<3>(Inertia::LINEAR);
StU = data.U.template middleRows<3>(Inertia::LINEAR);
StU.diagonal() += Im;

pinocchio::internal::PerformStYSInversion<Scalar>::run(StU, data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;

if(update_I)
{
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_spherical_v<JointModel>, void>
calc_aba(JointModel const & /* model */,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
bool const & update_I)
{
static Eigen::Matrix<Scalar, 3, 3> StU;

using Inertia = pinocchio::Inertia;

data.U = Ia.template block<6,3>(0, Inertia::ANGULAR);
StU = data.U.template middleRows<3>(Inertia::ANGULAR);
StU.diagonal() += Im;

pinocchio::internal::PerformStYSInversion<Scalar>::run(StU, data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;

if (update_I)
{
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_spherical_zyx_v<JointModel>, void>
calc_aba(JointModel const & /* model */,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
bool const & update_I)
{
static Eigen::Matrix<Scalar, 3, 3> StU;

using Inertia = pinocchio::Inertia;

data.U.noalias() = Ia.template middleCols<3>(Inertia::ANGULAR) * data.S.angularSubspace();
StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Inertia::ANGULAR);
StU.diagonal() += Im;

pinocchio::internal::PerformStYSInversion<Scalar>::run(StU, data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;

if (update_I)
{
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_freeflyer_v<JointModel>, void>
calc_aba(JointModel const & /* model */,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
bool const & update_I)
{
static Eigen::Matrix<Scalar, 6, 6> StU;

data.U = Ia;
StU = Ia;
StU.diagonal() += Im;

pinocchio::internal::PerformStYSInversion<Scalar>::run(StU, data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;

if(update_I)
{
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_composite_v<JointModel>, void>
calc_aba(JointModel const & /* model */,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & Im,
Eigen::MatrixBase<Matrix6Like> & Ia,
bool const & update_I)
{
static Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> StU;

data.U.noalias() = Ia * data.S.matrix();
StU.noalias() = data.S.matrix().transpose() * data.U;
StU.diagonal() += Im;

pinocchio::internal::PerformStYSInversion<Scalar>::run(StU, data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;

if(update_I)
{
Ia.noalias() -= data.UDinv * data.U.transpose();
}
}

template<typename JointModel, typename Matrix6Like>
static std::enable_if_t<is_pinocchio_joint_planar_v<JointModel>
|| is_pinocchio_joint_translation_v<JointModel>
|| is_pinocchio_joint_spherical_v<JointModel>
|| is_pinocchio_joint_spherical_zyx_v<JointModel>
|| is_pinocchio_joint_freeflyer_v<JointModel>
|| is_pinocchio_joint_mimic_v<JointModel>
|| is_pinocchio_joint_composite_v<JointModel>, void>
template<typename JointModel, typename VectorLike, typename Matrix6Like>
static std::enable_if_t< is_pinocchio_joint_mimic_v<JointModel>, void>
calc_aba(JointModel const & model,
typename JointModel::JointDataDerived & data,
Eigen::MatrixBase<VectorLike> const & /* Im */,
Eigen::MatrixBase<Matrix6Like> & Ia,
Scalar const & /* Im */,
bool const & update_I)
{
// TODO: Not implemented
model.calc_aba(data.derived(), Ia, update_I);
}

Expand Down
Loading