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;
38 #define PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION "SimpleAdmittanceController: qRef computation " 40 #define PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION "SimpleAdmittanceController: dqRef computation " 42 #define INPUT_SIGNALS m_KpSIN << m_stateSIN << m_tauSIN << m_tauDesSIN 44 #define OUTPUT_SIGNALS m_qRefSOUT << m_dqRefSOUT 52 "SimpleAdmittanceController");
65 , m_initSucceeded(false)
72 addCommand(
"useExternalState", makeDirectSetter(*
this,&
m_useState, docDirectSetter(
"use external state",
"bool")));
73 addCommand(
"isUsingExternalState", makeDirectGetter(*
this,&
m_useState, docDirectGetter(
"use external state",
"bool")));
79 return SEND_MSG(
"n must be at least 1", MSG_TYPE_ERROR);
80 if(!m_KpSIN.isPlugged())
81 return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
82 if(!m_tauSIN.isPlugged())
83 return SEND_MSG(
"Init failed: signal tau is not plugged", MSG_TYPE_ERROR);
84 if(!m_tauDesSIN.isPlugged())
85 return SEND_MSG(
"Init failed: signal tauDes is not plugged", MSG_TYPE_ERROR);
107 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dqRef before initialization!");
113 const Vector & tauDes = m_tauDesSIN(iter);
114 const Vector & tau = m_tauSIN(iter);
115 const Vector & Kp = m_KpSIN(iter);
117 assert(tau.size()==
m_n &&
"Unexpected size of signal tau");
118 assert(tauDes.size()==
m_n &&
"Unexpected size of signal tauDes");
119 assert(Kp.size()==
m_n &&
"Unexpected size of signal Kp");
121 const Vector & dqRef = Kp.cwiseProduct(tauDes-tau);
134 SEND_WARNING_STREAM_MSG(
"Cannot compute signal qRef before initialization!");
140 const Vector & dqRef = m_dqRefSOUT(iter);
142 assert(dqRef.size()==
m_n &&
"Unexpected size of signal dqRef");
146 if(!m_stateSIN.isPlugged())
148 SEND_MSG(
"Signal state is requested, but is not plugged", MSG_TYPE_ERROR);
151 const Vector & state = m_stateSIN(iter);
152 assert(state.size()==
m_n &&
"Unexpected size of signal state");
174 os <<
"SimpleAdmittanceController " << getName();
177 getProfiler().report_all(3, os);
179 catch (ExceptionSignal e) {}
#define PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION
virtual void display(std::ostream &os) const
#define PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleAdmittanceController(const std::string &name)
void setPosition(const dynamicgraph::Vector &position)
void init(const double &dt, const unsigned &n)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)