Commit 2eadae0f authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[DistributeWrench] surfaceWrench

parent 864a3d27
......@@ -85,9 +85,11 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ankleWrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(surfaceWrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(surfaceWrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
......
......@@ -48,7 +48,7 @@ namespace dynamicgraph
#define INNER_SIGNALS m_kinematics_computations << m_qp_computations
#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_ankleWrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_ankleWrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_ankleWrenchLeftSOUT << m_surfaceWrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_ankleWrenchRightSOUT << m_surfaceWrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
......@@ -76,9 +76,11 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_INNER(qp_computations, int, m_wrenchDesSIN << m_rhoSIN << m_phaseSIN << WEIGHT_SIGNALS << m_kinematics_computationsSINNER)
, CONSTRUCT_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector, m_qp_computationsSINNER)
, CONSTRUCT_SIGNAL_OUT(ankleWrenchLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
, CONSTRUCT_SIGNAL_OUT(surfaceWrenchLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
, CONSTRUCT_SIGNAL_OUT(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
, CONSTRUCT_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector, m_qp_computationsSINNER)
, CONSTRUCT_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector, m_wrenchRightSOUT)
, CONSTRUCT_SIGNAL_OUT(surfaceWrenchRight, dynamicgraph::Vector, m_wrenchRightSOUT)
, CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
, CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
, CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, m_wrenchRefSOUT)
......@@ -520,6 +522,40 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(surfaceWrenchLeft, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal surfaceWrenchLeft before initialization!");
return s;
}
if(s.size()!=6)
s.resize(6);
const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
s = m_contactLeft.actInv(pinocchio::Force(wrenchLeft)).toVector();
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(surfaceWrenchRight, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal surfaceWrenchRight before initialization!");
return s;
}
if(s.size()!=6)
s.resize(6);
const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
s = m_contactRight.actInv(pinocchio::Force(wrenchRight)).toVector();
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(copLeft, dynamicgraph::Vector)
{
if(!m_initSucceeded)
......
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