diff --git a/CMakeLists.txt b/CMakeLists.txt
index ea3a6375087fe20477b5a22563da0545965bb86e..9caa4619d4e70ff6e624c51750c2836769e3d42c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,6 +32,7 @@ ADD_REQUIRED_DEPENDENCY("urdfdom >= v0.3.0")
# ----------------------------------------------------
SET(${PROJECT_NAME}_HEADERS
exception.hpp
+ spatial/symmetric3.hpp
spatial/se3.hpp
spatial/motion.hpp
spatial/force.hpp
@@ -86,6 +87,9 @@ ENDFOREACH(header)
ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL unittest/tspatial.cpp)
PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
+ADD_EXECUTABLE(symmetric EXCLUDE_FROM_ALL unittest/symmetric.cpp)
+PKG_CONFIG_USE_DEPENDENCY(symmetric eigenpy)
+
ADD_EXECUTABLE(constraint EXCLUDE_FROM_ALL unittest/constraint.cpp)
PKG_CONFIG_USE_DEPENDENCY(constraint eigenpy)
diff --git a/src/spatial/symmetric3.hpp b/src/spatial/symmetric3.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f6845d1ed06eff6f2e91e7df054f20a4f61f15b3
--- /dev/null
+++ b/src/spatial/symmetric3.hpp
@@ -0,0 +1,241 @@
+/* Copyright LAAS-CNRS, 2014
+ *
+ * This file is originally copied from metapod/tools/spatial/lti.hh.
+ * Authors: Olivier Stasse (LAAS, CNRS) and Sébastien Barthélémy (Aldebaran Robotics)
+ * The file was modified in pinocchio by Nicolas Mansard (LAAS, CNRS)
+ *
+ * metapod is free software, distributed under the terms of the GNU Lesser
+ * General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ */
+
+#ifndef __se3__symmetric3_hpp__
+#define __se3__symmetric3_hpp__
+
+#include <ostream>
+
+namespace se3
+{
+
+ template<typename _Scalar, int _Options>
+ class Symmetric3Tpl
+ {
+ public:
+ typedef _Scalar Scalar;
+ enum { Options = _Options };
+ typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
+ typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
+ typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
+ typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
+ typedef Eigen::Matrix<Scalar,3,2,Options> Matrix32;
+
+ public:
+ Symmetric3Tpl(): data() {}
+ Symmetric3Tpl(const Matrix3 &I)
+ {
+ assert( (I-I.transpose()).isMuchSmallerThan(I) );
+ data(0) = I(0,0);
+ data(1) = I(1,0); data(2) = I(1,1);
+ data(3) = I(2,0); data(4) = I(2,1); data(5) = I(2,2);
+ }
+ Symmetric3Tpl(const Vector6 &I) : data(I) {}
+ Symmetric3Tpl(const double & a0,const double & a1,const double & a2,
+ const double & a3,const double & a4,const double & a5)
+ { data << a0,a1,a2,a3,a4,a5; }
+
+ 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 );
+ }
+ /* Shoot a positive definite matrix. */
+ static Symmetric3Tpl RandomPositive()
+ {
+ double
+ a = double(std::rand())/RAND_MAX*2.0-1.0,
+ b = double(std::rand())/RAND_MAX*2.0-1.0,
+ c = double(std::rand())/RAND_MAX*2.0-1.0,
+ d = double(std::rand())/RAND_MAX*2.0-1.0,
+ e = double(std::rand())/RAND_MAX*2.0-1.0,
+ f = double(std::rand())/RAND_MAX*2.0-1.0;
+ return Symmetric3Tpl(a*a+b*b+d*d,
+ a*b+b*c+d*e, b*b+c*c+e*e,
+ a*d+b*e+d*f, b*d+c*e+e*f, d*d+e*e+f*f );
+ }
+
+ Matrix3 matrix() const
+ {
+ Matrix3 res;
+ res(0,0) = data(0); res(0,1) = data(1); res(0,2) = data(3);
+ res(1,0) = data(1); res(1,1) = data(2); res(1,2) = data(4);
+ res(2,0) = data(3); res(2,1) = data(4); res(2,2) = data(5);
+ return res;
+ }
+ operator Matrix3 () const { return matrix(); }
+
+ Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
+ {
+ return Symmetric3Tpl(data+s2.data);
+ }
+
+ Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
+ {
+ data += s2.data; return *this;
+ }
+
+ Vector3 operator*(const Vector3 &v) const
+ {
+ return Vector3(
+ data(0) * v(0) + data(1) * v(1) + data(3) * v(2),
+ data(1) * v(0) + data(2) * v(1) + data(4) * v(2),
+ data(3) * v(0) + data(4) * v(1) + data(5) * v(2)
+ );
+ }
+
+ // Matrix3 operator*(const Matrix3 &a) const
+ // {
+ // Matrix3 r;
+ // for(unsigned int i=0; i<3; ++i)
+ // {
+ // r(0,i) = data(0) * a(0,i) + data(1) * a(1,i) + data(3) * a(2,i);
+ // r(1,i) = data(1) * a(0,i) + data(2) * a(1,i) + data(4) * a(2,i);
+ // r(2,i) = data(3) * a(0,i) + data(4) * a(1,i) + data(5) * a(2,i);
+ // }
+ // return r;
+ // }
+
+ const Scalar& operator()(const int &i,const int &j) const
+ {
+ return ((i!=2)&&(j!=2)) ? data[i+j] : data[i+j+1];
+ }
+
+ Symmetric3Tpl operator-(const Matrix3 &S) const
+ {
+ assert( (S-S.transpose()).isMuchSmallerThan(S) );
+ return Symmetric3Tpl( data(0)-S(0,0),
+ data(1)-S(1,0), data(2)-S(1,1),
+ data(3)-S(2,0), data(4)-S(2,1), data(5)-S(2,2) );
+ }
+
+ Symmetric3Tpl operator+(const Matrix3 &S) const
+ {
+ assert( (S-S.transpose()).isMuchSmallerThan(S) );
+ return Symmetric3Tpl( data(0)+S(0,0),
+ data(1)+S(1,0), data(2)+S(1,1),
+ data(3)+S(2,0), data(4)+S(2,1), data(5)+S(2,2) );
+ }
+
+ /* --- Symmetric R*S*R' and R'*S*R products --- */
+ public: //private:
+
+ /** \brief Computes L for a symmetric matrix A.
+ */
+ Matrix32 decomposeltI() const
+ {
+ Matrix32 L;
+ L <<
+ data(0) - data(5), data(1),
+ data(1), data(2) - data(5),
+ 2*data(3), data(4) + data(4);
+ 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
+ {
+ 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;
+ }
+
+
+ public: //todo: make private
+
+ Vector6 data;
+ };
+
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+
+} // namespace se3
+
+#endif // ifndef __se3__symmetric3_hpp__
+
diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ebce9e22b9cb99bd99f59ed8f54729024ef039d1
--- /dev/null
+++ b/unittest/symmetric.cpp
@@ -0,0 +1,228 @@
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/tools/timer.hpp"
+
+#include <boost/random.hpp>
+#include <assert.h>
+
+#include "pinocchio/spatial/symmetric3.hpp"
+
+#include <Eigen/StdVector>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION( Eigen::Matrix3d );
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Symmetric3);
+
+void timeSym3(const se3::Symmetric3 & S,
+ const se3::Symmetric3::Matrix3 & R,
+ se3::Symmetric3 & res)
+{
+ res = S.rotate(R);
+}
+
+void testSym3()
+{
+ using namespace se3;
+ typedef Symmetric3::Matrix3 Matrix3;
+ typedef Symmetric3::Vector3 Vector3;
+
+ {
+ // op(Matrix3)
+ {
+ Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+ Symmetric3 S(M);
+ assert( S.matrix().isApprox(M) );
+ }
+
+ // S += S
+ {
+ Symmetric3
+ S = Symmetric3::Random(),
+ S2 = Symmetric3::Random();
+ Symmetric3 Scopy = S;
+ S+=S2;
+ assert( S.matrix().isApprox( S2.matrix()+Scopy.matrix()) );
+ }
+
+ // S + M
+ {
+ Symmetric3 S = Symmetric3::Random();
+ Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+
+ Symmetric3 S2 = S + M;
+ assert( S2.matrix().isApprox( S.matrix()+M ));
+
+ S2 = S - M;
+ assert( S2.matrix().isApprox( S.matrix()-M ));
+ }
+
+ // S*v
+ {
+ Symmetric3 S = Symmetric3::Random();
+ Vector3 v = Vector3::Random();
+ Vector3 Sv = S*v;
+ assert( Sv.isApprox( S.matrix()*v ));
+ }
+
+ // Random
+ for(int i=0;i<100;++i )
+ {
+ Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+ Symmetric3 S = Symmetric3::RandomPositive();
+ Vector3 v = Vector3::Random();
+ assert( (v.transpose()*(S*v))[0] > 0);
+ }
+
+ // Identity
+ {
+ assert( Symmetric3::Identity().matrix().isApprox( Matrix3::Identity() ) );
+ }
+
+ // Skew2
+ {
+ Vector3 v = Vector3::Random();
+ Symmetric3 vxvx = Symmetric3::SkewSq(v);
+
+ Vector3 p = Vector3::UnitX();
+ assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+ p = Vector3::UnitY();
+ assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+ p = Vector3::UnitZ();
+ assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+ }
+
+ // (i,j)
+ {
+ Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+ M << 1,2,4,2,3,5,4,5,6 ;
+ Symmetric3 S(M); // = Symmetric3::RandomPositive();
+ for(int i=0;i<3;++i)
+ for(int j=0;j<3;++j)
+ assert( S(i,j) == M(i,j) );
+ }
+ }
+
+ // SRS
+ {
+ Symmetric3 S = Symmetric3::RandomPositive();
+ Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
+
+ Symmetric3 RSRt = S.rotate(R);
+ assert( RSRt.matrix().isApprox( R*S.matrix()*R.transpose() ));
+
+ Symmetric3 RtSR = S.rotate(R.transpose());
+ assert( RtSR.matrix().isApprox( R.transpose()*S.matrix()*R ));
+
+ }
+
+ // Time test
+ {
+ const int NBT = 100000;
+ Symmetric3 S = Symmetric3::RandomPositive();
+
+ std::vector<Symmetric3> Sres (NBT);
+ std::vector<Matrix3> Rs (NBT);
+ for(int i=0;i<NBT;++i)
+ Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
+
+ StackTicToc timer(StackTicToc::US); timer.tic();
+ SMOOTH(NBT)
+ {
+ timeSym3(S,Rs[_smooth],Sres[_smooth]);
+ }
+ timer.toc(std::cout,NBT);
+ }
+}
+/* --- METAPOD ---------------------------------------------- */
+#include <metapod/tools/spatial/lti.hh>
+#include <metapod/tools/spatial/rm-general.hh>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>);
+
+void timeLTI(const metapod::Spatial::ltI<double>& S,
+ const metapod::Spatial::RotationMatrixTpl<double>& R,
+ metapod::Spatial::ltI<double> & res)
+{
+ res = R.rotSymmetricMatrix(S);
+}
+
+void testLTI()
+{
+ using namespace metapod::Spatial;
+
+ typedef ltI<double> Sym3;
+ typedef Eigen::Matrix3d Matrix3;
+ typedef RotationMatrixTpl<double> R3;
+
+ Matrix3 M = Matrix3::Random();
+ Sym3 S(M),S2;
+
+ R3 R; R.randomInit();
+
+ R.rotTSymmetricMatrix(S);
+ timeLTI(S,R,S2);
+
+ assert( S2.toMatrix().isApprox( R.toMatrix().transpose()*S.toMatrix()*R.toMatrix()) );
+
+ const int NBT = 100000;
+ std::vector<Sym3> Sres (NBT);
+ std::vector<R3> Rs (NBT);
+ for(int i=0;i<NBT;++i)
+ Rs[i].randomInit();
+
+ StackTicToc timer(StackTicToc::US); timer.tic();
+ SMOOTH(NBT)
+ {
+ timeLTI(S,Rs[_smooth],Sres[_smooth]);
+ }
+ timer.toc(std::cout,NBT);
+ //std::cout << Rs[std::rand() % NBT] << std::endl;
+
+}
+
+void timeSelfAdj( const Eigen::Matrix3d & A,
+ const Eigen::Matrix3d & Sdense,
+ Eigen::Matrix3d & ASA )
+{
+ typedef se3::Inertia::Symmetric3 Sym3;
+ Sym3 S(Sdense);
+ ASA.triangularView<Eigen::Upper>()
+ = A * S * A.transpose();
+}
+
+void testSelfAdj()
+{
+ using namespace se3;
+ typedef Inertia::Matrix3 Matrix3;
+ typedef Inertia::Symmetric3 Sym3;
+
+ Matrix3 M = Inertia::Matrix3::Random();
+ Sym3 S(M);
+
+ Matrix3 M2 = Inertia::Matrix3::Random();
+ M.triangularView<Eigen::Upper>() = M2;
+
+ Matrix3 A = Matrix3::Random(), ASA1, ASA2;
+ ASA1.triangularView<Eigen::Upper>() = A * S * A.transpose();
+ timeSelfAdj(A,M,ASA2);
+ assert(ASA1.isApprox(ASA2));
+
+ StackTicToc timer(StackTicToc::US); timer.tic();
+ SMOOTH(1000000)
+ {
+ timeSelfAdj(A,M,ASA2);
+ }
+ timer.toc(std::cout,1000000);
+}
+
+
+int main()
+{
+ //testSelfAdj();
+ testLTI();
+ testSym3();
+ testSym3();
+ testLTI();
+
+ std::cout << std::endl;
+ return 0;
+}