Distinguish between active and inactive contacts in constraints

ARC-OPT · May 6, 2024 · 79c0e55 · 79c0e55
1 parent 4beec62
commit 79c0e55
Show file tree

Hide file tree

Showing 7 changed files with 95 additions and 64 deletions.
diff --git a/src/constraints/ContactsAccelerationConstraint.cpp b/src/constraints/ContactsAccelerationConstraint.cpp
@@ -9,23 +9,29 @@ namespace wbc{
         uint nj = robot_model->noOfJoints();
         uint na = robot_model->noOfActuatedJoints();
         uint nc = contacts.size();
+        uint nac = contacts.getNumberOfActiveContacts();
 
         // number of optimization variables:
         // - Reduced TSID (no torques): Number robot joints nj (incl. floating base) + 6 x number of contacts nc
         // - Full TSID: Number robot joints (incl. floating base) nj + Number actuated robot joints na + 6 x number of contacts nc
         uint nv = reduced ? (nj + nc*6) : (nj + na + nc*6);
 
-        // Constrain only contact positions (number of constraints = 3 x number of contacts), not orientations (e.g. in case of point contact, the rotation is allowed to change)
-        A_mtx.resize(nc*3, nv);
-        b_vec.resize(nc*3);
+        // Constrain only contact positions (number of constraints = 3 x number of contacts), not orientations
+        // (e.g. in case of point contact, the rotation is allowed to change). Also constrain only the active contacts
+        A_mtx.resize(nac*3, nv);
+        b_vec.resize(nac*3);
 
         A_mtx.setZero();
         b_vec.setZero();
 
+        uint row_idx = 0;
         for(uint i = 0; i < contacts.size(); i++){
-            const base::Acceleration& a = robot_model->spatialAccelerationBias(robot_model->worldFrame(), contacts.names[i]);
-            b_vec.segment(i*3, 3) = -a.linear; // use only linear part of spatial acceleration bias
-            A_mtx.block(i*3,  0, 3, nj) = robot_model->spaceJacobian(robot_model->worldFrame(), contacts.names[i]).topRows<3>(); // use only top 3 rows of Jacobian (only linear part)
+            if(contacts[i].active){
+                const base::Acceleration& a = robot_model->spatialAccelerationBias(robot_model->worldFrame(), contacts.names[i]);
+                b_vec.segment(row_idx*3, 3) = -a.linear; // use only linear part of spatial acceleration bias
+                A_mtx.block(row_idx*3,  0, 3, nj) = robot_model->spaceJacobian(robot_model->worldFrame(), contacts.names[i]).topRows<3>(); // use only top 3 rows of Jacobian (only linear part)
+                row_idx++;
+            }
         }
     }
 

diff --git a/src/constraints/ContactsFrictionPointConstraint.cpp b/src/constraints/ContactsFrictionPointConstraint.cpp
@@ -11,39 +11,43 @@ void ContactsFrictionPointConstraint::update(RobotModelPtr robot_model){
     uint nj = robot_model->noOfJoints();
     uint na = robot_model->noOfActuatedJoints();
     uint nc = contacts.size();
+    uint nac = contacts.getNumberOfActiveContacts();
 
     uint nv = reduced ? nj + 6*nc : nj + na + 6*nc;
 
     const uint row_skip = use_torques ? 8 : 4;
     const uint col_skip = use_torques ? 6 : 3;
 
-    A_mtx.resize(nc*row_skip, nv);
-    lb_vec.resize(nc*row_skip);
-    ub_vec.resize(nc*row_skip);
+    A_mtx.resize(nac*row_skip, nv);
+    lb_vec.resize(nac*row_skip);
+    ub_vec.resize(nac*row_skip);
 
     A_mtx.setZero();
     ub_vec.setZero();
     lb_vec.setZero();
 
     uint start_idx = reduced ? nj : nj + na;
-
+    uint idx = 0;
     for(uint i = 0; i < contacts.size(); i++){
 
-        double mu=contacts[i].mu;
-
-        // We assume that contact surface normal is always world_z, TODO: Make this dynamically (re-)configurable
-        Eigen::MatrixXd a(row_skip,col_skip);
-        a << 1,0,-mu,
-             0,1,-mu,
-             1,0, mu,
-             0,1, mu;
-        Eigen::VectorXd lb(row_skip), ub(row_skip);
-        lb << -1e10,-1e10,0,0;
-        ub << 0,0,1e10,1e10;
-
-        lb_vec.segment(i*row_skip,row_skip) = lb;
-        ub_vec.segment(i*row_skip,row_skip) = ub;
-        A_mtx.block<row_skip,col_skip>(i*row_skip,start_idx+i*6) = a;
+        if(contacts[i].active){
+            double mu=contacts[i].mu;
+
+            // We assume that contact surface normal is always world_z, TODO: Make this dynamically (re-)configurable
+            Eigen::MatrixXd a(row_skip,col_skip);
+            a << 1,0,-mu,
+                 0,1,-mu,
+                 1,0, mu,
+                 0,1, mu;
+            Eigen::VectorXd lb(row_skip), ub(row_skip);
+            lb << -1e10,-1e10,0,0;
+            ub << 0,0,1e10,1e10;
+
+            lb_vec.segment(idx*row_skip,row_skip) = lb;
+            ub_vec.segment(idx*row_skip,row_skip) = ub;
+            A_mtx.block<row_skip,col_skip>(idx*row_skip,start_idx+i*6) = a;
+            idx++;
+        }
     }
 }
 

