19 #include <sot/core/debug.hh> 20 #include <sot/core/stop-watch.hh> 21 #include <dynamic-graph/factory.h> 22 #include <dynamic-graph/command-bind.h> 23 #include <dynamic-graph/all-commands.h> 29 namespace talos_balance
31 namespace dg = ::dynamicgraph;
35 #define INPUT_SIGNALS m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN << m_dfzAdmittanceSIN << m_vdcFrequencySIN << m_vdcDampingSIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN 37 #define INNER_SIGNALS m_dz_ctrlSOUT << m_dz_posSOUT 39 #define OUTPUT_SIGNALS m_vRightSOUT << m_vLeftSOUT << m_gainRightSOUT << m_gainLeftSOUT 47 "FootForceDifferenceController");
54 , CONSTRUCT_SIGNAL_IN(phase, int)
55 , CONSTRUCT_SIGNAL_IN(gainSwing, double)
56 , CONSTRUCT_SIGNAL_IN(gainStance, double)
57 , CONSTRUCT_SIGNAL_IN(gainDouble, double)
58 , CONSTRUCT_SIGNAL_IN(dfzAdmittance, double)
59 , CONSTRUCT_SIGNAL_IN(vdcFrequency, double)
60 , CONSTRUCT_SIGNAL_IN(vdcDamping, double)
65 , CONSTRUCT_SIGNAL_IN(posRightDes, MatrixHomogeneous)
66 , CONSTRUCT_SIGNAL_IN(posLeftDes, MatrixHomogeneous)
67 , CONSTRUCT_SIGNAL_IN(posRight, MatrixHomogeneous)
68 , CONSTRUCT_SIGNAL_IN(posLeft, MatrixHomogeneous)
69 , CONSTRUCT_SIGNAL_INNER(dz_ctrl, double, m_dfzAdmittanceSIN << m_vdcDampingSIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN << m_posRightSIN << m_posLeftSIN)
70 , CONSTRUCT_SIGNAL_INNER(dz_pos, double, m_vdcFrequencySIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN)
71 , CONSTRUCT_SIGNAL_OUT(vRight,
dynamicgraph::
Vector, m_phaseSIN << m_dz_ctrlSINNER << m_dz_posSINNER)
72 , CONSTRUCT_SIGNAL_OUT(vLeft,
dynamicgraph::
Vector, m_phaseSIN << m_dz_ctrlSINNER << m_dz_posSINNER)
73 , CONSTRUCT_SIGNAL_OUT(gainRight, double, m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN)
74 , CONSTRUCT_SIGNAL_OUT(gainLeft, double, m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN)
75 , m_initSucceeded(false)
96 SEND_WARNING_STREAM_MSG(
"Can't compute dz_ctrl before initialization!");
100 const double & dfzAdmittance = m_dfzAdmittanceSIN(iter);
101 const double & vdcDamping = m_vdcDampingSIN(iter);
103 const Eigen::VectorXd & wrenchRightDes = m_wrenchRightDesSIN(iter);
104 const Eigen::VectorXd & wrenchLeftDes = m_wrenchLeftDesSIN(iter);
105 const Eigen::VectorXd & wrenchRight = m_wrenchRightSIN(iter);
106 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSIN(iter);
108 const MatrixHomogeneous & posRight = m_posRightSIN(iter);
109 const MatrixHomogeneous & posLeft = m_posLeftSIN(iter);
111 const double RTz = posRight.translation()[2];
112 const double LTz = posLeft.translation()[2];
114 const double RFz_d = wrenchRightDes[2];
115 const double LFz_d = wrenchLeftDes[2];
117 const double RFz = wrenchRight[2];
118 const double LFz = wrenchLeft[2];
120 const double dz_ctrl = dfzAdmittance * ((LFz_d - RFz_d) - (LFz - RFz)) - vdcDamping * (RTz - LTz);
131 SEND_WARNING_STREAM_MSG(
"Can't compute dz_pos before initialization!");
135 const double & vdcFrequency = m_vdcFrequencySIN(iter);
137 const MatrixHomogeneous & posRightDes = m_posRightDesSIN(iter);
138 const MatrixHomogeneous & posLeftDes = m_posLeftDesSIN(iter);
139 const MatrixHomogeneous & posRight = m_posRightSIN(iter);
140 const MatrixHomogeneous & posLeft = m_posLeftSIN(iter);
142 const double RTz_d = posRightDes.translation()[2];
143 const double LTz_d = posLeftDes.translation()[2];
145 const double RTz = posRight.translation()[2];
146 const double LTz = posLeft.translation()[2];
148 const double dz_pos = vdcFrequency * ((LTz_d + RTz_d) - (LTz + RTz));
159 SEND_WARNING_STREAM_MSG(
"Can't compute vRight before initialization!");
165 const double & dz_ctrl = m_dz_ctrlSINNER(iter);
166 const double & dz_pos = m_dz_posSINNER(iter);
167 const int & phase = m_phaseSIN(iter);
172 s[2] = 0.5 * (dz_pos + dz_ctrl);
181 SEND_WARNING_STREAM_MSG(
"Can't compute vLeft before initialization!");
187 const double & dz_ctrl = m_dz_ctrlSINNER(iter);
188 const double & dz_pos = m_dz_posSINNER(iter);
189 const int & phase = m_phaseSIN(iter);
194 s[2] = 0.5 * (dz_pos - dz_ctrl);
203 SEND_WARNING_STREAM_MSG(
"Can't compute gainRight before initialization!");
207 const int & phase = m_phaseSIN(iter);
208 const double & gainSwing = m_gainSwingSIN(iter);
209 const double & gainStance = m_gainStanceSIN(iter);
210 const double & gainDouble = m_gainDoubleSIN(iter);
226 SEND_WARNING_STREAM_MSG(
"Can't compute gainLeft before initialization!");
230 const int & phase = m_phaseSIN(iter);
231 const double & gainSwing = m_gainSwingSIN(iter);
232 const double & gainStance = m_gainStanceSIN(iter);
233 const double & gainDouble = m_gainDoubleSIN(iter);
253 os <<
"FootForceDifferenceController " << getName();
256 getProfiler().report_all(3, os);
258 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)