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_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION "JointPositionController: dqRef computation " 39 #define INPUT_SIGNALS m_KpSIN << m_stateSIN << m_qDesSIN << m_dqDesSIN 41 #define OUTPUT_SIGNALS m_dqRefSOUT 49 "JointPositionController");
61 , m_initSucceeded(false)
72 return SEND_MSG(
"n must be at least 1", MSG_TYPE_ERROR);
73 if(!m_KpSIN.isPlugged())
74 return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
75 if(!m_stateSIN.isPlugged())
76 return SEND_MSG(
"Init failed: signal q is not plugged", MSG_TYPE_ERROR);
77 if(!m_qDesSIN.isPlugged())
78 return SEND_MSG(
"Init failed: signal qDes is not plugged", MSG_TYPE_ERROR);
79 if(!m_dqDesSIN.isPlugged())
80 return SEND_MSG(
"Init failed: signal dqDes is not plugged", MSG_TYPE_ERROR);
94 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dqRef before initialization!");
100 const Vector & state = m_stateSIN(iter);
101 const Vector & qDes = m_qDesSIN(iter);
102 const Vector & dqDes = m_dqDesSIN(iter);
103 const Vector & Kp = m_KpSIN(iter);
105 assert(state.size()==
m_n+6 &&
"Unexpected size of signal state");
106 assert(qDes.size()==
m_n &&
"Unexpected size of signal qDes");
107 assert(dqDes.size()==
m_n &&
"Unexpected size of signal dqDes");
108 assert(Kp.size()==
m_n &&
"Unexpected size of signal Kp");
112 s = dqDes + Kp.cwiseProduct(qDes-q);
128 os <<
"JointPositionController " << getName();
131 getProfiler().report_all(3, os);
133 catch (ExceptionSignal e) {}
virtual void display(std::ostream &os) const
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW JointPositionController(const std::string &name)
void init(const unsigned &n)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)