diff --git a/src/spatial/force.hpp b/src/spatial/force.hpp
index 46fbcf0f7679a8dabbe6bb11490b2bf15b5c5961..388ce02cbc04a82ed2463e19e86859cf5d10d083 100644
--- a/src/spatial/force.hpp
+++ b/src/spatial/force.hpp
@@ -35,242 +35,239 @@ namespace se3
SPATIAL_TYPEDEF_ARG(Derived_t);
public:
- Derived_t & derived() { return *static_cast<Derived_t*>(this); }
- const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }
-
- const Angular_t & angular() const { return derived().angular_impl(); }
- const Linear_t & linear() const { return derived().linear_impl(); }
- Angular_t & angular() { return derived().angular_impl(); }
- Linear_t & linear() { return derived().linear_impl(); }
- void angular(const Angular_t & R) { derived().angular_impl(R); }
- void linear(const Linear_t & R) { derived().linear_impl(R); }
-
-
- Vector6 toVector() const
- {
- // std::cout << "2Homo base" << std::endl;
- return derived().toVector_impl();
- }
- operator Vector6 () const { return toVector(); }
-
-
- void disp(std::ostream & os) const
- {
- os << "base disp" << std::endl;
- static_cast<const Derived_t*>(this)->disp_impl(os);
- }
-
- Derived_t & operator= (const Derived_t & other) { return derived().__equl__(other); }
- Derived_t& operator+= (const Derived_t & phi) { return derived().__pequ__(phi); }
- Derived_t operator+(const Derived_t & phi) const { return derived().__plus__(phi); }
- Derived_t operator*(double a) const { return derived().__mult__(a); }
- Derived_t operator-() const { return derived().__minus__(); }
- Derived_t operator-(const Derived_t & phi) const { return derived().__minus__(phi); }
-
-
- /// af = aXb.act(bf)
- Derived_t se3Action(const SE3 & m) const
- {
- return derived().se3Action_impl(m);
- }
-
- Derived_t se3ActionInverse(const SE3 & m) const
- {
- return derived().se3ActionInverse_impl(m);
- }
-
- friend std::ostream & operator << (std::ostream & os,const ForceBase<Derived> & X)
- {
- os << "base <<" << std::endl;
- X.disp(os);
- return os;
- }
+ Derived_t & derived() { return *static_cast<Derived_t*>(this); }
+ const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }
- };
+ const Angular_t & angular() const { return derived().angular_impl(); }
+ const Linear_t & linear() const { return derived().linear_impl(); }
+ Angular_t & angular() { return derived().angular_impl(); }
+ Linear_t & linear() { return derived().linear_impl(); }
+ void angular(const Angular_t & R) { derived().angular_impl(R); }
+ void linear(const Linear_t & R) { derived().linear_impl(R); }
- template<typename T, int U>
- struct traits< ForceTpl<T, U> >
+ Vector6 toVector() const
{
- typedef T Scalar_t;
- typedef Eigen::Matrix<T,3,1,U> Vector3;
- typedef Eigen::Matrix<T,4,1,U> Vector4;
- typedef Eigen::Matrix<T,6,1,U> Vector6;
- typedef Eigen::Matrix<T,3,3,U> Matrix3;
- typedef Eigen::Matrix<T,4,4,U> Matrix4;
- typedef Eigen::Matrix<T,6,6,U> Matrix6;
- typedef Vector3 Angular_t;
- typedef Vector3 Linear_t;
- typedef Matrix6 ActionMatrix_t;
- typedef Eigen::Quaternion<T,U> Quaternion_t;
- typedef SE3Tpl<T,U> SE3;
- typedef ForceTpl<T,U> Force;
- typedef MotionTpl<T,U> Motion;
- typedef Symmetric3Tpl<T,U> Symmetric3;
- enum {
- LINEAR = 0,
- ANGULAR = 3
- };
- };
+ return derived().toVector_impl();
+ }
+ operator Vector6 () const { return toVector(); }
+
- template<typename _Scalar, int _Options>
- class ForceTpl : public ForceBase< ForceTpl< _Scalar, _Options > >
+ void disp(std::ostream & os) const
{
+ os << "base disp" << std::endl;
+ static_cast<const Derived_t*>(this)->disp_impl(os);
+ }
- public:
- friend class ForceBase< ForceTpl< _Scalar, _Options > >;
- SPATIAL_TYPEDEF_ARG(ForceTpl);
+ Derived_t & operator= (const Derived_t & other) { return derived().__equl__(other); }
+ Derived_t& operator+= (const Derived_t & phi) { return derived().__pequ__(phi); }
+ Derived_t operator+(const Derived_t & phi) const { return derived().__plus__(phi); }
+ Derived_t operator*(double a) const { return derived().__mult__(a); }
+ Derived_t operator-() const { return derived().__minus__(); }
+ Derived_t operator-(const Derived_t & phi) const { return derived().__minus__(phi); }
- public:
- ForceTpl() : m_n(), m_f() {}
+ /// af = aXb.act(bf)
+ Derived_t se3Action(const SE3 & m) const
+ {
+ return derived().se3Action_impl(m);
+ }
+
+ Derived_t se3ActionInverse(const SE3 & m) const
+ {
+ return derived().se3ActionInverse_impl(m);
+ }
+
+ friend std::ostream & operator << (std::ostream & os,const ForceBase<Derived> & X)
+ {
+ os << "base <<" << std::endl;
+ X.disp(os);
+ return os;
+ }
+
+ };
- template<typename f3_t,typename n3_t>
- ForceTpl(const Eigen::MatrixBase<f3_t> & f,const Eigen::MatrixBase<n3_t> & n)
- : m_n(n),
- m_f(f)
- {
-
- }
-
- template<typename f6>
- explicit ForceTpl(const Eigen::MatrixBase<f6> & f)
- : m_n(f.template segment<3>(ANGULAR)),
- m_f(f.template segment<3>(LINEAR))
- {
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(f6);
- assert( f.size() == 6 );
- }
-
-
- template<typename S2,int O2>
- explicit ForceTpl(const ForceTpl<S2,O2> & clone)
- : m_n(clone.angular()),
- m_f(clone.linear())
- {
-
- }
-
-
- static ForceTpl Zero() { return ForceTpl(Linear_t::Zero(), Angular_t::Zero()); }
- static ForceTpl Random() { return ForceTpl(Linear_t::Random(), Angular_t::Random()); }
-
-
- ForceTpl & setZero () { m_n.setZero (); m_f.setZero (); return *this; }
- ForceTpl & setRandom () { m_n.setRandom (); m_f.setRandom (); return *this; }
-
-
- public:
- Vector6 toVector_impl() const
- {
- // std::cout << "2Homo derived" << std::endl;
- Vector6 f;
- f.template segment<3>(ANGULAR) = m_n;
- f.template segment<3>(LINEAR) = m_f;
- return f;
- }
-
-
-
- void disp_impl(std::ostream & os) const
- {
- os
- << "f =\n" << m_f << std::endl
- << "tau =\n" << m_n << std::endl;
- }
-
- /// af = aXb.act(bf)
- ForceTpl se3Action_impl(const SE3 & m) const
- {
- Vector3 Rf (static_cast<Vector3>( (m.rotation()) * linear_impl() ) );
- return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular_impl());
- }
-
-
- ForceTpl se3ActionInverse_impl(const SE3 & m) const
- {
- return ForceTpl(m.rotation().transpose()*linear_impl(),
- m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) );
- }
-
- // Arithmetic operators
- template<typename S2, int O2>
- ForceTpl & operator= (const ForceTpl<S2,O2> & other)
- {
- m_n = other.angular ();
- m_f = other.linear ();
- return *this;
- }
-
- template<typename F6>
- ForceTpl & operator=(const Eigen::MatrixBase<F6> & phi)
- {
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(F6); assert(phi.size() == 6);
- m_n = phi.template segment<3>(ANGULAR);
- m_f = phi.template segment<3>(LINEAR);
- return *this;
- }
-
- // template <typename D>
- // ForceTpl operator + (ForceBase<D> a){
- // return ForceTpl(m_f+a.linear(), m_n + a.angular());
- // }
-
- // friend ForceTpl operator*(Scalar a, const ForceTpl & phi)
- // {
- // return ForceTpl(phi.n()*a, phi.f()*a);
- // }
-
- ForceTpl & __equl__(const ForceTpl & other)
- {
- m_n = other.angular();
- m_f = other.linear();
- return *this;
- }
-
-
- ForceTpl& __pequ__ (const ForceTpl & phi)
- {
- m_f += phi.m_f;
- m_n += phi.m_n;
- return *this;
- }
-
- ForceTpl __plus__(const ForceTpl & phi) const
- {
- return ForceTpl(m_f+phi.m_f,m_n+phi.m_n);
- }
-
- ForceTpl __mult__(double a) const
- {
- return ForceTpl(m_f*a, m_n*a);
- }
-
- ForceTpl __minus__() const
- {
- return ForceTpl(-m_f, -m_n);
- }
-
- ForceTpl __minus__(const ForceTpl & phi) const
- {
- return ForceTpl(m_f-phi.m_f,m_n-phi.m_n);
- }
- // SE3Tpl operator*(const SE3Tpl & m2) const { return this->act(m2); }
-
-
- public:
- const Angular_t & angular_impl() const { return m_n; }
- Angular_t & angular_impl() { return m_n; }
- void angular_impl(const Angular_t & R) { m_n = R; }
- const Linear_t & linear_impl() const { return m_f;}
- Linear_t & linear_impl() { return m_f;}
- void linear_impl(const Linear_t & p) { m_f=p; }
-
- protected:
- Angular_t m_n;
- Linear_t m_f;
+ template<typename T, int U>
+ struct traits< ForceTpl<T, U> >
+ {
+ typedef T Scalar_t;
+ typedef Eigen::Matrix<T,3,1,U> Vector3;
+ typedef Eigen::Matrix<T,4,1,U> Vector4;
+ typedef Eigen::Matrix<T,6,1,U> Vector6;
+ typedef Eigen::Matrix<T,3,3,U> Matrix3;
+ typedef Eigen::Matrix<T,4,4,U> Matrix4;
+ typedef Eigen::Matrix<T,6,6,U> Matrix6;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<T,U> Quaternion_t;
+ typedef SE3Tpl<T,U> SE3;
+ typedef ForceTpl<T,U> Force;
+ typedef MotionTpl<T,U> Motion;
+ typedef Symmetric3Tpl<T,U> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
};
+ };
+
+ template<typename _Scalar, int _Options>
+ class ForceTpl : public ForceBase< ForceTpl< _Scalar, _Options > >
+ {
+
+ public:
+ friend class ForceBase< ForceTpl< _Scalar, _Options > >;
+ SPATIAL_TYPEDEF_ARG(ForceTpl);
+
+
+ public:
+ ForceTpl() : m_n(), m_f() {}
+
+
+ template<typename f3_t,typename n3_t>
+ ForceTpl(const Eigen::MatrixBase<f3_t> & f,const Eigen::MatrixBase<n3_t> & n)
+ : m_n(n),
+ m_f(f)
+ {
+
+ }
+
+ template<typename f6>
+ explicit ForceTpl(const Eigen::MatrixBase<f6> & f)
+ : m_n(f.template segment<3>(ANGULAR)),
+ m_f(f.template segment<3>(LINEAR))
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(f6);
+ assert( f.size() == 6 );
+ }
+
+
+ template<typename S2,int O2>
+ explicit ForceTpl(const ForceTpl<S2,O2> & clone)
+ : m_n(clone.angular()),
+ m_f(clone.linear())
+ {
+
+ }
+
+
+ static ForceTpl Zero() { return ForceTpl(Linear_t::Zero(), Angular_t::Zero()); }
+ static ForceTpl Random() { return ForceTpl(Linear_t::Random(), Angular_t::Random()); }
+
+
+ ForceTpl & setZero () { m_n.setZero (); m_f.setZero (); return *this; }
+ ForceTpl & setRandom () { m_n.setRandom (); m_f.setRandom (); return *this; }
+
+
+ public:
+ Vector6 toVector_impl() const
+ {
+ Vector6 f;
+ f.template segment<3>(ANGULAR) = m_n;
+ f.template segment<3>(LINEAR) = m_f;
+ return f;
+ }
+
+
+
+ void disp_impl(std::ostream & os) const
+ {
+ os
+ << "f =\n" << m_f << std::endl
+ << "tau =\n" << m_n << std::endl;
+ }
+
+ /// af = aXb.act(bf)
+ ForceTpl se3Action_impl(const SE3 & m) const
+ {
+ Vector3 Rf (static_cast<Vector3>( (m.rotation()) * linear_impl() ) );
+ return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular_impl());
+ }
+
+
+ ForceTpl se3ActionInverse_impl(const SE3 & m) const
+ {
+ return ForceTpl(m.rotation().transpose()*linear_impl(),
+ m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) );
+ }
+
+ // Arithmetic operators
+ template<typename S2, int O2>
+ ForceTpl & operator= (const ForceTpl<S2,O2> & other)
+ {
+ m_n = other.angular ();
+ m_f = other.linear ();
+ return *this;
+ }
+
+ template<typename F6>
+ ForceTpl & operator=(const Eigen::MatrixBase<F6> & phi)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(F6); assert(phi.size() == 6);
+ m_n = phi.template segment<3>(ANGULAR);
+ m_f = phi.template segment<3>(LINEAR);
+ return *this;
+ }
+
+ // template <typename D>
+ // ForceTpl operator + (ForceBase<D> a){
+ // return ForceTpl(m_f+a.linear(), m_n + a.angular());
+ // }
+
+ // friend ForceTpl operator*(Scalar a, const ForceTpl & phi)
+ // {
+ // return ForceTpl(phi.n()*a, phi.f()*a);
+ // }
+
+ ForceTpl & __equl__(const ForceTpl & other)
+ {
+ m_n = other.angular();
+ m_f = other.linear();
+ return *this;
+ }
+
+
+ ForceTpl& __pequ__ (const ForceTpl & phi)
+ {
+ m_f += phi.m_f;
+ m_n += phi.m_n;
+ return *this;
+ }
+
+ ForceTpl __plus__(const ForceTpl & phi) const
+ {
+ return ForceTpl(m_f+phi.m_f,m_n+phi.m_n);
+ }
+
+ ForceTpl __mult__(double a) const
+ {
+ return ForceTpl(m_f*a, m_n*a);
+ }
+
+ ForceTpl __minus__() const
+ {
+ return ForceTpl(-m_f, -m_n);
+ }
+
+ ForceTpl __minus__(const ForceTpl & phi) const
+ {
+ return ForceTpl(m_f-phi.m_f,m_n-phi.m_n);
+ }
+
+
+ public:
+ const Angular_t & angular_impl() const { return m_n; }
+ Angular_t & angular_impl() { return m_n; }
+ void angular_impl(const Angular_t & R) { m_n = R; }
+ const Linear_t & linear_impl() const { return m_f;}
+ Linear_t & linear_impl() { return m_f;}
+ void linear_impl(const Linear_t & p) { m_f=p; }
+
+ protected:
+ Angular_t m_n;
+ Linear_t m_f;
+ };
typedef ForceTpl<double,0> Force;
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index 4f21929572ef9c46ff454c2f6b6b8e523ec2b2a4..8f1951cd15cfe53cce0dc74343c717f033a3427d 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -26,13 +26,12 @@
namespace se3
{
- template<class C> struct traitsInertia {};
template< class Derived>
class InertiaBase
{
protected:
-
+
typedef Derived Derived_t;
SPATIAL_TYPEDEF_ARG(Derived_t);
@@ -40,49 +39,48 @@ namespace se3
Derived_t & derived() { return *static_cast<Derived_t*>(this); }
const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }
- Scalar_t mass() const { return static_cast<const Derived_t*>(this)->mass(); }
+ Scalar_t mass() const { return static_cast<const Derived_t*>(this)->mass(); }
const Vector3 & lever() const { return static_cast<const Derived_t*>(this)->lever(); }
const Symmetric3 & inertia() const { return static_cast<const Derived_t*>(this)->inertia(); }
- Matrix6 matrix() const
- {
- return derived().matrix_impl();
- }
+ Matrix6 matrix() const
+ {
+ return derived().matrix_impl();
+ }
- operator Matrix6 () const { return matrix(); }
+ operator Matrix6 () const { return matrix(); }
- Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);}
- Derived_t& operator== (const Derived_t& other){return derived().isEqual(other);}
- 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); }
+ Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);}
+ Derived_t& operator== (const Derived_t& other){return derived().isEqual(other);}
+ 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); }
- /// aI = aXb.act(bI)
- Derived_t se3Action(const SE3 & M) const
- {
- return derived().se3Action_impl(M);
- }
+ /// aI = aXb.act(bI)
+ Derived_t se3Action(const SE3 & M) const
+ {
+ return derived().se3Action_impl(M);
+ }
- /// bI = aXb.actInv(aI)
- Derived_t se3ActionInverse(const SE3 & M) const
- {
- return derived().se3ActionInverse_impl(M);
- }
-
- void disp
- (std::ostream & os) const
- {
- os << "base disp" << std::endl;
- static_cast<const Derived_t*>(this)->disp_impl(os);
- }
+ /// bI = aXb.actInv(aI)
+ Derived_t se3ActionInverse(const SE3 & M) const
+ {
+ return derived().se3ActionInverse_impl(M);
+ }
- friend std::ostream & operator << (std::ostream & os,const InertiaBase<Derived_t> & X)
- {
- os << "base <<" << std::endl;
- X.disp(os);
- return os;
- }
+ void disp(std::ostream & os) const
+ {
+ os << "base disp" << std::endl;
+ static_cast<const Derived_t*>(this)->disp_impl(os);
+ }
+
+ friend std::ostream & operator << (std::ostream & os,const InertiaBase<Derived_t> & X)
+ {
+ os << "base <<" << std::endl;
+ X.disp(os);
+ return os;
+ }
};
@@ -109,97 +107,94 @@ namespace se3
LINEAR = 0,
ANGULAR = 3
};
- // typedef typename Derived<T, U>::Vector3 Linear_t;
};
template<typename _Scalar, int _Options>
- class InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > >
- {
+ class InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > >
+ {
public:
friend class InertiaBase< InertiaTpl< _Scalar, _Options > >;
SPATIAL_TYPEDEF_ARG(InertiaTpl);
-
-
public:
// Constructors
- InertiaTpl() : m(), c(), I() {}
-
- InertiaTpl(Scalar_t m_,
- const Vector3 &c_,
- const Matrix3 &I_)
- : m(m_),
- c(c_),
- I(I_)
- {
+ InertiaTpl() : m(), c(), I() {}
+
+ InertiaTpl(Scalar_t m_,
+ const Vector3 &c_,
+ const Matrix3 &I_)
+ : m(m_),
+ c(c_),
+ I(I_)
+ {
- }
+ }
- InertiaTpl(Scalar_t _m,
- const Vector3 &_c,
- const Symmetric3 &_I)
- : m(_m),
- c(_c),
- I(_I)
- {
+ InertiaTpl(Scalar_t _m,
+ const Vector3 &_c,
+ const Symmetric3 &_I)
+ : m(_m),
+ c(_c),
+ I(_I)
+ {
- }
+ }
InertiaTpl(const InertiaTpl & clone) // Clone constructor for std::vector
- : m(clone.m),
- c(clone.c),
- I(clone.I)
- {
+ : m(clone.m),
+ c(clone.c),
+ I(clone.I)
+ {
- }
+ }
- template<typename S2,int O2>
- InertiaTpl( const InertiaTpl<S2,O2> & clone )
- : m(clone.mass()),
- c(clone.lever()),
- I(clone.inertia().matrix())
- {
+ template<typename S2,int O2>
+ InertiaTpl( const InertiaTpl<S2,O2> & clone )
+ : m(clone.mass()),
+ c(clone.lever()),
+ I(clone.inertia().matrix())
+ {
- }
+ }
// Initializers
- static InertiaTpl Zero()
- {
- return InertiaTpl(0.,
- Vector3::Zero(),
- Symmetric3::Zero());
- }
+ static InertiaTpl Zero()
+ {
+ return InertiaTpl(0.,
+ Vector3::Zero(),
+ Symmetric3::Zero());
+ }
- static InertiaTpl Identity()
- {
- return InertiaTpl(1.,
- Vector3::Zero(),
- Symmetric3::Identity());
- }
+ static InertiaTpl Identity()
+ {
+ return InertiaTpl(1.,
+ Vector3::Zero(),
+ Symmetric3::Identity());
+ }
- static InertiaTpl Random()
- {
- /* We have to shoot "I" definite positive and not only symmetric. */
- return InertiaTpl(Eigen::internal::random<Scalar_t>()+1,
- Vector3::Random(),
- Symmetric3::RandomPositive());
- }
+ static InertiaTpl Random()
+ {
+ // We have to shoot "I" definite positive and not only symmetric.
+ return InertiaTpl(Eigen::internal::random<Scalar_t>()+1,
+ Vector3::Random(),
+ Symmetric3::RandomPositive());
+ }
- Matrix6 matrix_impl() const
- {
- Matrix6 M;
- const Matrix3 & c_cross = (skew(c));
- M.template block<3,3>(LINEAR, LINEAR ).template setZero ();
- M.template block<3,3>(LINEAR, LINEAR ).template diagonal ().template fill (m);
- M.template block<3,3>(ANGULAR,LINEAR ) = m * c_cross;
- M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3> (ANGULAR, LINEAR);
- M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)(I - M.template block<3,3>(ANGULAR, LINEAR) * c_cross);
-
- return M;
- }
-
+ Matrix6 matrix_impl() const
+ {
+ Matrix6 M;
+ const Matrix3 & c_cross = (skew(c));
+ M.template block<3,3>(LINEAR, LINEAR ).template setZero ();
+ M.template block<3,3>(LINEAR, LINEAR ).template diagonal ().template fill (m);
+ M.template block<3,3>(ANGULAR,LINEAR ) = m * c_cross;
+ M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3> (ANGULAR, LINEAR);
+ M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)(I - M.template block<3,3>(ANGULAR, LINEAR) * c_cross);
+
+ return M;
+ }
+
// Arithmetic operators
InertiaTpl& __equl__ (const InertiaTpl& clone) // Copy operator for std::vector
{
@@ -207,7 +202,7 @@ namespace se3
return *this;
}
- /* Requiered by std::vector boost::python bindings. */
+ // Requiered by std::vector boost::python bindings.
bool isEqual( const InertiaTpl& Y2 )
{
return (m==Y2.m) && (c==Y2.c) && (I==Y2.I);
@@ -223,8 +218,8 @@ namespace se3
const double & mab = m+Yb.m;
const Vector3 & AB = (c-Yb.c).eval();
return InertiaTpl( mab,
- (m*c+Yb.m*Yb.c)/mab,
- I+Yb.I - (m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB));
+ (m*c+Yb.m*Yb.c)/mab,
+ I+Yb.I - (m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB));
}
InertiaTpl& __pequ__(const InertiaTpl &Yb)
@@ -245,26 +240,26 @@ namespace se3
m*c.cross(v.linear()) + I*v.angular() - c.cross(mcxw) );
}
- // Getters
- Scalar_t mass() const { return m; }
- const Vector3 & lever() const { return c; }
- const Symmetric3 & inertia() const { return I; }
+ // Getters
+ Scalar_t mass() const { return m; }
+ const Vector3 & lever() const { return c; }
+ const Symmetric3 & inertia() const { return I; }
- /// aI = aXb.act(bI)
+ /// aI = aXb.act(bI)
InertiaTpl se3Action_impl(const SE3 & M) const
{
/* The multiplication RIR' has a particular form that could be used, however it
* does not seems to be more efficient, see http://stackoverflow.com/questions/
* 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
- return InertiaTpl(m,
- M.translation()+M.rotation()*c,
- I.rotate(M.rotation()) );
- }
+ return InertiaTpl( m,
+ M.translation()+M.rotation()*c,
+ I.rotate(M.rotation()) );
+ }
- /// bI = aXb.actInv(aI)
+ ///bI = aXb.actInv(aI)
InertiaTpl se3ActionInverse_impl(const SE3 & M) const
{
return InertiaTpl(m,
@@ -272,30 +267,28 @@ namespace se3
I.rotate(M.rotation().transpose()) );
}
- //
- Force vxiv( const Motion& v ) const
- {
- 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) );
- }
+ Force vxiv( const Motion& v ) const
+ {
+ 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) );
+ }
- void disp_impl(std::ostream &os) const
+ void disp_impl(std::ostream &os) const
{
os << "m =" << m << ";\n"
- << "c = [\n" << c.transpose() << "]';\n"
- << "I = [\n" << (Matrix3)I << "];";
+ << "c = [\n" << c.transpose() << "]';\n"
+ << "I = [\n" << (Matrix3)I << "];";
}
- public:
- private:
- Scalar_t m;
- Vector3 c;
- Symmetric3 I;
- };
+ private:
+ Scalar_t m;
+ Vector3 c;
+ Symmetric3 I;
+ };
- typedef InertiaTpl<double,0> Inertia;
+ typedef InertiaTpl<double,0> Inertia;
} // namespace se3
diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp
index efadc0cdb25fc360203e3252a62fdd7c6fdc0a45..61dc69bff76c775a2eb6315c5781fd6d687717c2 100644
--- a/src/spatial/motion.hpp
+++ b/src/spatial/motion.hpp
@@ -31,7 +31,7 @@ namespace se3
class MotionBase
{
protected:
-
+
typedef Derived Derived_t;
SPATIAL_TYPEDEF_ARG(Derived_t);
@@ -48,16 +48,16 @@ namespace se3
void linear(const Linear_t & R) { static_cast< Derived_t*>(this)->linear_impl(R); }
Vector6 toVector() const
- {
- return derived().toVector_impl();
- }
+ {
+ return derived().toVector_impl();
+ }
operator Vector6 () const { return toVector(); }
ActionMatrix_t toActionMatrix() const
- {
- return derived().toActionMatrix_impl();
- }
+ {
+ return derived().toActionMatrix_impl();
+ }
operator Matrix6 () const { return toActionMatrix(); }
@@ -91,10 +91,10 @@ namespace se3
}
void disp(std::ostream & os) const
- {
- os << "base disp" << std::endl;
- derived().disp_impl(os);
- }
+ {
+ os << "base disp" << std::endl;
+ derived().disp_impl(os);
+ }
friend std::ostream & operator << (std::ostream & os, const MotionBase<Derived_t> & mv)
{
@@ -144,12 +144,12 @@ namespace se3
template<typename v1,typename v2>
MotionTpl(const Eigen::MatrixBase<v1> & v, const Eigen::MatrixBase<v2> & w)
- : m_w(w), m_v(v) {}
+ : m_w(w), m_v(v) {}
template<typename v6>
explicit MotionTpl(const Eigen::MatrixBase<v6> & v)
- : m_w(v.template segment<3>(ANGULAR))
- , m_v(v.template segment<3>(LINEAR))
+ : m_w(v.template segment<3>(ANGULAR))
+ , m_v(v.template segment<3>(LINEAR))
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(v6);
assert( v.size() == 6 );
@@ -158,7 +158,7 @@ namespace se3
template<typename S2,int O2>
explicit MotionTpl(const MotionTpl<S2,O2> & clone)
- : m_w(clone.angular()),m_v(clone.linear()) {}
+ : m_w(clone.angular()),m_v(clone.linear()) {}
// initializers
static MotionTpl Zero() { return MotionTpl(Vector3::Zero(), Vector3::Zero()); }
@@ -247,26 +247,26 @@ namespace se3
MotionTpl cross(const MotionTpl& v2) const
{
return MotionTpl( m_v.cross(v2.m_w)+m_w.cross(v2.m_v),
- m_w.cross(v2.m_w) );
+ m_w.cross(v2.m_w) );
}
Force cross(const Force& phi) const
{
- return Force
- ( m_w.cross(phi.linear()),
- m_w.cross(phi.angular())+m_v.cross(phi.linear()) );
+ return Force( m_w.cross(phi.linear()),
+ m_w.cross(phi.angular())+m_v.cross(phi.linear()) );
}
MotionTpl se3Action_impl(const SE3 & m) const
{
Vector3 Rw (static_cast<Vector3>(m.rotation() * angular_impl()));
- return MotionTpl(m.rotation()*linear_impl() + m.translation().cross(Rw), Rw);
+ return MotionTpl( m.rotation()*linear_impl() + m.translation().cross(Rw),
+ Rw);
}
/// bv = aXb.actInv(av)
MotionTpl se3ActionInverse_impl(const SE3 & m) const
{
- return MotionTpl(m.rotation().transpose()*(linear_impl()-m.translation().cross(angular_impl())),
- m.rotation().transpose()*angular_impl());
+ return MotionTpl( m.rotation().transpose()*(linear_impl()-m.translation().cross(angular_impl())),
+ m.rotation().transpose()*angular_impl());
}
void disp_impl(std::ostream & os) const
@@ -283,7 +283,8 @@ namespace se3
}
/**
- \brief Compute the spatial motion quantity of the parallel frame translated by translation_vector */
+ \brief Compute the spatial motion quantity of the parallel frame translated by translation_vector
+ */
MotionTpl translate (const Vector3 & translation_vector) const
{
return MotionTpl (m_v + m_w.cross (translation_vector), m_w);
@@ -293,7 +294,7 @@ namespace se3
private:
Vector3 m_w;
Vector3 m_v;
- };
+ }; // class MotionTpl
template<typename S,int O>
MotionTpl<S,O> operator^( const MotionTpl<S,O> &m1, const MotionTpl<S,O> &m2 ) { return m1.cross(m2); }
@@ -303,35 +304,36 @@ namespace se3
typedef MotionTpl<double> Motion;
-struct BiasZero;
-template<>
-struct traits< BiasZero >
-{
-typedef double Scalar_t;
-typedef Eigen::Matrix<double,3,1,0> Vector3;
-typedef Eigen::Matrix<double,4,1,0> Vector4;
-typedef Eigen::Matrix<double,6,1,0> Vector6;
-typedef Eigen::Matrix<double,3,3,0> Matrix3;
-typedef Eigen::Matrix<double,4,4,0> Matrix4;
-typedef Eigen::Matrix<double,6,6,0> Matrix6;
-typedef Matrix6 ActionMatrix_t;
-typedef Vector3 Angular_t;
-typedef Vector3 Linear_t;
-typedef Eigen::Quaternion<double,0> Quaternion_t;
-typedef SE3Tpl<double,0> SE3;
-typedef ForceTpl<double,0> Force;
-typedef MotionTpl<double,0> Motion;
-typedef Symmetric3Tpl<double,0> Symmetric3;
-enum {
-LINEAR = 0,
-ANGULAR = 3
-};
-};
-
-struct BiasZero : public MotionBase< BiasZero >
-{
-operator Motion () const { return Motion::Zero(); }
-}; // struct BiasZero
+ struct BiasZero;
+
+ template<>
+ struct traits< BiasZero >
+ {
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ };
+
+ struct BiasZero : public MotionBase< BiasZero >
+ {
+ operator Motion () const { return Motion::Zero(); }
+ }; // struct BiasZero
const Motion & operator+( const Motion& v, const BiasZero&) { return v; }
const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index 1d28912cc774cdb996fd9c987407733e22bcf4e2..736131ddcec13839b20bf4c48861678ce930bc81 100644
--- a/src/spatial/se3.hpp
+++ b/src/spatial/se3.hpp
@@ -69,17 +69,14 @@ namespace se3
void translation(const Linear_t & R) { derived().translation_impl(R); }
- // Unable to change this with Matrix4 toHomogen...
Matrix4 toHomogeneousMatrix() const
{
- // std::cout << "2Homo base" << std::endl;
return derived().toHomogeneousMatrix_impl();
}
operator Matrix4() const { return toHomogeneousMatrix(); }
Matrix6 toActionMatrix() const
{
- // std::cout << "2Action base" << std::endl;
return derived().toActionMatrix_impl();
}
operator Matrix6() const { return toActionMatrix(); }
@@ -93,20 +90,19 @@ namespace se3
Derived_t operator*(const Derived_t & m2) const { return derived().__mult__(m2); }
- // ay = aXb.act(by)
+ /// ay = aXb.act(by)
template<typename D>
typename internal::ActionReturn<D>::Type act (const D & d) const
{
return derived().act_impl(d);
}
- /// by = aXb.actInv(ay)
+
+ /// by = aXb.actInv(ay)
template<typename D> typename internal::ActionReturn<D>::Type actInv(const D & d) const
{
return derived().actInv_impl(d);
}
- // Vector3 act (const Vector3& p) const { return derived().act_impl(p); }
- // Vector3 actInv(const Vector3& p) const { return derived().actInv_impl(p); }
Derived_t act (const Derived_t& m2) const { return derived().act_impl(m2); }
Derived_t actInv(const Derived_t& m2) const { return derived().actInv_impl(m2); }
@@ -121,171 +117,160 @@ namespace se3
};
- template<typename T, int U>
- struct traits< SE3Tpl<T, U> >
- {
- typedef T Scalar_t;
- typedef Eigen::Matrix<T,3,1,U> Vector3;
- typedef Eigen::Matrix<T,4,1,U> Vector4;
- typedef Eigen::Matrix<T,6,1,U> Vector6;
- typedef Eigen::Matrix<T,3,3,U> Matrix3;
- typedef Eigen::Matrix<T,4,4,U> Matrix4;
- typedef Eigen::Matrix<T,6,6,U> Matrix6;
- typedef Matrix3 Angular_t;
- typedef Vector3 Linear_t;
- typedef Matrix6 ActionMatrix_t;
- typedef Eigen::Quaternion<T,U> Quaternion_t;
- typedef SE3Tpl<T,U> SE3;
- typedef ForceTpl<T,U> Force;
- typedef MotionTpl<T,U> Motion;
- typedef Symmetric3Tpl<T,U> Symmetric3;
- enum {
- LINEAR = 0,
- ANGULAR = 3
- };
+ template<typename T, int U>
+ struct traits< SE3Tpl<T, U> >
+ {
+ typedef T Scalar_t;
+ typedef Eigen::Matrix<T,3,1,U> Vector3;
+ typedef Eigen::Matrix<T,4,1,U> Vector4;
+ typedef Eigen::Matrix<T,6,1,U> Vector6;
+ typedef Eigen::Matrix<T,3,3,U> Matrix3;
+ typedef Eigen::Matrix<T,4,4,U> Matrix4;
+ typedef Eigen::Matrix<T,6,6,U> Matrix6;
+ typedef Matrix3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<T,U> Quaternion_t;
+ typedef SE3Tpl<T,U> SE3;
+ typedef ForceTpl<T,U> Force;
+ typedef MotionTpl<T,U> Motion;
+ typedef Symmetric3Tpl<T,U> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
};
+ };
- template<typename _Scalar, int _Options>
- class SE3Tpl : public SE3Base< SE3Tpl< _Scalar, _Options > >
- {
-
- public:
- friend class SE3Base< SE3Tpl< _Scalar, _Options > >;
- SPATIAL_TYPEDEF_ARG(SE3Tpl);
-
+ template<typename _Scalar, int _Options>
+ class SE3Tpl : public SE3Base< SE3Tpl< _Scalar, _Options > >
+ {
- public:
- SE3Tpl(): rot(), trans() {};
+ public:
+ friend class SE3Base< SE3Tpl< _Scalar, _Options > >;
+ SPATIAL_TYPEDEF_ARG(SE3Tpl);
- template<typename M3,typename v3>
- SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
- : rot(R), trans(p)
- {
- }
+ public:
+ SE3Tpl(): rot(), trans() {};
- SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
- template<typename S2, int O2>
- SE3Tpl( const SE3Tpl<S2,O2> & clone ) //cf SE3Tpl
- : rot(clone.rotation()),trans(clone.translation()) {}
+ template<typename M3,typename v3>
+ SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
+ : rot(R), trans(p)
+ {
+ }
- template<typename S2, int O2>
- SE3Tpl & operator= (const SE3Tpl<S2,O2> & other) // cf SE3TplTpl
- {
- rot = other.rotation ();
- trans = other.translation ();
- return *this;
- }
+ SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
- static SE3Tpl Identity()
- {
- return SE3Tpl(1);
- }
+ template<typename S2, int O2>
+ SE3Tpl( const SE3Tpl<S2,O2> & clone )
+ : rot(clone.rotation()),trans(clone.translation()) {}
- SE3Tpl & setIdentity () { rot.setIdentity (); trans.setZero (); return *this;}
+ template<typename S2, int O2>
+ SE3Tpl & operator= (const SE3Tpl<S2,O2> & other)
+ {
+ rot = other.rotation ();
+ trans = other.translation ();
+ return *this;
+ }
- /// aXb = bXa.inverse()
- SE3Tpl inverse() const
- {
- return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
- }
+ static SE3Tpl Identity()
+ {
+ return SE3Tpl(1);
+ }
- static SE3Tpl Random()
- {
- Quaternion_t q(Vector4::Random());
- q.normalize();
- return SE3Tpl(q.matrix(),Vector3::Random());
- }
+ SE3Tpl & setIdentity () { rot.setIdentity (); trans.setZero (); return *this;}
- SE3Tpl & setRandom ()
- {
- Quaternion_t q(Vector4::Random());
- q.normalize ();
- rot = q.matrix ();
- trans.setRandom ();
+ /// aXb = bXa.inverse()
+ SE3Tpl inverse() const
+ {
+ return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
+ }
- return *this;
- }
+ static SE3Tpl Random()
+ {
+ Quaternion_t q(Vector4::Random());
+ q.normalize();
+ return SE3Tpl(q.matrix(),Vector3::Random());
+ }
- public:
- // Unable to change this with Matrix4 toHomogen...
- Matrix4 toHomogeneousMatrix_impl() const
- {
- // std::cout << "2Homo derived" << std::endl;
- Matrix4 M;
- M.template block<3,3>(LINEAR,LINEAR) = rot;
- M.template block<3,1>(LINEAR,ANGULAR) = trans;
- M.template block<1,3>(ANGULAR,LINEAR).setZero();
- M(3,3) = 1;
- return M;
- }
+ SE3Tpl & setRandom ()
+ {
+ Quaternion_t q(Vector4::Random());
+ q.normalize ();
+ rot = q.matrix ();
+ trans.setRandom ();
- /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
- Matrix6 toActionMatrix_impl() const
- {
- Matrix6 M;
- M.template block<3,3>(ANGULAR,ANGULAR)
- = M.template block<3,3>(LINEAR,LINEAR) = rot;
- M.template block<3,3>(ANGULAR,LINEAR).setZero();
- M.template block<3,3>(LINEAR,ANGULAR)
- = skew(trans) * M.template block<3,3>(ANGULAR,ANGULAR);
- return M;
- }
+ return *this;
+ }
- void disp_impl(std::ostream & os) const
- {
- os << "SE3Tpl disp" << std::endl;
- os << " R =\n" << rot << std::endl
- << " p = " << trans.transpose() << std::endl;
- }
+ public:
+ Matrix4 toHomogeneousMatrix_impl() const
+ {
+ // std::cout << "2Homo derived" << std::endl;
+ Matrix4 M;
+ M.template block<3,3>(LINEAR,LINEAR) = rot;
+ M.template block<3,1>(LINEAR,ANGULAR) = trans;
+ M.template block<1,3>(ANGULAR,LINEAR).setZero();
+ M(3,3) = 1;
+ return M;
+ }
+
+ /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
+ Matrix6 toActionMatrix_impl() const
+ {
+ Matrix6 M;
+ M.template block<3,3>(ANGULAR,ANGULAR)
+ = M.template block<3,3>(LINEAR,LINEAR) = rot;
+ M.template block<3,3>(ANGULAR,LINEAR).setZero();
+ M.template block<3,3>(LINEAR,ANGULAR)
+ = skew(trans) * M.template block<3,3>(ANGULAR,ANGULAR);
+ return M;
+ }
+
+ void disp_impl(std::ostream & os) const
+ {
+ os << "SE3Tpl disp" << std::endl;
+ os << " R =\n" << rot << std::endl
+ << " p = " << trans.transpose() << std::endl;
+ }
- /// --- GROUP ACTIONS ON M6, F6 and I6 ---
- // ay = aXb.act(by)
- template<typename D>
- typename internal::ActionReturn<D>::Type act_impl (const D & d) const
- {
- return d.se3Action(*this);
- }
- /// by = aXb.actInv(ay)
- template<typename D> typename internal::ActionReturn<D>::Type actInv_impl(const D & d) const
- {
- return d.se3ActionInverse(*this);
- }
+ /// --- GROUP ACTIONS ON M6, F6 and I6 ---
- Vector3 act_impl (const Vector3& p) const { return (rot*p+trans).eval(); }
- Vector3 actInv_impl(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }
+ /// ay = aXb.act(by)
+ template<typename D>
+ typename internal::ActionReturn<D>::Type act_impl (const D & d) const
+ {
+ return d.se3Action(*this);
+ }
+ /// by = aXb.actInv(ay)
+ template<typename D> typename internal::ActionReturn<D>::Type actInv_impl(const D & d) const
+ {
+ return d.se3ActionInverse(*this);
+ }
- SE3Tpl act_impl (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
- SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}
+ Vector3 act_impl (const Vector3& p) const { return (rot*p+trans).eval(); }
+ Vector3 actInv_impl(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }
- /// Operators
- // operator Matrix4() const { return toHomogeneousMatrix(); }
- // operator Matrix6() const { return toActionMatrix(); }
+ SE3Tpl act_impl (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
+ SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}
- // friend std::ostream & operator << (std::ostream & os,const SE3Tpl & X)
- // {
- // os << "derived <<" << std::endl;
- // return os;
- // // X.disp(os); return os;
- // }
- SE3Tpl __mult__(const SE3Tpl & m2) const { return this->act(m2);}
- // SE3Tpl operator*(const SE3Tpl & m2) const { return this->act(m2); }
+ SE3Tpl __mult__(const SE3Tpl & m2) const { return this->act(m2);}
- public:
- const Angular_t & rotation_impl() const { return rot; }
- Angular_t & rotation_impl() { return rot; }
- void rotation_impl(const Angular_t & R) { rot = R; }
- const Linear_t & translation_impl() const { return trans;}
- Linear_t & translation_impl() { return trans;}
- void translation_impl(const Linear_t & p) { trans=p; }
+ public:
+ const Angular_t & rotation_impl() const { return rot; }
+ Angular_t & rotation_impl() { return rot; }
+ void rotation_impl(const Angular_t & R) { rot = R; }
+ const Linear_t & translation_impl() const { return trans;}
+ Linear_t & translation_impl() { return trans;}
+ void translation_impl(const Linear_t & p) { trans=p; }
- protected:
- Angular_t rot;
- Linear_t trans;
- };
+ protected:
+ Angular_t rot;
+ Linear_t trans;
+ };
typedef SE3Tpl<double,0> SE3;