Commit 3969a85c authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[DummyWalkingPatternGenerator] Add omega, rho, and phase sout

parent c42f3fa4
......@@ -67,6 +67,8 @@ namespace dynamicgraph {
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(rho, double);
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(footLeft, MatrixHomogeneous);
DECLARE_SIGNAL_IN(footRight, MatrixHomogeneous);
......@@ -92,6 +94,10 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(footRightDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(waistDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(omegaDes, double);
DECLARE_SIGNAL_OUT(rhoDes, double);
DECLARE_SIGNAL_OUT(phaseDes, int);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
......
......@@ -36,11 +36,11 @@ namespace dynamicgraph
//Size to be aligned "-------------------------------------------------------"
#define PROFILE_DUMMYWALKINGPATTERNGENERATOR_DCM_COMPUTATION "DummyWalkingPatternGenerator: dcm computation "
#define INPUT_SIGNALS m_omegaSIN << m_footLeftSIN << m_footRightSIN << m_waistSIN << m_comSIN << m_vcomSIN << m_acomSIN << m_zmpSIN << m_referenceFrameSIN
#define INPUT_SIGNALS m_omegaSIN << m_rhoSIN << m_phaseSIN << m_footLeftSIN << m_footRightSIN << m_waistSIN << m_comSIN << m_vcomSIN << m_acomSIN << m_zmpSIN << m_referenceFrameSIN
#define INNER_SIGNALS m_rfSINNER
#define OUTPUT_SIGNALS m_comDesSOUT << m_vcomDesSOUT << m_acomDesSOUT << m_dcmDesSOUT << m_zmpDesSOUT << m_footLeftDesSOUT << m_footRightDesSOUT << m_waistDesSOUT
#define OUTPUT_SIGNALS m_comDesSOUT << m_vcomDesSOUT << m_acomDesSOUT << m_dcmDesSOUT << m_zmpDesSOUT << m_footLeftDesSOUT << m_footRightDesSOUT << m_waistDesSOUT << m_omegaDesSOUT << m_rhoDesSOUT << m_phaseDesSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
......@@ -56,6 +56,8 @@ namespace dynamicgraph
DummyWalkingPatternGenerator::DummyWalkingPatternGenerator(const std::string& name)
: Entity(name)
, CONSTRUCT_SIGNAL_IN(omega, double)
, CONSTRUCT_SIGNAL_IN(rho, double)
, CONSTRUCT_SIGNAL_IN(phase, int)
, CONSTRUCT_SIGNAL_IN(footLeft, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_IN(footRight, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_IN(waist, MatrixHomogeneous)
......@@ -73,6 +75,9 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_OUT(footLeftDes, MatrixHomogeneous, m_footLeftSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(footRightDes, MatrixHomogeneous, m_footRightSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(waistDes, MatrixHomogeneous, m_waistSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(omegaDes, double, m_omegaSIN)
, CONSTRUCT_SIGNAL_OUT(rhoDes, double, m_rhoSIN)
, CONSTRUCT_SIGNAL_OUT(phaseDes, int, m_phaseSIN)
, m_initSucceeded(false)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
......@@ -283,6 +288,42 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(omegaDes, double)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal omegaDes before initialization!");
return s;
}
s = m_omegaSIN(iter);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(rhoDes, double)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal rhoDes before initialization!");
return s;
}
s = m_rhoSIN(iter);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(phaseDes, int)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal phaseDes before initialization!");
return s;
}
s = m_phaseSIN(iter);
return s;
}
/* --- COMMANDS ---------------------------------------------------------- */
/* ------------------------------------------------------------------- */
......
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