[Spatial] Add operator-= to Force and Motion classes

src/spatial/force.hpp

@@ -127,10 +127,17 @@ namespace se3
      */
     Derived_t & operator= (const Derived_t & other) { return derived().__equl__(other); }
 
-    /** \brief replaces *this by *this + other.
-     *  \return a reference to *this
+    /** 
+     * \brief Replaces *this by *this + other.
+     * \return a reference to *this
      */
     Derived_t & operator+= (const Derived_t & phi) { return derived().__pequ__(phi); }
+    
+    /** 
+     * \brief Replaces *this by *this - other.
+     * \return a reference to *this
+     */
+    Derived_t & operator-= (const Derived_t & phi) { return derived().__mequ__(phi); }
 
     /** \return an expression of the sum of *this and other
      */
@@ -307,6 +314,7 @@ namespace se3
 
     ForceTpl & __equl__(const ForceTpl & other) { data = other.data; return *this; }
     ForceTpl & __pequ__ (const ForceTpl & phi) { data += phi.data; return *this; }
+    ForceTpl & __mequ__ (const ForceTpl & phi) { data -= phi.data; return *this; }
     ForceTpl __plus__(const ForceTpl & phi) const { return ForceTpl(data + phi.data); }
     ForceTpl __mult__(const double a) const { return ForceTpl(a*data); }
     ForceTpl __minus__() const { return ForceTpl(-data); }

src/spatial/motion.hpp

@@ -62,6 +62,7 @@ namespace se3
     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 & operator-=(const Derived_t & v2) { return derived().__mequ__(v2); }
 
     bool isApprox (const Derived_t & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
     { return derived().isApprox_impl(other, prec);}
@@ -192,6 +193,7 @@ namespace se3
     MotionTpl __plus__(const MotionTpl & v2) const { return MotionTpl(data + v2.data); }
     MotionTpl __minus__(const MotionTpl & v2) const { return MotionTpl(data - v2.data); }
     MotionTpl& __pequ__(const MotionTpl & v2) { data += v2.data; return *this; }
+    MotionTpl& __mequ__(const MotionTpl & v2) { data -= v2.data; return *this; }
     
     Scalar dot(const Force & f) const { return data.dot(f.toVector()); }