diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 8d6b7a56cb81475c7a3ee178b3f35fde3fd6a1a9..fb6d8b9cfd2580fce67d28744c8e2fb8593ad71f 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -214,7 +214,7 @@ namespace se3
typedef JointPlanar Joint;
SE3_JOINT_TYPEDEF;
- typedef Motion::Scalar Scalar;
+ typedef Motion::Scalar_t Scalar;
typedef Eigen::Matrix<Scalar,6,6> Matrix6;
typedef Eigen::Matrix<Scalar,3,3> Matrix3;
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index 7326aa6850fb41bffb203e98525a8c9ea0b23bad..82bd3ace36fce54432a575b87fe447b223f76728 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -231,7 +231,7 @@ namespace se3
typedef JointSphericalZYX Joint;
SE3_JOINT_TYPEDEF;
- typedef Motion::Scalar Scalar;
+ typedef Motion::Scalar_t Scalar;
typedef Eigen::Matrix<Scalar,6,6> Matrix6;
typedef Eigen::Matrix<Scalar,3,3> Matrix3;
diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp
index 86448a15e1f37150f050e10547502c7ba5405c5b..2fa3a84248605ca1e544776a1338156aafd3c815 100644
--- a/src/spatial/motion.hpp
+++ b/src/spatial/motion.hpp
@@ -25,27 +25,127 @@
namespace se3
{
+
+
+ template< class Derived>
+ class MotionBase
+ {
+ protected:
+
+
+ typedef Derived Derived_t;
+ 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 static_cast<const Derived_t*>(this)->angular_impl(); }
+ const Linear_t & linear() const { return static_cast<const Derived_t*>(this)->linear_impl(); }
+ Angular_t & angular() { return static_cast<Derived_t*>(this)->angular_impl(); }
+ Linear_t & linear() { return static_cast<Derived_t*>(this)->linear_impl(); }
+ void angular(const Angular_t & R) { static_cast< Derived_t*>(this)->angular_impl(R); }
+ void linear(const Linear_t & R) { static_cast< Derived_t*>(this)->linear_impl(R); }
+
+ Vector6 toVector() const
+ {
+ return derived().toVector_impl();
+ }
+
+ operator Vector6 () const { return toVector(); }
+
+ ActionMatrix_t toActionMatrix() const
+ {
+ return derived().toActionMatrix_impl();
+ }
+
+ operator Matrix6 () const { return toActionMatrix(); }
+
+
+ Derived_t operator-() const
+ {
+ return derived().__minus__();
+ }
+
+ Derived_t operator+(const Derived_t & v2) const
+ {
+ return derived().__plus__(v2);
+ }
+ Derived_t operator-(const Derived_t & v2) const
+ {
+ return derived().__minus__(v2);
+ }
+ Derived_t& operator+=(const Derived_t & v2)
+ {
+ return derived().__pequ__(v2);
+ }
+
+ Derived_t se3Action(const SE3 & m) const
+ {
+ return derived().se3Action_impl(m);
+ }
+ /// bv = aXb.actInv(av)
+ Derived_t se3ActionInverse(const SE3 & m) const
+ {
+ return derived().se3ActionInverse_impl(m);
+ }
+
+ void disp(std::ostream & os) const
+ {
+ os << "base disp" << std::endl;
+ derived().disp_impl(os);
+ }
+
+ friend std::ostream & operator << (std::ostream & os, const MotionBase<Derived_t> & mv)
+ {
+ os << "base <<" << std::endl;
+ mv.disp(os);
+ return os;
+ }
+
+ };
+
+
+ template<typename T, int U>
+ struct traits< MotionTpl<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 Matrix6 ActionMatrix_t;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_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 MotionTpl
+ class MotionTpl : public MotionBase< MotionTpl< _Scalar, _Options > >
{
public:
- typedef _Scalar Scalar;
- enum { Options = _Options };
- typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
- typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
- typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
- typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
- typedef Matrix6 ActionMatrix;
- typedef ForceTpl<Scalar,Options> Force;
- typedef SE3Tpl<Scalar,Options> SE3;
- enum { LINEAR = 0, ANGULAR = 3 };
+ SPATIAL_TYPEDEF_ARG(MotionTpl);
+
public:
// Constructors
MotionTpl() : m_w(), m_v() {}
+
template<typename v1,typename v2>
MotionTpl(const Eigen::MatrixBase<v1> & v, const Eigen::MatrixBase<v2> & w)
: m_w(w), m_v(v) {}
+
template<typename v6>
explicit MotionTpl(const Eigen::MatrixBase<v6> & v)
: m_w(v.template segment<3>(ANGULAR))
@@ -54,6 +154,8 @@ namespace se3
EIGEN_STATIC_ASSERT_VECTOR_ONLY(v6);
assert( v.size() == 6 );
}
+
+
template<typename S2,int O2>
explicit MotionTpl(const MotionTpl<S2,O2> & clone)
: m_w(clone.angular()),m_v(clone.linear()) {}
@@ -65,18 +167,19 @@ namespace se3
MotionTpl & setZero () { m_v.setZero (); m_w.setZero (); return *this; }
MotionTpl & setRandom () { m_v.setRandom (); m_w.setRandom (); return *this; }
- Vector6 toVector() const
+
+ Vector6 toVector_impl() const
{
Vector6 v;
v.template segment<3>(ANGULAR) = m_w;
v.template segment<3>(LINEAR) = m_v;
return v;
}
- operator Vector6 () const { return toVector(); }
+
- ActionMatrix toActionMatrix () const
+ ActionMatrix_t toActionMatrix_impl () const
{
- ActionMatrix X;
+ ActionMatrix_t X;
X.block <3,3> (ANGULAR, ANGULAR) = X.block <3,3> (LINEAR, LINEAR) = skew (m_w);
X.block <3,3> (LINEAR, ANGULAR) = skew (m_v);
X.block <3,3> (ANGULAR, LINEAR).setZero ();
@@ -85,12 +188,12 @@ namespace se3
}
// Getters
- const Vector3 & angular() const { return m_w; }
- const Vector3 & linear() const { return m_v; }
- Vector3 & angular() { return m_w; }
- Vector3 & linear() { return m_v; }
- void angular(const Vector3 & w) { m_w=w; }
- void linear (const Vector3 & v) { m_v=v; }
+ const Vector3 & angular_impl() const { return m_w; }
+ const Vector3 & linear_impl() const { return m_v; }
+ Vector3 & angular_impl() { return m_w; }
+ Vector3 & linear_impl() { return m_v; }
+ void angular_impl(const Vector3 & w) { m_w=w; }
+ void linear_impl(const Vector3 & v) { m_v=v; }
// Arithmetic operators
template<typename S2, int O2>
@@ -110,20 +213,20 @@ namespace se3
return *this;
}
- MotionTpl operator-() const
+ MotionTpl __minus__() const
{
return MotionTpl(-m_v, -m_w);
}
- MotionTpl operator+(const MotionTpl & v2) const
+ MotionTpl __plus__(const MotionTpl & v2) const
{
return MotionTpl(m_v+v2.m_v,m_w+v2.m_w);
}
- MotionTpl operator-(const MotionTpl & v2) const
+ MotionTpl __minus__(const MotionTpl & v2) const
{
return MotionTpl(m_v-v2.m_v,m_w-v2.m_w);
}
- MotionTpl& operator+=(const MotionTpl & v2)
+ MotionTpl& __pequ__(const MotionTpl & v2)
{
m_v+=v2.m_v;
m_w+=v2.m_w;
@@ -147,30 +250,29 @@ namespace se3
m_w.cross(v2.m_w) );
}
- ForceTpl<Scalar,Options> cross(const ForceTpl<Scalar,Options>& phi) const
+ Force cross(const Force& phi) const
{
- return ForceTpl<Scalar,Options>
+ return Force
( m_w.cross(phi.linear()),
m_w.cross(phi.angular())+m_v.cross(phi.linear()) );
}
- MotionTpl se3Action(const SE3 & m) const
+ MotionTpl se3Action_impl(const SE3 & m) const
{
- Vector3 Rw (static_cast<Vector3>(m.rotation() * angular()));
- return MotionTpl(m.rotation()*linear() + m.translation().cross(Rw), Rw);
+ Vector3 Rw (static_cast<Vector3>(m.rotation() * angular_impl()));
+ return MotionTpl(m.rotation()*linear_impl() + m.translation().cross(Rw), Rw);
}
/// bv = aXb.actInv(av)
- MotionTpl se3ActionInverse(const SE3 & m) const
+ MotionTpl se3ActionInverse_impl(const SE3 & m) const
{
- return MotionTpl(m.rotation().transpose()*(linear()-m.translation().cross(angular())),
- m.rotation().transpose()*angular());
+ return MotionTpl(m.rotation().transpose()*(linear_impl()-m.translation().cross(angular_impl())),
+ m.rotation().transpose()*angular_impl());
}
- friend std::ostream & operator << (std::ostream & os, const MotionTpl & mv)
+ void disp_impl(std::ostream & os) const
{
- os << " v = " << mv.linear().transpose () << std::endl
- << " w = " << mv.angular().transpose () << std::endl;
- return os;
+ os << " v = " << linear_impl().transpose () << std::endl
+ << " w = " << angular_impl().transpose () << std::endl;
}
/** \brief Compute the classical acceleration of point according to the spatial velocity and spatial acceleration of the frame centered on this point