diff --git a/src/constraints/ContactsFrictionSurfaceConstraint.cpp b/src/constraints/ContactsFrictionSurfaceConstraint.cpp
@@ -9,50 +9,56 @@ void ContactsFrictionSurfaceConstraint::update(RobotModelPtr robot_model){
     uint nj = robot_model->noOfJoints();
     uint na = robot_model->noOfActuatedJoints();
     uint nc = contacts.size();
+    uint nac = contacts.getNumberOfActiveContacts();
 
     uint nv = reduced ? nj + 6*nc : nj + na + 6*nc;
 
     const uint row_skip = 16, col_skip = 6;
 
-    A_mtx.resize(nc*row_skip, nv);
-    lb_vec.resize(nc*row_skip);
-    ub_vec.resize(nc*row_skip);
+    A_mtx.resize(nac*row_skip, nv);
+    lb_vec.resize(nac*row_skip);
+    ub_vec.resize(nac*row_skip);
 
     A_mtx.setZero();
     ub_vec.setZero();
     lb_vec.setZero();
 
     uint start_idx = reduced ? nj : nj + na;
 
+    uint idx = 0;
     for(uint i = 0; i < nc; i++){
-        double mu=contacts[i].mu;
-        double wx = contacts[i].wx, wy = contacts[i].wy;
-
-        Eigen::MatrixXd a(row_skip,col_skip);
-        a << -1,  0, -mu,  0,  0, 0,
-              1,  0, -mu,  0,  0, 0,
-              0, -1, -mu,  0,  0, 0,
-              0,  1, -mu,  0,  0, 0,
-              0,  0, -wy, -1,  0, 0,
-              0,  0, -wy,  1,  0, 0,
-              0,  0, -wx,  0, -1, 0,
-              0,  0, -wx,  0,  1, 0,
-              -wy, -wx, -(wx+wy)*mu,  mu,  mu, -1,
-              -wy,  wx, -(wx+wy)*mu,  mu, -mu, -1,
-               wy, -wx, -(wx+wy)*mu, -mu,  mu, -1,
-               wy,  wx, -(wx+wy)*mu, -mu, -mu, -1,
-               wy,  wx, -(wx+wy)*mu,  mu,  mu,  1,
-               wy, -wx, -(wx+wy)*mu,  mu, -mu,  1,
-              -wy,  wx, -(wx+wy)*mu, -mu,  mu,  1,
-              -wy, -wx, -(wx+wy)*mu, -mu, -mu,  1;
-
-        Eigen::VectorXd lb(row_skip), ub(row_skip);
-        lb.setConstant(-1e10);
-        ub.setZero();
-
-        lb_vec.segment(i*row_skip,row_skip) = lb;
-        ub_vec.segment(i*row_skip,row_skip) = ub;
-        A_mtx.block<row_skip,col_skip>(i*row_skip,start_idx+i*6) = a;
+
+        if(contacts[i].active){
+            double mu=contacts[i].mu;
+            double wx = contacts[i].wx, wy = contacts[i].wy;
+
+            Eigen::MatrixXd a(row_skip,col_skip);
+            a << -1,  0, -mu,  0,  0, 0,
+                  1,  0, -mu,  0,  0, 0,
+                  0, -1, -mu,  0,  0, 0,
+                  0,  1, -mu,  0,  0, 0,
+                  0,  0, -wy, -1,  0, 0,
+                  0,  0, -wy,  1,  0, 0,
+                  0,  0, -wx,  0, -1, 0,
+                  0,  0, -wx,  0,  1, 0,
+                  -wy, -wx, -(wx+wy)*mu,  mu,  mu, -1,
+                  -wy,  wx, -(wx+wy)*mu,  mu, -mu, -1,
+                   wy, -wx, -(wx+wy)*mu, -mu,  mu, -1,
+                   wy,  wx, -(wx+wy)*mu, -mu, -mu, -1,
+                   wy,  wx, -(wx+wy)*mu,  mu,  mu,  1,
+                   wy, -wx, -(wx+wy)*mu,  mu, -mu,  1,
+                  -wy,  wx, -(wx+wy)*mu, -mu,  mu,  1,
+                  -wy, -wx, -(wx+wy)*mu, -mu, -mu,  1;
+
+            Eigen::VectorXd lb(row_skip), ub(row_skip);
+            lb.setConstant(-1e10);
+            ub.setZero();
+
+            lb_vec.segment(idx*row_skip,row_skip) = lb;
+            ub_vec.segment(idx*row_skip,row_skip) = ub;
+            A_mtx.block<row_skip,col_skip>(idx*row_skip,start_idx+i*6) = a;
+            idx++;
+        }
     }
 }
 

