19 #include <sot/core/debug.hh> 20 #include <dynamic-graph/factory.h> 21 #include <dynamic-graph/all-commands.h> 22 #include <sot/core/stop-watch.hh> 29 namespace talos_balance
31 namespace dg = ::dynamicgraph;
36 #define INPUT_SIGNALS m_phaseSIN << m_rightRollCoupledSIN << m_rightRollDecoupledSIN << m_rightPitchCoupledSIN << m_rightPitchDecoupledSIN << m_leftRollCoupledSIN << m_leftRollDecoupledSIN << m_leftPitchCoupledSIN << m_leftPitchDecoupledSIN 38 #define OUTPUT_SIGNALS m_selecLeftSOUT << m_selecRightSOUT << m_rightRollSOUT << m_rightPitchSOUT << m_leftRollSOUT << m_leftPitchSOUT 46 "AnkleJointSelector");
53 , CONSTRUCT_SIGNAL_IN(phase, int)
62 , CONSTRUCT_SIGNAL_OUT(selecLeft, Flags, m_phaseSIN)
63 , CONSTRUCT_SIGNAL_OUT(selecRight, Flags, m_phaseSIN)
64 , CONSTRUCT_SIGNAL_OUT(rightRoll,
dynamicgraph::
Vector, m_phaseSIN << m_rightRollCoupledSIN << m_rightRollDecoupledSIN)
65 , CONSTRUCT_SIGNAL_OUT(rightPitch,
dynamicgraph::
Vector, m_phaseSIN << m_rightPitchCoupledSIN << m_rightPitchDecoupledSIN)
66 , CONSTRUCT_SIGNAL_OUT(leftRoll,
dynamicgraph::
Vector, m_phaseSIN << m_leftRollCoupledSIN << m_leftRollDecoupledSIN)
67 , CONSTRUCT_SIGNAL_OUT(leftPitch,
dynamicgraph::
Vector, m_phaseSIN << m_leftPitchCoupledSIN << m_leftPitchDecoupledSIN)
70 , m_initSucceeded(false)
75 addCommand(
"init", makeCommandVoid1(*
this, &
AnkleJointSelector::init, docCommandVoid1(
"Initialize the entity.",
"Number of joints")));
92 SEND_WARNING_STREAM_MSG(
"Cannot compute selecLeft before initialization!");
96 const int & phase = m_phaseSIN(iter);
107 SEND_WARNING_STREAM_MSG(
"Cannot compute selecRight before initialization!");
111 const int & phase = m_phaseSIN(iter);
122 SEND_WARNING_STREAM_MSG(
"Cannot compute leftRoll before initialization!");
128 const int & phase = m_phaseSIN(iter);
129 const dg::Vector & leftRollCoupled = m_leftRollCoupledSIN(iter);
130 const dg::Vector & leftRollDecoupled = m_leftRollDecoupledSIN(iter);
133 s = leftRollDecoupled;
146 SEND_WARNING_STREAM_MSG(
"Cannot compute leftPitch before initialization!");
152 const int & phase = m_phaseSIN(iter);
153 const dg::Vector & leftPitchCoupled = m_leftPitchCoupledSIN(iter);
154 const dg::Vector & leftPitchDecoupled = m_leftPitchDecoupledSIN(iter);
157 s = leftPitchDecoupled;
159 s = leftPitchCoupled;
170 SEND_WARNING_STREAM_MSG(
"Cannot compute rightRoll before initialization!");
176 const int & phase = m_phaseSIN(iter);
177 const dg::Vector & rightRollCoupled = m_rightRollCoupledSIN(iter);
178 const dg::Vector & rightRollDecoupled = m_rightRollDecoupledSIN(iter);
181 s = rightRollDecoupled;
183 s = rightRollCoupled;
194 SEND_WARNING_STREAM_MSG(
"Cannot compute rightPitch before initialization!");
200 const int & phase = m_phaseSIN(iter);
201 const dg::Vector & rightPitchCoupled = m_rightPitchCoupledSIN(iter);
202 const dg::Vector & rightPitchDecoupled = m_rightPitchDecoupledSIN(iter);
205 s = rightPitchDecoupled;
207 s = rightPitchCoupled;
222 os <<
"AnkleJointSelector " << getName();
225 getProfiler().report_all(3, os);
227 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AnkleJointSelector(const std::string &name)
virtual void display(std::ostream &os) const
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)