Commit 28cfaad1 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add time variation of a Inertia according to the current spatial velocity of a the body

parent 2e414ab5
//
// 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(); }
......@@ -306,6 +307,27 @@ namespace se3
return res;
}
Matrix6 variation(const Motion & v) const
{
Matrix6 res;
const Motion mv(v*m);
res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),c) + skewSquare(c,mv.angular());
res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose();
res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable
res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),c) - skewSquare(c,mv.linear());
res.template block<3,3>(LINEAR,LINEAR) = (typename Symmetric3::Matrix3)(I - AlphaSkewSquare(m,c));
res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular());
res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR));
res.template block<3,3>(LINEAR,LINEAR).setZero();
return res;
}
// Getters
Scalar mass() const { return m; }
......
......@@ -341,6 +341,19 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
Inertia aI_approx(aI);
aI_approx.mass() += eps;
BOOST_CHECK(aI_approx.isApprox(aI,eps));
// Test Variation
Inertia::Matrix6 aIvariation = aI.variation(v);
Motion::ActionMatrix_t vAction = v.toActionMatrix();
Motion::ActionMatrix_t vDualAction = v.toDualActionMatrix();
Inertia::Matrix6 aImatrix = aI.matrix();
Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction;
BOOST_CHECK(aIvariation.isApprox(aIvariation_ref));
BOOST_CHECK(vxIv.isApprox(Force(aIvariation*v.toVector())));
}
BOOST_AUTO_TEST_CASE ( test_ActOnSet )
......
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