diff --git a/src/constraints/ContactsVelocityConstraint.cpp b/src/constraints/ContactsVelocityConstraint.cpp
@@ -14,7 +14,7 @@ namespace wbc{
         b_vec.setZero();
 
         for(uint i = 0; i < nc; ++i)
-            A_mtx.block(i*3, 0, 3, nj) = contacts[i].active * robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).topRows<3>();
+            A_mtx.block(i*3, 0, 3, nj) = robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).topRows<3>();
     }
 
 

diff --git a/src/constraints/EffortLimitsAccelerationConstraint.cpp b/src/constraints/EffortLimitsAccelerationConstraint.cpp
@@ -22,8 +22,11 @@ namespace wbc{
         //! NOTE! -> not considering selection matrix
 
         A_mtx.block(0,0,na,nj) = robot_model->jointSpaceInertiaMatrix().bottomRows(na);
-        for(uint i=0; i < nc; ++i)
-            A_mtx.block(0,nj+i*6,na,6) = -robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).transpose().bottomRows(na);
+        for(uint i=0; i < nc; ++i){
+            if(contacts[i].active){
+                A_mtx.block(0,nj+i*6,na,6) = -robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).transpose().bottomRows(na);
+            }
+        }
 
         // enforce joint effort limits (only if torques are part of the optimization problem)
         Eigen::VectorXd b = robot_model->biasForces().tail(na);

diff --git a/src/constraints/RigidbodyDynamicsConstraint.cpp b/src/constraints/RigidbodyDynamicsConstraint.cpp
@@ -20,9 +20,11 @@ namespace wbc{
             b_vec.resize(6);
 
             A_mtx.block(0,  0, 6, nj) =  robot_model->jointSpaceInertiaMatrix().topRows<6>();
-            for(uint i = 0; i < contacts.size(); i++)
-                A_mtx.block(0, nj+i*6, 6, 6) = -robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).transpose().topRows<6>();
+            for(uint i = 0; i < contacts.size(); i++){
+                if(contacts[i].active)
+                    A_mtx.block(0, nj+i*6, 6, 6) = -robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).transpose().topRows<6>();
             b_vec = -robot_model->biasForces().topRows<6>();
+            }
         }
         else
         {
@@ -33,9 +35,11 @@ namespace wbc{
 
             A_mtx.block(0,  0, nj, nj) =  robot_model->jointSpaceInertiaMatrix();
             A_mtx.block(0, nj, nj, na) = -robot_model->selectionMatrix().transpose();
-            for(uint i = 0; i < contacts.size(); i++)
-                A_mtx.block(0, nj+na+i*6, nj, 6) = -robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).transpose();
-            b_vec = -robot_model->biasForces();
+            for(uint i = 0; i < contacts.size(); i++){
+                if(contacts[i].active)
+                    A_mtx.block(0, nj+na+i*6, nj, 6) = -robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).transpose();
+                b_vec = -robot_model->biasForces();
+            }
         }
 
     }

diff --git a/src/core/RobotModelConfig.hpp b/src/core/RobotModelConfig.hpp
@@ -24,6 +24,14 @@ class ActiveContact{
 };
 
 class ActiveContacts : public base::NamedVector<ActiveContact>{
+public:
+    uint getNumberOfActiveContacts() const{
+        uint nac = 0;
+        for(auto e : elements){
+            if(e.active) nac++;
+        }
+        return nac;
+    }
 };
 
 /**