Commit 53435c34 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[DsitributeWrench] Add securities

parent 37c3d396
......@@ -337,10 +337,17 @@ namespace dynamicgraph
Bineq(32) = - m_eps;
Bineq(33) = - m_eps;
bool success = m_qp2.solve(Q, C, Aeq, Beq, Aineq, Bineq);
const bool success = m_qp2.solve(Q, C, Aeq, Beq, Aineq, Bineq);
m_emergency_stop_triggered = !success;
if(m_emergency_stop_triggered)
{
m_wrenchLeft.setZero(6);
m_wrenchRight.setZero(6);
return success;
}
const Eigen::VectorXd & result = m_qp2.result();
m_wrenchLeft = result.head<6>();
......@@ -379,10 +386,17 @@ namespace dynamicgraph
Eigen::VectorXd & Bineq = m_Bineq1;
Bineq.setZero();
bool success = m_qp1.solve(Q, C, Aeq, Beq, Aineq, Bineq);
const bool success = m_qp1.solve(Q, C, Aeq, Beq, Aineq, Bineq);
m_emergency_stop_triggered = !success;
if(m_emergency_stop_triggered)
{
m_wrenchLeft.setZero(6);
m_wrenchRight.setZero(6);
return success;
}
const Eigen::VectorXd & result = m_qp1.result();
if(phase>0) {
......@@ -438,7 +452,7 @@ namespace dynamicgraph
if(!success)
{
SEND_WARNING_STREAM_MSG("No solution to the QP problem!");
SEND_ERROR_STREAM_MSG("No solution to the QP problem!");
return s;
}
......@@ -521,6 +535,12 @@ namespace dynamicgraph
const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
if(m_emergency_stop_triggered)
{
s.setZero(3);
return s;
}
s = computeCoP(wrenchLeft, m_contactLeft);
return s;
......@@ -538,6 +558,12 @@ namespace dynamicgraph
const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
if(m_emergency_stop_triggered)
{
s.setZero(3);
return s;
}
s = computeCoP(wrenchRight, m_contactRight);
return s;
......
Supports Markdown
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