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_PIDD_DDX_REF_COMPUTATION "SimplePIDD: ddx_ref computation " 39 #define INPUT_SIGNALS m_KpSIN << m_KiSIN << m_KdSIN << m_decayFactorSIN << m_xSIN << m_x_desSIN << m_dxSIN << m_dx_desSIN << m_ddx_desSIN 41 #define OUTPUT_SIGNALS m_ddx_refSOUT << m_dx_refSOUT 59 , CONSTRUCT_SIGNAL_IN(decayFactor, double)
67 , m_initSucceeded(false)
72 addCommand(
"init", makeCommandVoid2(*
this, &
SimplePIDD::init, docCommandVoid2(
"Initialize the entity.",
"time step",
"number of elements")));
103 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ddx_ref before initialization!");
110 const Vector & Kp = m_KpSIN(iter);
111 const Vector & Ki = m_KiSIN(iter);
112 const Vector & Kd = m_KiSIN(iter);
113 const double & decayFactor = m_decayFactorSIN(iter);
115 const Vector & x = m_xSIN(iter);
116 const Vector & x_des = m_x_desSIN(iter);
118 const Vector & dx = m_dxSIN(iter);
119 const Vector & dx_des = m_dx_desSIN(iter);
121 const Vector & ddx_des = m_ddx_desSIN(iter);
124 Vector dx_err = dx_des - dx;
126 Vector ddx_ref = ddx_des + Kd.cwiseProduct(dx_err) + Kp.cwiseProduct(x_err) + Ki.cwiseProduct(
m_integralError);
142 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dx_ref before initialization!");
146 const Vector & ddx_ref = m_ddx_refSOUT(iter);
163 os <<
"SimplePIDD " << getName();
166 getProfiler().report_all(3, os);
168 catch (ExceptionSignal e) {}
void init(const double &dt, const int &N)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
virtual void display(std::ostream &os) const
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
dynamicgraph::Vector m_dx_ref
true if the entity has been successfully initialized
void resetIntegralError()
#define PROFILE_SIMPLE_PIDD_DDX_REF_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimplePIDD(const std::string &name)
dynamicgraph::Vector m_integralError
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)