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;
39 #define PROFILE_SATURATION_SOUT_COMPUTATION "Saturation: sOut computation" 41 #define INPUT_SIGNALS m_xSIN << m_ySIN << m_kSIN << m_xLimSIN << m_yLimSIN 43 #define OUTPUT_SIGNALS m_yOutSOUT 60 , CONSTRUCT_SIGNAL_IN(k, double)
76 const double &x = m_xSIN(iter)[0];
78 const double &y = s[0];
79 const double &k = m_kSIN(iter);
80 const double &xLim = m_xLimSIN(iter)[0];
81 const double &yLim = m_yLimSIN(iter)[0];
85 assert(k > 0 &&
"k must be strictly positive");
86 assert(xLim > 0 &&
"xLim must be strictly positive");
87 assert(yLim > 0 &&
"yLim must be strictly positive");
89 if ((x <= -xLim) or (x > xLim))
93 else if (-xLim + yLim / k < x and x <= xLim - yLim / k)
95 r = std::min(std::max(y, -yLim), yLim);
97 else if (-xLim < x and x <= -xLim + yLim / k)
99 r = std::min(std::max(y, -k * (x + xLim)), k * (x + xLim));
101 else if (xLim - yLim / k < x and x <= xLim)
103 r = std::min(std::max(y, -yLim + k * (x - xLim + yLim / k)), yLim - k * (x - xLim + yLim / k));
121 os <<
"Saturation " << getName();
124 getProfiler().report_all(3, os);
126 catch (ExceptionSignal e)
#define PROFILE_SATURATION_SOUT_COMPUTATION
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Saturation(const std::string &name)
virtual void display(std::ostream &os) const
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)