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_SIMPLE_PID_DX_REF_COMPUTATION "SimplePID: dx_ref computation " 39 #define INPUT_SIGNALS m_KpSIN << m_KiSIN << m_decayFactorSIN << m_xSIN << m_x_desSIN << m_dx_desSIN 41 #define OUTPUT_SIGNALS m_dx_refSOUT 58 , CONSTRUCT_SIGNAL_IN(decayFactor, double)
63 , m_initSucceeded(false)
68 addCommand(
"init", makeCommandVoid2(*
this, &
SimplePID::init, docCommandVoid2(
"Initialize the entity.",
"time step",
"number of elements")));
92 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dx_ref before initialization!");
99 const Vector & Kp = m_KpSIN(iter);
100 const Vector & Ki = m_KiSIN(iter);
101 const double & decayFactor = m_decayFactorSIN(iter);
103 const Vector & x = m_xSIN(iter);
104 const Vector & x_des = m_x_desSIN(iter);
106 const Vector & dx_des = m_dx_desSIN(iter);
130 os <<
"SimplePID " << getName();
133 getProfiler().report_all(3, os);
135 catch (ExceptionSignal e) {}
dynamicgraph::Vector m_integralError
true if the entity has been successfully initialized
virtual void display(std::ostream &os) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimplePID(const std::string &name)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
void init(const double &dt, const int &N)
void resetIntegralError()
#define PROFILE_SIMPLE_PID_DX_REF_COMPUTATION
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)