From 79c0e5528c539343216ebf67561e3c1687afcf15 Mon Sep 17 00:00:00 2001 From: dmronga Date: Mon, 6 May 2024 12:14:46 +0200 Subject: [PATCH] Distinguish between active and inactive contacts in constraints --- .../ContactsAccelerationConstraint.cpp | 18 +++-- .../ContactsFrictionPointConstraint.cpp | 42 ++++++------ .../ContactsFrictionSurfaceConstraint.cpp | 68 ++++++++++--------- .../ContactsVelocityConstraint.cpp | 2 +- .../EffortLimitsAccelerationConstraint.cpp | 7 +- .../RigidbodyDynamicsConstraint.cpp | 14 ++-- src/core/RobotModelConfig.hpp | 8 +++ 7 files changed, 95 insertions(+), 64 deletions(-) diff --git a/src/constraints/ContactsAccelerationConstraint.cpp b/src/constraints/ContactsAccelerationConstraint.cpp index 0b6c2b93..c353adaa 100644 --- 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 index 3bc86da4..08f21558 100644 --- 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(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(idx*row_skip,start_idx+i*6) = a; + idx++; + } } } diff --git a/src/constraints/ContactsFrictionSurfaceConstraint.cpp b/src/constraints/ContactsFrictionSurfaceConstraint.cpp index c81f6dbb..f2a37635 100644 --- a/src/constraints/ContactsFrictionSurfaceConstraint.cpp +++ b/src/constraints/ContactsFrictionSurfaceConstraint.cpp @@ -9,14 +9,15 @@ 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(); @@ -24,35 +25,40 @@ void ContactsFrictionSurfaceConstraint::update(RobotModelPtr robot_model){ 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(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(idx*row_skip,start_idx+i*6) = a; + idx++; + } } } diff --git a/src/constraints/ContactsVelocityConstraint.cpp b/src/constraints/ContactsVelocityConstraint.cpp index d814d3a8..d4ce15ab 100644 --- 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 index 28e27f6c..cd47e42e 100644 --- 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 index a23e055c..ba18c7bf 100644 --- 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 index 352049a1..16364636 100644 --- a/src/core/RobotModelConfig.hpp +++ b/src/core/RobotModelConfig.hpp @@ -24,6 +24,14 @@ class ActiveContact{ }; class ActiveContacts : public base::NamedVector{ +public: + uint getNumberOfActiveContacts() const{ + uint nac = 0; + for(auto e : elements){ + if(e.active) nac++; + } + return nac; + } }; /**