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");
66 , m_initSucceeded(false)
73 addCommand(
"useExternalState", makeDirectSetter(*
this,&
m_useState, docDirectSetter(
"use external state",
"bool")));
74 addCommand(
"isUsingExternalState", makeDirectGetter(*
this,&
m_useState, docDirectGetter(
"use external state",
"bool")));
80 return SEND_MSG(
"n must be at least 1", MSG_TYPE_ERROR);
81 if(!m_KpSIN.isPlugged())
82 return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
83 if(!m_tauSIN.isPlugged())
84 return SEND_MSG(
"Init failed: signal tau is not plugged", MSG_TYPE_ERROR);
85 if(!m_tauDesSIN.isPlugged())
86 return SEND_MSG(
"Init failed: signal tauDes is not plugged", MSG_TYPE_ERROR);
108 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dqRef before initialization!");
116 const Vector & tauDes = m_tauDesSIN(iter);
117 const Vector & tau = m_tauSIN(iter);
118 const Vector & Kp = m_KpSIN(iter);
120 assert(tau.size()==
m_n &&
"Unexpected size of signal tau");
121 assert(tauDes.size()==
m_n &&
"Unexpected size of signal tauDes");
122 assert(Kp.size()==
m_n &&
"Unexpected size of signal Kp");
124 s = Kp.cwiseProduct(tauDes-tau);
135 SEND_WARNING_STREAM_MSG(
"Cannot compute signal qRef before initialization!");
143 const Vector & dqRef = m_dqRefSOUT(iter);
145 assert(dqRef.size()==
m_n &&
"Unexpected size of signal dqRef");
149 if(!m_stateSIN.isPlugged())
151 SEND_MSG(
"Signal state is requested, but is not plugged", MSG_TYPE_ERROR);
154 const Vector & state = m_stateSIN(iter);
155 assert(state.size()==
m_n &&
"Unexpected size of signal state");
177 os <<
"SimpleAdmittanceController " << getName();
180 getProfiler().report_all(3, os);
182 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)