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_DUMMYWALKINGPATTERNGENERATOR_DCM_COMPUTATION "DummyWalkingPatternGenerator: dcm computation " 39 #define INPUT_SIGNALS m_omegaSIN << m_rhoSIN << m_phaseSIN << m_footLeftSIN << m_footRightSIN << m_waistSIN << m_comSIN << m_vcomSIN << m_acomSIN << m_zmpSIN << m_referenceFrameSIN 41 #define INNER_SIGNALS m_rfSINNER 43 #define OUTPUT_SIGNALS m_comDesSOUT << m_vcomDesSOUT << m_acomDesSOUT << m_dcmDesSOUT << m_zmpDesSOUT << m_footLeftDesSOUT << m_footRightDesSOUT << m_waistDesSOUT << m_omegaDesSOUT << m_rhoDesSOUT << m_phaseDesSOUT 51 "DummyWalkingPatternGenerator");
58 , CONSTRUCT_SIGNAL_IN(omega, double)
59 , CONSTRUCT_SIGNAL_IN(rho, double)
60 , CONSTRUCT_SIGNAL_IN(phase, int)
61 , CONSTRUCT_SIGNAL_IN(footLeft, MatrixHomogeneous)
62 , CONSTRUCT_SIGNAL_IN(footRight, MatrixHomogeneous)
63 , CONSTRUCT_SIGNAL_IN(waist, MatrixHomogeneous)
68 , CONSTRUCT_SIGNAL_IN(referenceFrame, MatrixHomogeneous)
69 , CONSTRUCT_SIGNAL_INNER(rf, MatrixHomogeneous, m_referenceFrameSIN)
73 , CONSTRUCT_SIGNAL_OUT(dcmDes,
dynamicgraph::
Vector, m_omegaSIN << m_comDesSOUT << m_vcomDesSOUT)
74 , CONSTRUCT_SIGNAL_OUT(zmpDes,
dynamicgraph::
Vector, m_omegaSIN << m_comDesSOUT << m_acomDesSOUT << m_rfSINNER << m_zmpSIN)
75 , CONSTRUCT_SIGNAL_OUT(footLeftDes, MatrixHomogeneous, m_footLeftSIN << m_rfSINNER)
76 , CONSTRUCT_SIGNAL_OUT(footRightDes, MatrixHomogeneous, m_footRightSIN << m_rfSINNER)
77 , CONSTRUCT_SIGNAL_OUT(waistDes, MatrixHomogeneous, m_waistSIN << m_rfSINNER)
78 , CONSTRUCT_SIGNAL_OUT(omegaDes, double, m_omegaSIN)
79 , CONSTRUCT_SIGNAL_OUT(rhoDes, double, m_rhoSIN)
80 , CONSTRUCT_SIGNAL_OUT(phaseDes, int, m_phaseSIN)
81 , m_initSucceeded(false)
91 return m.linear().transpose()*(v-m.translation());
96 MatrixHomogeneous res;
97 res.linear() = m1.linear().transpose()*m2.linear();
98 res.translation() = m1.linear().transpose()*(m2.translation()-m1.translation());
115 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rf before initialization!");
119 s = m_referenceFrameSIN.isPlugged() ? m_referenceFrameSIN(iter) : MatrixHomogeneous::Identity();
128 SEND_WARNING_STREAM_MSG(
"Cannot compute signal comDes before initialization!");
134 const Vector & com = m_comSIN(iter);
135 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
137 assert( com.size()==3 &&
"Unexpected size of signal com" );
139 s =
actInv(referenceFrame, com);
148 SEND_WARNING_STREAM_MSG(
"Cannot compute signal vcomDes before initialization!");
154 const Vector & vcom = m_vcomSIN(iter);
155 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
157 assert( vcom.size()==3 &&
"Unexpected size of signal vcom" );
159 s = referenceFrame.linear().transpose()*vcom;
168 SEND_WARNING_STREAM_MSG(
"Cannot compute signal acomDes before initialization!");
174 const Vector & acom = m_acomSIN(iter);
175 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
177 assert( acom.size()==3 &&
"Unexpected size of signal acom" );
179 s = referenceFrame.linear().transpose()*acom;
188 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dcmDes before initialization!");
194 const double & omega = m_omegaSIN(iter);
195 const Vector & comDes = m_comDesSOUT(iter);
196 const Vector & vcomDes = m_vcomDesSOUT(iter);
198 s = comDes + vcomDes/omega;
207 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpDes before initialization!");
213 Eigen::Vector3d zmpDes;
215 if(m_zmpSIN.isPlugged())
217 const Vector & zmp = m_zmpSIN(iter);
218 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
222 zmpDes[2] = zmp.size()>2 ? zmp[2] : 0.;
224 zmpDes =
actInv(referenceFrame, zmpDes);
228 const double & omega = m_omegaSIN(iter);
229 const Vector & comDes = m_comDesSOUT(iter);
230 const Vector & acomDes = m_acomDesSOUT(iter);
232 zmpDes = comDes - acomDes/(omega*omega);
245 SEND_WARNING_STREAM_MSG(
"Cannot compute signal footLeftDes before initialization!");
249 MatrixHomogeneous footLeft = m_footLeftSIN(iter);
251 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
253 s =
actInv(referenceFrame, footLeft);
262 SEND_WARNING_STREAM_MSG(
"Cannot compute signal footRightDes before initialization!");
266 MatrixHomogeneous footRight = m_footRightSIN(iter);
268 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
270 s =
actInv(referenceFrame, footRight);
279 SEND_WARNING_STREAM_MSG(
"Cannot compute signal waistDes before initialization!");
283 const MatrixHomogeneous & waist = m_waistSIN(iter);
284 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
286 s =
actInv(referenceFrame, waist);
295 SEND_WARNING_STREAM_MSG(
"Cannot compute signal omegaDes before initialization!");
299 s = m_omegaSIN(iter);
307 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rhoDes before initialization!");
319 SEND_WARNING_STREAM_MSG(
"Cannot compute signal phaseDes before initialization!");
323 s = m_phaseSIN(iter);
335 os <<
"DummyWalkingPatternGenerator " << getName();
338 getProfiler().report_all(3, os);
340 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DummyWalkingPatternGenerator(const std::string &name)
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
virtual void display(std::ostream &os) const
dynamicgraph::Vector actInv(MatrixHomogeneous m, dynamicgraph::Vector v)
true if the entity has been successfully initialized
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)