From f3667f3ff0a6ac52be1ff6d8123b090c8a0dad22 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Mon, 1 Sep 2014 15:22:30 +0200
Subject: [PATCH] Minor correction to improve computation time in Sym3. (error
in the commit: unittest symmetric working, but tspatial is broken. Fixed in
the following commit.)
---
src/spatial/inertia.hpp | 71 ++++---
src/spatial/symmetric3.hpp | 139 +++++++-------
unittest/symmetric.cpp | 25 ++-
unittest/tspatial.cpp | 375 +++++++++++++++++++------------------
4 files changed, 305 insertions(+), 305 deletions(-)
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index da6ec2433..e5a420039 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -1,6 +1,7 @@
#ifndef __se3_inertia_hpp__
#define __se3_inertia_hpp__
+#include "pinocchio/spatial/symmetric3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
@@ -22,33 +23,30 @@ namespace se3
typedef SE3Tpl<Scalar,Options> SE3;
typedef InertiaTpl<Scalar,Options> Inertia;
enum { LINEAR = 0, ANGULAR = 3 };
- typedef Eigen::SelfAdjointView<Matrix3,Eigen::Upper> Symmetric3;
+ typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
public:
// Constructors
- InertiaTpl() : m(), c(), dense_I(), I(dense_I) {}
+ InertiaTpl() : m(), c(), I() {}
InertiaTpl(Scalar m_,
const Vector3 &c_,
const Matrix3 &I_)
: m(m_),
c(c_),
- dense_I(I_),
- I(dense_I) {}
+ I(I_) {}
InertiaTpl(Scalar _m,
const Vector3 &_c,
const Symmetric3 &_I)
: m(_m),
c(_c),
- dense_I(),
- I(dense_I) { I = _I; }
+ I(_I) { }
InertiaTpl(const InertiaTpl & clone) // Clone constructor for std::vector
: m(clone.m),
c(clone.c),
- dense_I(clone.dense_I),
- I(dense_I) {}
+ I(clone.I) {}
InertiaTpl& operator= (const InertiaTpl& clone) // Copy operator for std::vector
{
- m=clone.m; c=clone.c; dense_I=clone.dense_I;
+ m=clone.m; c=clone.c; I=clone.I;
return *this;
}
@@ -68,15 +66,9 @@ namespace se3
static Inertia Random()
{
/* We have to shoot "I" definite positive and not only symmetric. */
- Matrix3 M3 = Matrix3::Random(); // TODO clean that mess
- Matrix3 D = Vector3::Random().cwiseAbs().asDiagonal();
- Matrix3 M3D2 = M3+2*D;
- Matrix3 posdiag
- = M3D2.template selfadjointView<Eigen::Upper>();
return InertiaTpl(Eigen::internal::random<Scalar>(),
Vector3::Random(),
- //Matrix3::Random().template selfadjointView<Eigen::Upper>());
- posdiag);
+ Symmetric3::RandomPositive());
}
// Getters
@@ -84,32 +76,40 @@ namespace se3
const Vector3 & lever() const { return c; }
const Symmetric3 & inertia() const { return I; }
- Matrix6 toMatrix() const
+ Matrix6 matrix() const
{
Matrix6 M;
M.template block<3,3>(LINEAR, LINEAR ) = m*Matrix3::Identity();
M.template block<3,3>(LINEAR, ANGULAR) = -m*skew(c);
M.template block<3,3>(ANGULAR,LINEAR ) = m*skew(c);
- M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)I - m*skew(c)*skew(c);
+ M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)(I - typename Symmetric3::SkewSquare(c));
return M;
}
- operator Matrix6 () const { return toMatrix(); }
+ operator Matrix6 () const { return matrix(); }
// Arithmetic operators
- Inertia operator+(const InertiaTpl &other) const
+ friend Inertia operator+(const InertiaTpl &Ya, const InertiaTpl &Yb)
{
- const double & mm = m+other.m;
- const Matrix3 & X = skew((c-other.c).eval());
- Matrix3 mmmXX = (m*other.m/mm*X*X).eval(); // TODO clean this mess
- return InertiaTpl(mm, (m*c+other.m*other.c)/mm, dense_I+other.dense_I-mmmXX); // TODO: += in Eigen::Symmetric?
+ /* Y_{a+b} = ( m_a+m_b,
+ * (m_a*c_a + m_b*c_b ) / (m_a + m_b),
+ * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
+ */
+
+ const double & mab = Ya.m+Yb.m;
+ const Vector3 & mmmAB = ( ((Ya.m*Yb.m)/mab)*(Ya.c-Yb.c) ).eval();
+ return InertiaTpl( mab,
+ (Ya.m*Ya.c+Yb.m*Yb.c)/mab,
+ Ya.I+Yb.I - typename Symmetric3::SkewSquare(mmmAB));
}
- Inertia& operator+=(const InertiaTpl &other)
+
+ Inertia& operator+=(const InertiaTpl &Yb)
{
- const Matrix3 & X = skew((c-other.c).eval());
- Matrix3 mmXX = (m*other.m*X*X).eval(); // TODO: clean this mess
- c *=m; c+=other.m*other.c;
- m+=other.m; c/=m;
- dense_I+=other.dense_I-mmXX/m; // TODO: += in Eigen::Symmetric?
+ const Inertia& Ya = *this;
+ const double & mab = Ya.m+Yb.m;
+ const Vector3 & mmmAB = ( ((Ya.m*Yb.m)/mab)*(Ya.c-Yb.c) ).eval();
+ c *= m; c += Yb.m*Yb.c; c /= mab;
+ I += Yb.I; I -= typename Symmetric3::SkewSquare(mmmAB);
+ m += Yb.m;
return *this;
}
@@ -128,22 +128,21 @@ namespace se3
* 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
return Inertia( m,
M.translation()+M.rotation()*c,
- //dense_I);
- M.rotation()*I*M.rotation().transpose());
+ I.rotate(M.rotation()) );
}
/// bI = aXb.actInv(aI)
Inertia se3ActionInverse(const SE3 & M) const
{
return Inertia( m,
M.rotation().transpose()*(c-M.translation()),
- M.rotation().transpose()*I*M.rotation());
+ I.rotate(M.rotation().transpose()) );
}
//
Force vxiv( const Motion& v ) const
{
- Vector3 mcxw = m*c.cross(v.angular());
- Vector3 mv_mcxw = m*v.linear()-mcxw;
+ const Vector3 & mcxw = m*c.cross(v.angular());
+ const Vector3 & mv_mcxw = m*v.linear()-mcxw;
return Force( v.angular().cross(mv_mcxw),
v.angular().cross(c.cross(mv_mcxw)+I*v.angular())-v.linear().cross(mcxw) );
}
@@ -160,11 +159,9 @@ namespace se3
private:
Scalar m;
Vector3 c;
- Matrix3 dense_I;
Symmetric3 I;
};
-
typedef InertiaTpl<double,0> Inertia;
} // namespace se3
diff --git a/src/spatial/symmetric3.hpp b/src/spatial/symmetric3.hpp
index f6845d1ed..1dffb9cd1 100644
--- a/src/spatial/symmetric3.hpp
+++ b/src/spatial/symmetric3.hpp
@@ -48,13 +48,42 @@ namespace se3
static Symmetric3Tpl Zero() { return Symmetric3Tpl(Vector6::Zero() ); }
static Symmetric3Tpl Random() { return Symmetric3Tpl(Vector6::Random().eval()); }
static Symmetric3Tpl Identity() { return Symmetric3Tpl( 1, 0, 1, 0, 0, 1); }
- static Symmetric3Tpl SkewSq( const Vector3 & v )
- {
- const double & x = v[0], & y = v[1], & z = v[2];
- return Symmetric3Tpl( -y*y-z*z,
- x*y , -x*x-z*z,
- x*z , y*z , -x*x-y*y );
+
+ struct SkewSquare
+ {
+ const Vector3 & v;
+ SkewSquare( const Vector3 & v ) : v(v) {}
+ operator Symmetric3Tpl () const
+ {
+ const double & x = v[0], & y = v[1], & z = v[2];
+ return Symmetric3Tpl( -y*y-z*z,
+ x*y , -x*x-z*z,
+ x*z , y*z , -x*x-y*y );
+ }
+ }; // struct SkewSquare
+ Symmetric3Tpl operator- (const SkewSquare & v) const
+ {
+ const double & x = v.v[0], & y = v.v[1], & z = v.v[2];
+ return Symmetric3Tpl( data[0]+y*y+z*z,
+ data[1]-x*y , data[2]+x*x+z*z,
+ data[3]-x*z , data[4]-y*z , data[5]+x*x+y*y );
+ }
+ Symmetric3Tpl& operator-= (const SkewSquare & v)
+ {
+ const double & x = v.v[0], & y = v.v[1], & z = v.v[2];
+ data[0]+=y*y+z*z;
+ data[1]-=x*y ; data[2]+=x*x+z*z;
+ data[3]-=x*z ; data[4]-=y*z ; data[5]+=x*x+y*y;
+ return *this;
}
+ // static Symmetric3Tpl SkewSq( const Vector3 & v )
+ // {
+ // const double & x = v[0], & y = v[1], & z = v[2];
+ // return Symmetric3Tpl( -y*y-z*z,
+ // x*y , -x*x-z*z,
+ // x*z , y*z , -x*x-y*y );
+ // }
+
/* Shoot a positive definite matrix. */
static Symmetric3Tpl RandomPositive()
{
@@ -82,7 +111,7 @@ namespace se3
Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
{
- return Symmetric3Tpl(data+s2.data);
+ return Symmetric3Tpl((data+s2.data).eval());
}
Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
@@ -147,84 +176,42 @@ namespace se3
return L;
}
-
- /* R*S*R' */
- Symmetric3Tpl rotate(const Matrix3 & R) const
- {
- assert( (R.cols()==3) && (R.rows()==3) );
- assert( (R.transpose()*R).isApprox(Matrix3::Identity()) );
-
- Symmetric3Tpl Sres;
- Matrix2 Y;
- Matrix32 L;
-
- // 4 a
- L = decomposeltI();
-
- // Y = R' L ===> (12 m + 8 a)
- Y = R.template block<2,3>(1,0) * L;
-
- // Sres= Y R ===> (16 m + 8a)
- Sres.data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
- Sres.data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
- Sres.data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
- Sres.data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
- Sres.data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
-
- // r=R' v ( 6m + 3a)
- const Vector3 r( -R(0,0)*data(4) + R(0,1)*data(3),
- -R(1,0)*data(4) + R(1,1)*data(3),
- -R(2,0)*data(4) + R(2,1)*data(3) );
-
- // Sres_11 (3a)
- Sres.data(0) = L(0,0) + L(1,1) - Sres.data(2) - Sres.data(5);
-
- // Sres + D + (Ev)x ( 9a)
- Sres.data(0) += data(5);
- Sres.data(1) += r(2); Sres.data(2)+= data(5);
- Sres.data(3) +=-r(1); Sres.data(4)+= r(0); Sres.data(5) += data(5);
-
- return Sres;
- }
-
/* R*S*R' */
template<typename D>
- Symmetric3Tpl rotateTpl(const Eigen::MatrixBase<D> & R) const
+ Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
{
assert( (R.cols()==3) && (R.rows()==3) );
assert( (R.transpose()*R).isApprox(Matrix3::Identity()) );
- Symmetric3Tpl Sres;
- Matrix2 Y;
- Matrix32 L;
-
- // 4 a
- L = decomposeltI();
+ Symmetric3Tpl Sres;
+
+ // 4 a
+ const Matrix32 & L = decomposeltI();
+
+ // Y = R' L ===> (12 m + 8 a)
+ const Matrix2 & Y = R.template block<2,3>(1,0) * L;
- // Y = R' L ===> (12 m + 8 a)
- Y = R.template block<2,3>(1,0) * L;
-
- // Sres= Y R ===> (16 m + 8a)
- Sres.data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
- Sres.data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
- Sres.data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
- Sres.data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
- Sres.data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
-
- // r=R' v ( 6m + 3a)
- const Vector3 r( -R(0,0)*data(4) + R(0,1)*data(3),
- -R(1,0)*data(4) + R(1,1)*data(3),
- -R(2,0)*data(4) + R(2,1)*data(3) );
-
- // Sres_11 (3a)
- Sres.data(0) = L(0,0) + L(1,1) - Sres.data(2) - Sres.data(5);
+ // Sres= Y R ===> (16 m + 8a)
+ Sres.data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
+ Sres.data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
+ Sres.data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
+ Sres.data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
+ Sres.data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
+
+ // r=R' v ( 6m + 3a)
+ const Vector3 r( -R(0,0)*data(4) + R(0,1)*data(3),
+ -R(1,0)*data(4) + R(1,1)*data(3),
+ -R(2,0)*data(4) + R(2,1)*data(3) );
+
+ // Sres_11 (3a)
+ Sres.data(0) = L(0,0) + L(1,1) - Sres.data(2) - Sres.data(5);
- // Sres + D + (Ev)x ( 9a)
- Sres.data(0) += data(5);
- Sres.data(1) += r(2); Sres.data(2)+= data(5);
- Sres.data(3) +=-r(1); Sres.data(4)+= r(0); Sres.data(5) += data(5);
+ // Sres + D + (Ev)x ( 9a)
+ Sres.data(0) += data(5);
+ Sres.data(1) += r(2); Sres.data(2)+= data(5);
+ Sres.data(3) +=-r(1); Sres.data(4)+= r(0); Sres.data(5) += data(5);
- return Sres;
+ return Sres;
}
diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp
index 836d7e611..b5fcbafc3 100644
--- a/unittest/symmetric.cpp
+++ b/unittest/symmetric.cpp
@@ -2,7 +2,6 @@
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/tools/timer.hpp"
#include <boost/random.hpp>
@@ -82,7 +81,7 @@ void testSym3()
// Skew2
{
Vector3 v = Vector3::Random();
- Symmetric3 vxvx = Symmetric3::SkewSq(v);
+ Symmetric3 vxvx = Symmetric3::SkewSquare(v);
Vector3 p = Vector3::UnitX();
assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
@@ -90,6 +89,17 @@ void testSym3()
assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
p = Vector3::UnitZ();
assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+
+ Matrix3 vx = skew(v);
+ Matrix3 vxvx2 = (vx*vx).eval();
+ assert( vxvx.matrix().isApprox(vxvx2) );
+
+ Symmetric3 S = Symmetric3::RandomPositive();
+ assert( (S-Symmetric3::SkewSquare(v)).matrix()
+ .isApprox( S.matrix()-vxvx2 ) );
+ Symmetric3 S2 = S;
+ S -= Symmetric3::SkewSquare(v);
+ assert(S.matrix().isApprox( S2.matrix()-vxvx2 ) );
}
// (i,j)
@@ -112,7 +122,6 @@ void testSym3()
Symmetric3 RtSR = S.rotate(R.transpose());
assert( RtSR.matrix().isApprox( R.transpose()*S.matrix()*R ));
-
}
// Time test
@@ -182,7 +191,7 @@ void timeSelfAdj( const Eigen::Matrix3d & A,
const Eigen::Matrix3d & Sdense,
Eigen::Matrix3d & ASA )
{
- typedef se3::Inertia::Symmetric3 Sym3;
+ typedef Eigen::SelfAdjointView<Eigen::Matrix3d,Eigen::Upper> Sym3;
Sym3 S(Sdense);
ASA.triangularView<Eigen::Upper>()
= A * S * A.transpose();
@@ -191,17 +200,17 @@ void timeSelfAdj( const Eigen::Matrix3d & A,
void testSelfAdj()
{
using namespace se3;
- typedef Inertia::Matrix3 Matrix3;
- typedef Inertia::Symmetric3 Sym3;
+ typedef Eigen::Matrix3d Matrix3;
+ typedef Eigen::SelfAdjointView<Matrix3,Eigen::Upper> Sym3;
- Matrix3 M = Inertia::Matrix3::Random();
+ Matrix3 M = Matrix3::Random();
Sym3 S(M);
{
Matrix3 Scp = S;
assert( Scp-Scp.transpose()==Matrix3::Zero());
}
- Matrix3 M2 = Inertia::Matrix3::Random();
+ Matrix3 M2 = Matrix3::Random();
M.triangularView<Eigen::Upper>() = M2;
Matrix3 A = Matrix3::Random(), ASA1, ASA2;
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
index 554b7e3c8..1effe7e71 100644
--- a/unittest/tspatial.cpp
+++ b/unittest/tspatial.cpp
@@ -5,180 +5,180 @@
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
-bool testSE3()
-{
- using namespace se3;
- typedef Eigen::Matrix<double,4,4> Matrix4;
- typedef SE3::Matrix6 Matrix6;
- typedef SE3::Vector3 Vector3;
-
- SE3 amb = SE3::Random();
- SE3 bmc = SE3::Random();
- SE3 amc = amb*bmc;
-
- Matrix4 aMb = amb;
- Matrix4 bMc = bmc;
-
- // Test internal product
- Matrix4 aMc = amc;
- assert(aMc.isApprox(aMb*bMc));
-
- Matrix4 bMa = amb.inverse();
- assert(bMa.isApprox(aMb.inverse()));
-
- { // Test point action
- Vector3 p = Vector3::Random();
- Eigen::Matrix<double,4,1> p4; p4.head(3) = p; p4[3] = 1;
-
- Vector3 Mp = (aMb*p4).head(3);
- assert(amb.act(p).isApprox(Mp));
- Vector3 Mip = (aMb.inverse()*p4).head(3);
- assert(amb.actInv(p).isApprox(Mip));
- }
-
- { // Test action matrix
- Matrix6 aXb = amb;
- Matrix6 bXc = bmc;
- Matrix6 aXc = amc;
- assert(aXc.isApprox(aXb*bXc));
-
- Matrix6 bXa = amb.inverse();
- assert(bXa.isApprox(aXb.inverse()));
- }
-
-
- return true;
-}
-
-bool testMotion()
-{
- using namespace se3;
- typedef Eigen::Matrix<double,4,4> Matrix4;
- typedef SE3::Matrix6 Matrix6;
- typedef SE3::Vector3 Vector3;
- typedef Motion::Vector6 Vector6;
-
- SE3 amb = SE3::Random();
- SE3 bmc = SE3::Random();
- SE3 amc = amb*bmc;
-
- Motion bv = Motion::Random();
- Motion bv2 = Motion::Random();
-
- Vector6 bv_vec = bv;
- Vector6 bv2_vec = bv2;
+// bool testSE3()
+// {
+// using namespace se3;
+// typedef Eigen::Matrix<double,4,4> Matrix4;
+// typedef SE3::Matrix6 Matrix6;
+// typedef SE3::Vector3 Vector3;
+
+// SE3 amb = SE3::Random();
+// SE3 bmc = SE3::Random();
+// SE3 amc = amb*bmc;
+
+// Matrix4 aMb = amb;
+// Matrix4 bMc = bmc;
+
+// // Test internal product
+// Matrix4 aMc = amc;
+// assert(aMc.isApprox(aMb*bMc));
+
+// Matrix4 bMa = amb.inverse();
+// assert(bMa.isApprox(aMb.inverse()));
+
+// { // Test point action
+// Vector3 p = Vector3::Random();
+// Eigen::Matrix<double,4,1> p4; p4.head(3) = p; p4[3] = 1;
+
+// Vector3 Mp = (aMb*p4).head(3);
+// assert(amb.act(p).isApprox(Mp));
+// Vector3 Mip = (aMb.inverse()*p4).head(3);
+// assert(amb.actInv(p).isApprox(Mip));
+// }
+
+// { // Test action matrix
+// Matrix6 aXb = amb;
+// Matrix6 bXc = bmc;
+// Matrix6 aXc = amc;
+// assert(aXc.isApprox(aXb*bXc));
+
+// Matrix6 bXa = amb.inverse();
+// assert(bXa.isApprox(aXb.inverse()));
+// }
+
+
+// return true;
+// }
+
+// bool testMotion()
+// {
+// using namespace se3;
+// typedef Eigen::Matrix<double,4,4> Matrix4;
+// typedef SE3::Matrix6 Matrix6;
+// typedef SE3::Vector3 Vector3;
+// typedef Motion::Vector6 Vector6;
+
+// SE3 amb = SE3::Random();
+// SE3 bmc = SE3::Random();
+// SE3 amc = amb*bmc;
+
+// Motion bv = Motion::Random();
+// Motion bv2 = Motion::Random();
+
+// Vector6 bv_vec = bv;
+// Vector6 bv2_vec = bv2;
- // Test .+.
- Vector6 bvPbv2_vec = bv+bv2;
- assert( bvPbv2_vec.isApprox(bv_vec+bv2_vec) );
- // Test -.
- Vector6 Mbv_vec = -bv;
- assert( Mbv_vec.isApprox(-bv_vec) );
- // Test .+=.
- Motion bv3 = bv; bv3 += bv2;
- assert(bv3.toVector().isApprox(bv_vec+bv2_vec));
- // Test .=V6
- bv3 = bv2_vec;
- assert(bv3.toVector().isApprox(bv2_vec));
- // Test constructor from V6
- Motion bv4(bv2_vec);
- assert(bv4.toVector().isApprox(bv2_vec));
-
- // Test action
- Matrix6 aXb = amb;
- assert( amb.act(bv).toVector().isApprox(aXb*bv_vec));
- // Test action inverse
- Matrix6 bXc = bmc;
- assert( bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec));
-
- // Test double action
- Motion cv = Motion::Random();
- bv = bmc.act(cv);
- assert( amb.act(bv).toVector().isApprox(amc.act(cv).toVector()) );
-
- // Simple test for cross product vxv
- Motion vxv = bv.cross(bv);
- assert(vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
-
- // Simple test for cross product vxf
- Force f = bv.toVector();
- Force vxf = bv.cross(f);
- assert( vxf.linear().isApprox( bv.angular().cross(f.linear())));
- assert( vxf.angular().isMuchSmallerThan(1e-3));
-
- // Test frame change for vxf
- Motion av = Motion::Random();
- Force af = Force::Random();
- bv = amb.actInv(av);
- Force bf = amb.actInv(af);
- Force avxf = av.cross(af);
- Force bvxf = bv.cross(bf);
- assert( avxf.toVector().isApprox( amb.act(bvxf).toVector()) );
-
- // Test frame change for vxv
- av = Motion::Random();
- Motion aw = Motion::Random();
- bv = amb.actInv(av);
- Motion bw = amb.actInv(aw);
- Motion avxw = av.cross(aw);
- Motion bvxw = bv.cross(bw);
- assert( avxw.toVector().isApprox( amb.act(bvxw).toVector()) );
-
- return true;
-}
-
-
-bool testForce()
-{
- using namespace se3;
- typedef Eigen::Matrix<double,4,4> Matrix4;
- typedef SE3::Matrix6 Matrix6;
- typedef SE3::Vector3 Vector3;
- typedef Force::Vector6 Vector6;
-
- SE3 amb = SE3::Random();
- SE3 bmc = SE3::Random();
- SE3 amc = amb*bmc;
-
- Force bf = Force::Random();
- Force bf2 = Force::Random();
-
- Vector6 bf_vec = bf;
- Vector6 bf2_vec = bf2;
+// // Test .+.
+// Vector6 bvPbv2_vec = bv+bv2;
+// assert( bvPbv2_vec.isApprox(bv_vec+bv2_vec) );
+// // Test -.
+// Vector6 Mbv_vec = -bv;
+// assert( Mbv_vec.isApprox(-bv_vec) );
+// // Test .+=.
+// Motion bv3 = bv; bv3 += bv2;
+// assert(bv3.toVector().isApprox(bv_vec+bv2_vec));
+// // Test .=V6
+// bv3 = bv2_vec;
+// assert(bv3.toVector().isApprox(bv2_vec));
+// // Test constructor from V6
+// Motion bv4(bv2_vec);
+// assert(bv4.toVector().isApprox(bv2_vec));
+
+// // Test action
+// Matrix6 aXb = amb;
+// assert( amb.act(bv).toVector().isApprox(aXb*bv_vec));
+// // Test action inverse
+// Matrix6 bXc = bmc;
+// assert( bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec));
+
+// // Test double action
+// Motion cv = Motion::Random();
+// bv = bmc.act(cv);
+// assert( amb.act(bv).toVector().isApprox(amc.act(cv).toVector()) );
+
+// // Simple test for cross product vxv
+// Motion vxv = bv.cross(bv);
+// assert(vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
+
+// // Simple test for cross product vxf
+// Force f = bv.toVector();
+// Force vxf = bv.cross(f);
+// assert( vxf.linear().isApprox( bv.angular().cross(f.linear())));
+// assert( vxf.angular().isMuchSmallerThan(1e-3));
+
+// // Test frame change for vxf
+// Motion av = Motion::Random();
+// Force af = Force::Random();
+// bv = amb.actInv(av);
+// Force bf = amb.actInv(af);
+// Force avxf = av.cross(af);
+// Force bvxf = bv.cross(bf);
+// assert( avxf.toVector().isApprox( amb.act(bvxf).toVector()) );
+
+// // Test frame change for vxv
+// av = Motion::Random();
+// Motion aw = Motion::Random();
+// bv = amb.actInv(av);
+// Motion bw = amb.actInv(aw);
+// Motion avxw = av.cross(aw);
+// Motion bvxw = bv.cross(bw);
+// assert( avxw.toVector().isApprox( amb.act(bvxw).toVector()) );
+
+// return true;
+// }
+
+
+// bool testForce()
+// {
+// using namespace se3;
+// typedef Eigen::Matrix<double,4,4> Matrix4;
+// typedef SE3::Matrix6 Matrix6;
+// typedef SE3::Vector3 Vector3;
+// typedef Force::Vector6 Vector6;
+
+// SE3 amb = SE3::Random();
+// SE3 bmc = SE3::Random();
+// SE3 amc = amb*bmc;
+
+// Force bf = Force::Random();
+// Force bf2 = Force::Random();
+
+// Vector6 bf_vec = bf;
+// Vector6 bf2_vec = bf2;
- // Test .+.
- Vector6 bfPbf2_vec = bf+bf2;
- assert( bfPbf2_vec.isApprox(bf_vec+bf2_vec) );
- // Test -.
- Vector6 Mbf_vec = -bf;
- assert( Mbf_vec.isApprox(-bf_vec) );
- // Test .+=.
- Force bf3 = bf; bf3 += bf2;
- assert(bf3.toVector().isApprox(bf_vec+bf2_vec));
- // Test .= V6
- bf3 = bf2_vec;
- assert(bf3.toVector().isApprox(bf2_vec));
- // Test constructor from V6
- Force bf4(bf2_vec);
- assert(bf4.toVector().isApprox(bf2_vec));
-
- // Test action
- Matrix6 aXb = amb;
- assert( amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec));
- // Test action inverse
- Matrix6 bXc = bmc;
- assert( bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec));
- // Test double action
- Force cf = Force::Random();
- bf = bmc.act(cf);
- assert( amb.act(bf).toVector().isApprox(amc.act(cf).toVector()) );
-
- // Simple test for cross product
- // Force vxv = bf.cross(bf);
- // assert(vxv.toVector().isMuchSmallerThan(bf.toVector()));
-
- return true;
-}
+// // Test .+.
+// Vector6 bfPbf2_vec = bf+bf2;
+// assert( bfPbf2_vec.isApprox(bf_vec+bf2_vec) );
+// // Test -.
+// Vector6 Mbf_vec = -bf;
+// assert( Mbf_vec.isApprox(-bf_vec) );
+// // Test .+=.
+// Force bf3 = bf; bf3 += bf2;
+// assert(bf3.toVector().isApprox(bf_vec+bf2_vec));
+// // Test .= V6
+// bf3 = bf2_vec;
+// assert(bf3.toVector().isApprox(bf2_vec));
+// // Test constructor from V6
+// Force bf4(bf2_vec);
+// assert(bf4.toVector().isApprox(bf2_vec));
+
+// // Test action
+// Matrix6 aXb = amb;
+// assert( amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec));
+// // Test action inverse
+// Matrix6 bXc = bmc;
+// assert( bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec));
+// // Test double action
+// Force cf = Force::Random();
+// bf = bmc.act(cf);
+// assert( amb.act(bf).toVector().isApprox(amc.act(cf).toVector()) );
+
+// // Simple test for cross product
+// // Force vxv = bf.cross(bf);
+// // assert(vxv.toVector().isMuchSmallerThan(bf.toVector()));
+
+// return true;
+// }
bool testInertia()
{
@@ -197,7 +197,7 @@ bool testInertia()
assert( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );
Inertia I1 = Inertia::Identity();
- assert( I1.toMatrix() == Matrix6::Identity() );
+ assert( I1.matrix() == Matrix6::Identity() );
// Test motion-to-force map
Motion v = Motion::Random();
@@ -208,11 +208,20 @@ bool testInertia()
SE3 bma = SE3::Random();
Inertia bI = bma.act(aI);
Matrix6 bXa = bma;
- assert( (bma.rotation()*aI.inertia()*bma.rotation().transpose()).isApprox((Matrix3)bI.inertia()) );
- assert( (bXa.transpose().inverse() * aI.toMatrix() * bXa.inverse()).isApprox( bI.toMatrix()) );
+ assert( (bma.rotation()*aI.inertia().matrix()*bma.rotation().transpose())
+ .isApprox((Matrix3)bI.inertia()) );
+ std::cout << "bXa = [" << bma.toActionMatrix() << std::endl;
+ std::cout << "bY = [" << bI.matrix() << std::endl;
+ std::cout << "aY = [" << aI.matrix() << std::endl;
+ std::cout << "ac = [" << aI.lever() << std::endl;
+ std::cout << "bc = [" << bI.lever() << std::endl;
+ std::cout << "am = [" << aI.mass() << std::endl;
+ std::cout << "bm = [" << bI.mass() << std::endl;
+
+ assert( (bXa.transpose().inverse() * aI.matrix() * bXa.inverse()).isApprox( bI.matrix()) );
// Test inverse action
- assert( (bXa.transpose() * bI.toMatrix() * bXa).isApprox( bma.actInv(bI).toMatrix()) );
+ assert( (bXa.transpose() * bI.matrix() * bXa).isApprox( bma.actInv(bI).matrix()) );
// Test vxIv cross product
v = Motion::Random();
@@ -224,22 +233,20 @@ bool testInertia()
// Test operator+
I1 = Inertia::Random();
Inertia I2 = Inertia::Random();
- assert( (I1.toMatrix()+I2.toMatrix()).isApprox((I1+I2).toMatrix()) );
-
+ assert( (I1.matrix()+I2.matrix()).isApprox((I1+I2).matrix()) );
+ // operator +=
Inertia I12 = I1;
I12 += I2;
- assert( (I1.toMatrix()+I2.toMatrix()).isApprox(I12.toMatrix()) );
-
-
+ assert( (I1.matrix()-I2.matrix()).isApprox(I12.matrix()) );
return true;
}
int main()
{
- testSE3();
- testMotion();
- testForce();
+ // testSE3();
+ // testMotion();
+ // testForce();
testInertia();
return 1;
}
--
GitLab