From dc398db4bf087c528a8944eef1b4d4461d1a6362 Mon Sep 17 00:00:00 2001 From: dmronga Date: Mon, 22 Apr 2024 23:20:39 +0200 Subject: [PATCH] Important bug fix in ContactsAccelerationConstraint --- src/constraints/ContactsAccelerationConstraint.cpp | 10 +++++++--- src/constraints/RigidbodyDynamicsConstraint.cpp | 4 ++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/constraints/ContactsAccelerationConstraint.cpp b/src/constraints/ContactsAccelerationConstraint.cpp index 622e21c3..0b6c2b93 100644 --- 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 index bdc3a711..a23e055c 100644 --- 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(); }