Important bug fix in ContactsAccelerationConstraint

ARC-OPT · Apr 22, 2024 · dc398db · dc398db
1 parent 0e6870f
commit dc398db
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 5 deletions.
diff --git a/src/constraints/ContactsAccelerationConstraint.cpp b/src/constraints/ContactsAccelerationConstraint.cpp
@@ -10,8 +10,12 @@ namespace wbc{
         uint na = robot_model->noOfActuatedJoints();
         uint nc = contacts.size();
 
-        uint nv = reduced ? (nj + nc*3) : (nj + na + nc*3);
+        // 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);
 
@@ -20,8 +24,8 @@ namespace wbc{
 
         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;
-            A_mtx.block(i*3,  0, 3, nj) = robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).topRows<3>();
+            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)
         }
     }
 

diff --git a/src/constraints/RigidbodyDynamicsConstraint.cpp b/src/constraints/RigidbodyDynamicsConstraint.cpp
@@ -20,7 +20,7 @@ namespace wbc{
             b_vec.resize(6);
 
             A_mtx.block(0,  0, 6, nj) =  robot_model->jointSpaceInertiaMatrix().topRows<6>();
-            for(int i = 0; i < contacts.size(); i++)
+            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>();
             b_vec = -robot_model->biasForces().topRows<6>();
         }
@@ -33,7 +33,7 @@ namespace wbc{
 
             A_mtx.block(0,  0, nj, nj) =  robot_model->jointSpaceInertiaMatrix();
             A_mtx.block(0, nj, nj, na) = -robot_model->selectionMatrix().transpose();
-            for(int i = 0; i < contacts.size(); i++)
+            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();
         }