From 0e6870fd515e6f31ca0c89391a30d03f7d3ea5a8 Mon Sep 17 00:00:00 2001 From: dmronga Date: Mon, 22 Apr 2024 13:22:47 +0200 Subject: [PATCH] Make Contact constraints 3-dimensional, this seems to be more stable --- .../ContactsAccelerationConstraint.cpp | 16 ++++++---------- src/constraints/ContactsVelocityConstraint.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/src/constraints/ContactsAccelerationConstraint.cpp b/src/constraints/ContactsAccelerationConstraint.cpp index 0560674b..622e21c3 100644 --- a/src/constraints/ContactsAccelerationConstraint.cpp +++ b/src/constraints/ContactsAccelerationConstraint.cpp @@ -10,22 +10,18 @@ namespace wbc{ uint na = robot_model->noOfActuatedJoints(); uint nc = contacts.size(); - uint nv = reduced ? (nj + nc*6) : (nj + na + nc*6); + uint nv = reduced ? (nj + nc*3) : (nj + na + nc*3); - A_mtx.resize(nc*6, nv); - b_vec.resize(nc*6); + A_mtx.resize(nc*3, nv); + b_vec.resize(nc*3); A_mtx.setZero(); b_vec.setZero(); - for(int i = 0; i < contacts.size(); i++){ + for(uint i = 0; i < contacts.size(); i++){ const base::Acceleration& a = robot_model->spatialAccelerationBias(robot_model->worldFrame(), contacts.names[i]); - base::Vector6d acc; - acc.segment(0,3) = a.linear; - acc.segment(3,3) = a.angular; - - b_vec.segment(i*6, 6) = -acc; - A_mtx.block(i*6, 0, 6, nj) = robot_model->spaceJacobian(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>(); } } diff --git a/src/constraints/ContactsVelocityConstraint.cpp b/src/constraints/ContactsVelocityConstraint.cpp index 411b60d1..d814d3a8 100644 --- a/src/constraints/ContactsVelocityConstraint.cpp +++ b/src/constraints/ContactsVelocityConstraint.cpp @@ -9,12 +9,12 @@ namespace wbc{ uint nj = robot_model->noOfJoints(); uint nc = contacts.size(); - A_mtx.resize(nc*6, nj); - b_vec.resize(nc*6); + A_mtx.resize(nc*3, nj); + b_vec.resize(nc*3); b_vec.setZero(); - for(int i = 0; i < nc; ++i) - A_mtx.block(i*6, 0, 6, nj) = contacts[i].active * robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]); + 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>(); }