Skip to content
Snippets Groups Projects
Commit f3667f3f authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Minor correction to improve computation time in Sym3. (error in the commit:... 

Minor correction to improve computation time in Sym3. (error in the commit: unittest symmetric working, but tspatial is broken. Fixed in the following commit.)
parent c226662f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/spatial/inertia.hpp
+ 34
− 37
View file @ f3667f3f
#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
......
src/spatial/symmetric3.hpp
+ 63
− 76
View file @ f3667f3f
......@@ -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;
}
......
unittest/symmetric.cpp
+ 17
− 8
View file @ f3667f3f
......@@ -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;
......
unittest/tspatial.cpp
+ 191
− 184
View file @ f3667f3f
......@@ -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;
}
......
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