21 #include <sot/core/debug.hh> 22 #include <dynamic-graph/factory.h> 23 #include <dynamic-graph/all-commands.h> 24 #include <sot/core/stop-watch.hh> 29 namespace talos_balance {
30 namespace dg = ::dynamicgraph;
34 #define PROFILE_HIPFLEXIBILITYCOMPENSATION_TAUFILT_COMPUTATION \ 35 "HipFlexibilityCompensation: Torque filter computation " 36 #define PROFILE_HIPFLEXIBILITYCOMPENSATION_DELTAQ_COMPUTATION \ 37 "HipFlexibilityCompensation: Angular correction computation " 38 #define PROFILE_HIPFLEXIBILITYCOMPENSATION_QCMD_COMPUTATION \ 39 "HipFlexibilityCompensation: Corrected joint configuration computation " 41 #define JOINT_DES_SIGNALS m_q_desSIN 43 #define INPUT_SIGNALS m_tauSIN << m_K_rSIN << m_K_lSIN //<< m_K_dSIN 45 #define OUTPUT_SIGNALS m_tau_filtSOUT << m_delta_qSOUT << m_q_cmdSOUT 53 "HipFlexibilityCompensation");
62 , CONSTRUCT_SIGNAL_IN(K_l, double)
63 , CONSTRUCT_SIGNAL_IN(K_r, double)
67 , m_initSucceeded(false)
68 , m_torqueLowPassFilterFrequency(1)
69 , m_delta_q_saturation(0.01)
70 , m_rate_limiter(0.003){
75 addCommand(
"init", makeCommandVoid2(*
this,
77 docCommandVoid2(
"Initialize the entity.",
81 addCommand(
"setTorqueLowPassFilterFrequency", makeCommandVoid1(*
this,
83 docCommandVoid1(
"Set the LowPassFilter frequency for the torque computation.",
84 "Value of the frequency")));
85 addCommand(
"setAngularSaturation", makeCommandVoid1(*
this,
87 docCommandVoid1(
"Set the saturation for the angular correction computation.",
88 "Value of the saturation")));
89 addCommand(
"setRateLimiter", makeCommandVoid1(*
this,
91 docCommandVoid1(
"Set the rate for the rate limiter of delta_q.",
92 "Value of the limiter")));
95 docDirectGetter(
"Get the current value of the torque LowPassFilter frequency.",
"frequency (double)")));
98 docDirectGetter(
"Get the current value of the Angular Saturation.",
"saturation (double)")));
100 addCommand(
"getRateLimiter", makeDirectGetter(*
this, &
m_rate_limiter,
101 docDirectGetter(
"Get the current value of the rate limiter.",
"rate (double)")));
107 if (!m_q_desSIN.isPlugged())
108 return SEND_MSG(
"Init failed: signal q_des is not plugged", MSG_TYPE_ERROR);
109 if (!m_tauSIN.isPlugged())
110 return SEND_MSG(
"Init failed: signal tau is not plugged", MSG_TYPE_ERROR);
111 if (!m_K_rSIN.isPlugged())
112 return SEND_MSG(
"Init failed: signal K_r is not plugged", MSG_TYPE_ERROR);
113 if (!m_K_lSIN.isPlugged())
114 return SEND_MSG(
"Init failed: signal K_l is not plugged", MSG_TYPE_ERROR);
117 std::string robotName_nonconst(robotName);
119 if (!isNameInRobotUtil(robotName_nonconst)) {
120 SEND_MSG(
"You should have a robotUtil pointer initialized before", MSG_TYPE_ERROR);
123 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
127 const Vector& q_des = m_q_desSIN.accessCopy();
128 const Vector& tau = m_tauSIN.accessCopy();
149 double alpha = exp(-
m_dt * 2 * M_PI * frequency);
150 Vector output = alpha * previous_signal + signal * (1 - alpha);
155 Vector rate = (signal - previous_signal)/
m_dt;
157 for (
unsigned int i=0; i<signal.size(); i++){
163 output[i] = signal[i];
173 SEND_WARNING_STREAM_MSG(
"Cannot compute signal tau_filt before initialization!");
179 const Vector& tau = m_tauSIN(iter);
181 if(s.size() != tau.size())
182 s.resize(tau.size());
198 SEND_WARNING_STREAM_MSG(
"Cannot compute signal delta_q before initialization!");
204 const Vector &tau = m_tau_filtSOUT(iter);
205 double K_r = m_K_rSIN(iter);
206 double K_l = m_K_lSIN(iter);
208 if(s.size() != tau.size())
209 s.setZero(tau.size());
211 for (
unsigned int i=0; i<tau.size(); i++){
245 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_cmd before initialization!");
251 const Vector &q_des = m_q_desSIN(iter);
252 const Vector &delta_q = m_delta_qSOUT(iter);
254 if(s.size() != q_des.size())
255 s.resize(q_des.size());
258 limitedSignal.resize(delta_q.size());
265 s = q_des + limitedSignal;
278 os <<
"HipFlexibilityCompensation " << getName();
280 getProfiler().report_all(3, os);
281 }
catch (ExceptionSignal e) {}
void rateLimiter(const dynamicgraph::Vector &signal, dynamicgraph::Vector &previous_signal, dynamicgraph::Vector &output)
Compute the limiter of a signal given the previous signal (based on first derivative).
dynamicgraph::Vector m_previous_delta_q
#define PROFILE_HIPFLEXIBILITYCOMPENSATION_TAUFILT_COMPUTATION
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_HIPFLEXIBILITYCOMPENSATION_DELTAQ_COMPUTATION
virtual void display(std::ostream &os) const
double m_dt
true if the entity has been successfully initialized
void setAngularSaturation(const double &saturation)
Set the value of the saturation for the angular correction computation.
#define JOINT_DES_SIGNALS
EIGEN_MAKE_ALIGNED_OPERATOR_NEW HipFlexibilityCompensation(const std::string &name)
#define PROFILE_HIPFLEXIBILITYCOMPENSATION_QCMD_COMPUTATION
dynamicgraph::Vector m_previous_tau
double m_torqueLowPassFilterFrequency
void setRateLimiter(const double &rate)
Set the value of the limiter for the the rate limiter of delta_q.
void setTorqueLowPassFilterFrequency(const double &frequency)
Set the LowPassFilter frequency for the torque computation.
dynamicgraph::Vector lowPassFilter(const double &frequency, const dynamicgraph::Vector &signal, dynamicgraph::Vector &previous_signal)
Compute the low pass filter of a signal given a frequency and the previous signal.
RobotUtilShrPtr m_robot_util
double m_delta_q_saturation
void init(const double &dt, const std::string &robotName)
Initialize the entity.
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)