Reduce the constraint size of task-se3-equality based on provided mask, assume...

Reduce the constraint size of task-se3-equality based on provided mask, assume fixed mask for contact-point, rotate position error into world-oriented frame

include/tsid/contacts/contact-point.hpp

@@ -111,7 +111,6 @@ namespace tsid
        * local world oriented
        */
       void useLocalFrame(bool local_frame);
-      void setMotionMask(math::ConstRefVector mask);
 
     protected:
 

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

@@ -63,6 +63,8 @@ namespace tsid
       const Vector & getDesiredAcceleration() const;
       Vector getAcceleration(ConstRefVector dv) const;
 
+      virtual void setMask(math::ConstRefVector mask) override;
+
       const Vector & position_error() const;
       const Vector & velocity_error() const;
       const Vector & position() const;

src/contacts/contact-point.cpp

@@ -45,11 +45,15 @@ ContactPoint::ContactPoint(const std::string & name,
   m_regularizationTaskWeight(regularizationTaskWeight)
 {
   m_weightForceRegTask << 1, 1, 1e-3;
-  m_forceGenMat.resize(6,3);
+  m_forceGenMat.resize(3, 3);
   m_fRef = Vector3::Zero();
   updateForceGeneratorMatrix();
   updateForceInequalityConstraints();
   updateForceRegularizationTask();
+
+  math::Vector motion_mask(6);
+  motion_mask << 1., 1., 1., 0., 0., 0.;
+  m_motionTask.setMask(motion_mask);
 }
 
 void ContactPoint::useLocalFrame(bool local_frame)
@@ -57,11 +61,6 @@ void ContactPoint::useLocalFrame(bool local_frame)
   m_motionTask.useLocalFrame(local_frame);
 }
 
-void ContactPoint::setMotionMask(math::ConstRefVector mask)
-{
-  m_motionTask.setMask(mask);
-}
-
 void ContactPoint::updateForceInequalityConstraints()
 {
   Vector3 t1, t2;
@@ -117,7 +116,7 @@ void ContactPoint:: updateForceGeneratorMatrix()
   m_forceGenMat.setIdentity();
 }
 
-unsigned int ContactPoint::n_motion() const { return 6; }
+unsigned int ContactPoint::n_motion() const { return m_motionTask.dim(); }
 unsigned int ContactPoint::n_force() const { return 3; }
 
 const Vector & ContactPoint::Kp() const { return m_motionTask.Kp(); }

src/tasks/task-se3-equality.cpp

@@ -60,10 +60,15 @@ namespace tsid
       m_local_frame = true;
     }
 
+    void TaskSE3Equality::setMask(math::ConstRefVector mask)
+    {
+      TaskMotion::setMask(mask);
+      m_constraint.resize(dim(), m_J.cols());
+    }
+
     int TaskSE3Equality::dim() const
     {
-      //return self._mask.sum ()
-      return 6;
+      return m_mask.sum();
     }
 
     const Vector & TaskSE3Equality::Kp() const { return m_Kp; }
@@ -177,16 +182,18 @@ namespace tsid
         m_v_error = v_frame - m_wMl.actInv(m_v_ref);  // vel err in local frame
 
         // desired acc in local frame
-        m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
+        m_a_des = - m_Kp.cwiseProduct(m_p_error_vec)
                   - m_Kd.cwiseProduct(m_v_error.toVector())
                   + m_wMl.actInv(m_a_ref).toVector();
       } else {
+        m_p_error_vec = m_wMl.toActionMatrix() *   // pos err in local world-oriented frame
+            m_p_error.toVector();
         m_v_error = m_wMl.act(v_frame) - m_v_ref;  // vel err in local world-oriented frame
 
         m_drift = m_wMl.act(m_drift);
 
         // desired acc in local oriented frame
-        m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
+        m_a_des = - m_Kp.cwiseProduct(m_p_error_vec)
                   - m_Kd.cwiseProduct(m_v_error.toVector())
                   + m_a_ref.toVector();
 
@@ -199,8 +206,18 @@ namespace tsid
       m_v_ref_vec = m_v_ref.toVector();
       m_v = v_frame.toVector();
 
-      m_constraint.setMatrix(m_mask.asDiagonal() * m_J);
-      m_constraint.setVector(m_mask.asDiagonal() * (m_a_des - m_drift.toVector()));
+
+      m_a_des -= m_drift.toVector();
+
+      int idx = 0;
+      for (int i = 0; i < 6; i++) {
+        if (m_mask(i) != 1.) continue;
+
+        m_constraint.matrix().row(idx) = m_J.row(idx);
+        m_constraint.vector().row(idx) = m_a_des.row(idx);
+
+        idx += 1;
+      }
       return m_constraint;
     }
   }

unittest/tsid-formulation.cpp

@@ -162,7 +162,6 @@ class StandardRomeoInvDynCtrl
     postureTask->Kd(2.0*postureTask->Kp().cwiseSqrt());
     tsid->addMotionTask(*postureTask, w_posture, 1);
   }
-
 };
 
 const double StandardRomeoInvDynCtrl::lxp = 0.14;
@@ -553,16 +552,12 @@ BOOST_AUTO_TEST_CASE ( test_contact_point_invdyn_formulation_acc_force )
   std::array<ContactPoint*, 4> contacts;
 
   for (int i = 0; i < 4; i++) {
-    math::Vector motion_mask(6);
-    motion_mask << 1., 1., 1., 0., 0., 0.;
-
     ContactPoint* cp = new ContactPoint("contact_" + contactFrames[i], robot,
         contactFrames[i], contactNormal, mu, fMin, fMax, w_forceRef);
     cp->Kp(kp_contact*Vector::Ones(6));
     cp->Kd(2.0*cp->Kp().cwiseSqrt());
     cp->setReference(robot.framePosition(data, robot.model().getFrameId(contactFrames[i])));
     cp->useLocalFrame(false);
-    cp->setMotionMask(motion_mask);
     tsid->addRigidContact(*cp, 1);
 
     contacts[i] = cp;