Commit 7672ce5b authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add dualActionMatrix to Motion class

parent 68982317
......@@ -55,6 +55,7 @@ namespace se3
operator Vector6 () const { return toVector(); }
ActionMatrix_t toActionMatrix() const { return derived().toActionMatrix_impl(); }
ActionMatrix_t toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
operator Matrix6 () const { return toActionMatrix(); }
bool operator==(const Derived_t & other) const {return derived().isEqual(other);}
......@@ -160,6 +161,16 @@ namespace se3
return X;
}
ActionMatrix_t toDualActionMatrix_impl () const
{
ActionMatrix_t X;
X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) = skew (angular_impl());
X.template block <3,3> (ANGULAR, LINEAR) = skew (linear_impl());
X.template block <3,3> (LINEAR, ANGULAR).setZero ();
return X;
}
// Getters
ConstAngular_t angular_impl() const { return data.template segment<3> (ANGULAR); }
......
......@@ -134,8 +134,21 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
Motion vxv = bv.cross(bv);
BOOST_CHECK_SMALL(vxv.toVector().tail(3).norm(), 1e-3); //previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
// Test Action Matrix
Motion v2xv = bv2.cross(bv);
Motion::ActionMatrix_t actv2 = bv2.toActionMatrix();
BOOST_CHECK(v2xv.toVector().isApprox(actv2*bv.toVector()));
// Test Dual Action Matrix
Force f(bv.toVector());
Force v2xf = bv2.cross(f);
Motion::ActionMatrix_t dualactv2 = bv2.toDualActionMatrix();
BOOST_CHECK(v2xf.toVector().isApprox(dualactv2*f.toVector()));
BOOST_CHECK(dualactv2.isApprox(-actv2.transpose()));
// Simple test for cross product vxf
Force f = Force(bv.toVector());
Force vxf = bv.cross(f);
BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear()), 1e-12));
BOOST_CHECK_SMALL(vxf.angular().norm(), 1e-3);//previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3));
......
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