19 #include <sot/core/debug.hh> 20 #include <dynamic-graph/factory.h> 21 #include <dynamic-graph/command-bind.h> 23 #include <dynamic-graph/all-commands.h> 24 #include <sot/core/stop-watch.hh> 30 namespace talos_balance
32 namespace dg = ::dynamicgraph;
37 #define PROFILE_DUMMYDCMESTIMATOR_DCM_COMPUTATION "DummyDcmEstimator: dcm computation " 39 #define INPUT_SIGNALS m_omegaSIN << m_massSIN << m_comSIN << m_momentaSIN 41 #define OUTPUT_SIGNALS m_dcmSOUT 56 , CONSTRUCT_SIGNAL_IN(omega, double)
57 , CONSTRUCT_SIGNAL_IN(mass, double)
61 , m_initSucceeded(false)
66 addCommand(
"init", makeCommandVoid0(*
this, &
DummyDcmEstimator::init, docCommandVoid0(
"Initialize the entity.")));
71 if(!m_omegaSIN.isPlugged())
72 return SEND_MSG(
"Init failed: signal omega is not plugged", MSG_TYPE_ERROR);
73 if(!m_massSIN.isPlugged())
74 return SEND_MSG(
"Init failed: signal mass is not plugged", MSG_TYPE_ERROR);
75 if(!m_comSIN.isPlugged())
76 return SEND_MSG(
"Init failed: signal com is not plugged", MSG_TYPE_ERROR);
77 if(!m_momentaSIN.isPlugged())
78 return SEND_MSG(
"Init failed: signal momenta is not plugged", MSG_TYPE_ERROR);
91 SEND_WARNING_STREAM_MSG(
"Cannot compute signal com_dcom before initialization!");
97 const double & omega = m_omegaSIN(iter);
98 const double & mass = m_massSIN(iter);
99 const Vector & com = m_comSIN(iter);
100 const Vector & momenta = m_momentaSIN(iter);
102 assert( com.size()==3 &&
"Unexpected size of signal com" );
103 assert( (momenta.size()==3 || momenta.size()==6) &&
"Unexpected size of signal momenta" );
105 const Vector dcom = momenta.head<3>()/mass;
107 const Vector dcm = com + dcom/omega;
124 os <<
"DummyDcmEstimator " << getName();
127 getProfiler().report_all(3, os);
129 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_DUMMYDCMESTIMATOR_DCM_COMPUTATION
virtual void display(std::ostream &os) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DummyDcmEstimator(const std::string &name)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)