Commit 5a6b4528 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Fix bug in InverseDynamicsFormulationAccForce: when adding a contact, the...

Fix bug in InverseDynamicsFormulationAccForce: when adding a contact, the number of equality constraints was incremented even if the motion constraints were relaxed to the cost function.
parent 3dd022b9
Pipeline #15444 passed with stage
in 20 minutes and 53 seconds
import pinocchio as se3 import pinocchio as se3
from pinocchio import libpinocchio_pywrap as pin import pinocchio as pin
#from pinocchio import libpinocchio_pywrap as pin
import tsid import tsid
import numpy as np import numpy as np
from numpy import nan from numpy import nan
...@@ -112,6 +113,8 @@ sampleCom = trajCom.computeNext() ...@@ -112,6 +113,8 @@ sampleCom = trajCom.computeNext()
q_ref = q[7:] q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref) trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
print("Create QP solver with ", invdyn.nVar, " variables, ", invdyn.nEq,
" equality and ", invdyn.nIn, " inequality constraints")
solver = tsid.SolverHQuadProgFast("qp solver") solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
...@@ -239,7 +239,8 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact, ...@@ -239,7 +239,8 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact,
cl->forceRegTask = std::make_shared<ConstraintEquality>("_force_reg_task", forceRegConstr.rows(), m_v+m_k); cl->forceRegTask = std::make_shared<ConstraintEquality>("_force_reg_task", forceRegConstr.rows(), m_v+m_k);
m_hqpData[1].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase> >(force_regularization_weight, cl->forceRegTask)); m_hqpData[1].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase> >(force_regularization_weight, cl->forceRegTask));
m_eq += motionConstr.rows(); if(motionPriorityLevel==0)
m_eq += motionConstr.rows();
m_in += forceConstr.rows(); m_in += forceConstr.rows();
return true; return true;
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment