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_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 51 "DummyWalkingPatternGenerator");
58 , CONSTRUCT_SIGNAL_IN(omega, double)
59 , CONSTRUCT_SIGNAL_IN(footLeft, MatrixHomogeneous)
60 , CONSTRUCT_SIGNAL_IN(footRight, MatrixHomogeneous)
61 , CONSTRUCT_SIGNAL_IN(waist, MatrixHomogeneous)
66 , CONSTRUCT_SIGNAL_IN(referenceFrame, MatrixHomogeneous)
67 , CONSTRUCT_SIGNAL_INNER(rf, MatrixHomogeneous, m_referenceFrameSIN)
71 , CONSTRUCT_SIGNAL_OUT(dcmDes,
dynamicgraph::
Vector, m_omegaSIN << m_comDesSOUT << m_vcomDesSOUT)
72 , CONSTRUCT_SIGNAL_OUT(zmpDes,
dynamicgraph::
Vector, m_omegaSIN << m_comDesSOUT << m_acomDesSOUT << m_rfSINNER << m_zmpSIN)
73 , CONSTRUCT_SIGNAL_OUT(footLeftDes, MatrixHomogeneous, m_footLeftSIN << m_rfSINNER)
74 , CONSTRUCT_SIGNAL_OUT(footRightDes, MatrixHomogeneous, m_footRightSIN << m_rfSINNER)
75 , CONSTRUCT_SIGNAL_OUT(waistDes, MatrixHomogeneous, m_waistSIN << m_rfSINNER)
76 , m_initSucceeded(false)
86 return m.linear().transpose()*(v-m.translation());
91 MatrixHomogeneous res;
92 res.linear() = m1.linear().transpose()*m2.linear();
93 res.translation() = m1.linear().transpose()*(m2.translation()-m1.translation());
110 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rf before initialization!");
114 s = m_referenceFrameSIN.isPlugged() ? m_referenceFrameSIN(iter) : MatrixHomogeneous::Identity();
123 SEND_WARNING_STREAM_MSG(
"Cannot compute signal comDes before initialization!");
127 const Vector & com = m_comSIN(iter);
128 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
130 assert( com.size()==3 &&
"Unexpected size of signal com" );
143 SEND_WARNING_STREAM_MSG(
"Cannot compute signal vcomDes before initialization!");
147 const Vector & vcom = m_vcomSIN(iter);
148 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
150 assert( vcom.size()==3 &&
"Unexpected size of signal vcom" );
152 const Vector vcomDes(referenceFrame.linear().transpose()*vcom);
163 SEND_WARNING_STREAM_MSG(
"Cannot compute signal acomDes before initialization!");
167 const Vector & acom = m_acomSIN(iter);
168 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
170 assert( acom.size()==3 &&
"Unexpected size of signal acom" );
172 const Vector acomDes(referenceFrame.linear().transpose()*acom);
183 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dcmDes before initialization!");
187 const double & omega = m_omegaSIN(iter);
188 const Vector & comDes = m_comDesSOUT(iter);
189 const Vector & vcomDes = m_vcomDesSOUT(iter);
191 const Vector dcmDes = comDes + vcomDes/omega;
202 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpDes before initialization!");
208 if(m_zmpSIN.isPlugged())
210 const Vector & zmp = m_zmpSIN(iter);
211 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
216 zmpDes[2] = zmp.size()>2 ? zmp[2] : 0.;
218 zmpDes =
actInv(referenceFrame, zmpDes);
222 const double & omega = m_omegaSIN(iter);
223 const Vector & comDes = m_comDesSOUT(iter);
224 const Vector & acomDes = m_acomDesSOUT(iter);
226 zmpDes = comDes - acomDes/(omega*omega);
239 SEND_WARNING_STREAM_MSG(
"Cannot compute signal footLeftDes before initialization!");
243 MatrixHomogeneous footLeft = m_footLeftSIN(iter);
245 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
247 s =
actInv(referenceFrame, footLeft);
256 SEND_WARNING_STREAM_MSG(
"Cannot compute signal footRightDes before initialization!");
260 MatrixHomogeneous footRight = m_footRightSIN(iter);
262 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
264 s =
actInv(referenceFrame, footRight);
273 SEND_WARNING_STREAM_MSG(
"Cannot compute signal waistDes before initialization!");
277 const MatrixHomogeneous & waist = m_waistSIN(iter);
278 const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
280 s =
actInv(referenceFrame, waist);
293 os <<
"DummyWalkingPatternGenerator " << getName();
296 getProfiler().report_all(3, os);
298 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)