Improve TaskSE3Equality: apply mask before returning task acceleration,...

Improve TaskSE3Equality: apply mask before returning task acceleration, desired task acceleration, position error and velocity error.

include/tsid/tasks/task-se3-equality.hpp

@@ -60,13 +60,32 @@ namespace tsid
       void setReference(TrajectorySample & ref);
       const TrajectorySample & getReference() const;
 
+      /** Return the desired task acceleration (after applying the specified mask).
+       *  The value is expressed in local frame is the local_frame flag is true,
+       *  otherwise it is expressed in a local world-oriented frame.
+      */
       const Vector & getDesiredAcceleration() const;
+
+      /** Return the task acceleration (after applying the specified mask).
+       *  The value is expressed in local frame is the local_frame flag is true,
+       *  otherwise it is expressed in a local world-oriented frame.
+      */
       Vector getAcceleration(ConstRefVector dv) const;
 
       virtual void setMask(math::ConstRefVector mask);
 
+      /** Return the position tracking error (after applying the specified mask).
+       *  The error is expressed in local frame is the local_frame flag is true,
+       *  otherwise it is expressed in a local world-oriented frame.
+      */
       const Vector & position_error() const;
+
+      /** Return the velocity tracking error (after applying the specified mask).
+       *  The error is expressed in local frame is the local_frame flag is true,
+       *  otherwise it is expressed in a local world-oriented frame.
+      */
       const Vector & velocity_error() const;
+
       const Vector & position() const;
       const Vector & velocity() const;
       const Vector & position_ref() const;
@@ -81,10 +100,10 @@ namespace tsid
 
       /**
        * @brief Specifies if the jacobian and desired acceloration should be
-       * expressed in the local frame or the local world oriented frame.
+       * expressed in the local frame or the local world-oriented frame.
        *
        * @param local_frame If true, represent jacobian and acceloration in the
-       *   local frame. If false, represent them in the local world oriented frame.
+       *   local frame. If false, represent them in the local world-oriented frame.
        */
       void useLocalFrame(bool local_frame);
 
@@ -94,14 +113,16 @@ namespace tsid
       Index m_frame_id;
       Motion m_p_error, m_v_error;
       Vector m_p_error_vec, m_v_error_vec;
+      Vector m_p_error_masked_vec, m_v_error_masked_vec;
       Vector m_p, m_v;
       Vector m_p_ref, m_v_ref_vec;
       Motion m_v_ref, m_a_ref;
       SE3 m_M_ref, m_wMl;
       Vector m_Kp;
       Vector m_Kd;
-      Vector m_a_des;
+      Vector m_a_des, m_a_des_masked;
       Motion m_drift;
+      Vector m_drift_masked;
       Matrix6x m_J;
       Matrix6x m_J_rotated;
       ConstraintEquality m_constraint;

src/tasks/task-se3-equality.cpp

@@ -56,6 +56,7 @@ namespace tsid
 
       m_mask.resize(6);
       m_mask.fill(1.);
+      setMask(m_mask);
 
       m_local_frame = true;
     }
@@ -63,7 +64,12 @@ namespace tsid
     void TaskSE3Equality::setMask(math::ConstRefVector mask)
     {
       TaskMotion::setMask(mask);
-      m_constraint.resize(dim(), (unsigned int)m_J.cols());
+      int n = dim();
+      m_constraint.resize(n, (unsigned int)m_J.cols());
+      m_p_error_masked_vec.resize(n);
+      m_v_error_masked_vec.resize(n);
+      m_drift_masked.resize(n);
+      m_a_des_masked.resize(n);
     }
 
     int TaskSE3Equality::dim() const
@@ -102,12 +108,12 @@ namespace tsid
 
     const Vector & TaskSE3Equality::position_error() const
     {
-      return m_p_error_vec;
+      return m_p_error_masked_vec;
     }
 
     const Vector & TaskSE3Equality::velocity_error() const
     {
-      return m_v_error_vec;
+      return m_v_error_masked_vec;
     }
 
     const Vector & TaskSE3Equality::position() const
@@ -132,12 +138,12 @@ namespace tsid
 
     const Vector & TaskSE3Equality::getDesiredAcceleration() const
     {
-      return m_a_des;
+      return m_a_des_masked;
     }
 
     Vector TaskSE3Equality::getAcceleration(ConstRefVector dv) const
     {
-      return m_constraint.matrix()*dv + m_drift.toVector();
+      return m_constraint.matrix()*dv + m_drift_masked;
     }
 
     Index TaskSE3Equality::frame_id() const
@@ -192,7 +198,7 @@ namespace tsid
 
         m_drift = m_wMl.act(m_drift);
 
-        // desired acc in local oriented frame
+        // desired acc in local world-oriented frame
         m_a_des = - m_Kp.cwiseProduct(m_p_error_vec)
                   - m_Kd.cwiseProduct(m_v_error.toVector())
                   + m_a_ref.toVector();
@@ -212,6 +218,10 @@ namespace tsid
 
         m_constraint.matrix().row(idx) = m_J.row(i);
         m_constraint.vector().row(idx) = (m_a_des - m_drift.toVector()).row(i);
+        m_a_des_masked(idx)            = m_a_des(i);
+        m_drift_masked(idx)            = m_drift.toVector()(i);
+        m_p_error_masked_vec(idx)      = m_p_error_vec(i);
+        m_v_error_masked_vec(idx)      = m_v_error_vec(i);
 
         idx += 1;
       }