Commit da3244da authored by jcarpent's avatar jcarpent
Browse files

[C++] Add operator vtiv

This operator allows the computation of the kinetic energy related to a
body with spatial Inertia I and spatial velocity v
parent 2db5e88c
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -59,6 +59,7 @@ namespace se3
Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); }
Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); }
Force operator*(const Motion & v) const { return derived().__mult__(v); }
Scalar_t vtiv(const Motion & v) const { return derived().vtiv_impl(v); }
/// aI = aXb.act(bI)
Derived_t se3Action(const SE3 & M) const
......@@ -241,6 +242,17 @@ namespace se3
return Force( m*v.linear()-mcxw,
m*c.cross(v.linear()) + I*v.angular() - c.cross(mcxw) );
}
Scalar_t vtiv_impl(const Motion & v) const
{
const Vector3 cxw (c.cross(v.angular()));
Scalar_t res = m * (v.linear().squaredNorm() - 2.*v.linear().dot(cxw));
const Vector3 mcxcxw (-m*c.cross(cxw));
res += v.angular().dot(mcxcxw);
res += I.vtiv(v.angular());
return res;
}
// Getters
Scalar_t mass() const { return m; }
......
// Copyright LAAS-CNRS, 2014
// Copyright LAAS-CNRS, 2014-2016
// This file is originally copied from metapod/tools/spatial/lti.hh.
// Authors: Olivier Stasse (LAAS, CNRS) and Sébastien Barthélémy (Aldebaran Robotics)
......@@ -165,6 +165,22 @@ namespace se3
return res;
}
operator Matrix3 () const { return matrix(); }
Scalar vtiv (const Vector3 & v) const
{
const Scalar & x = v[0];
const Scalar & y = v[1];
const Scalar & z = v[2];
const Scalar xx = x*x;
const Scalar xy = x*y;
const Scalar xz = x*z;
const Scalar yy = y*y;
const Scalar yz = y*z;
const Scalar zz = z*z;
return data_(0)*xx + data_(2)*yy + data_(5)*zz + 2.*(data_(1)*xy + data_(3)*xz + data_(4)*yz);
}
Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
{
......
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -206,8 +206,17 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
Symmetric3 RtSR = S.rotate(R.transpose());
is_matrix_absolutely_closed(RtSR.matrix(), R.transpose()*S.matrix()*R, 1e-12);
}
// Test operator vtiv
{
Symmetric3 S = Symmetric3::RandomPositive();
Vector3 v = Vector3::Random();
double kinetic_ref = v.transpose() * S.matrix() * v;
double kinetic = S.vtiv(v);
BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
}
// Time test
// Time test
{
const size_t NBT = 100000;
Symmetric3 S = Symmetric3::RandomPositive();
......
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -262,6 +262,11 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
Inertia I12 = I1;
I12 += I2;
is_matrix_absolutely_closed(I1.matrix()+I2.matrix(), I12.matrix(), 1e-12);
// Test operator vtiv
double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector();
double kinetic = aI.vtiv(v);
BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
}
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