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_DCMCOMCONTROLLER_DDCOMREF_COMPUTATION "DcmComController: ddcomRef computation " 38 #define PROFILE_DCMCOMCONTROLLER_ZMPREF_COMPUTATION "DcmComController: zmpRef computation " 39 #define PROFILE_DCMCOMCONTROLLER_WRENCHREF_COMPUTATION "DcmComController: wrenchRef computation " 41 #define INPUT_SIGNALS m_KpSIN << m_KiSIN << m_decayFactorSIN << m_omegaSIN << m_massSIN << m_dcmSIN << m_dcmDesSIN << m_comDesSIN << m_ddcomDesSIN 43 #define OUTPUT_SIGNALS m_ddcomRefSOUT << m_zmpRefSOUT << m_wrenchRefSOUT 60 , CONSTRUCT_SIGNAL_IN(decayFactor, double)
61 , CONSTRUCT_SIGNAL_IN(omega, double)
62 , CONSTRUCT_SIGNAL_IN(mass, double)
68 , CONSTRUCT_SIGNAL_OUT(zmpRef,
dynamicgraph::
Vector, m_ddcomRefSOUT << m_comDesSIN << m_omegaSIN )
69 , CONSTRUCT_SIGNAL_OUT(wrenchRef,
dynamicgraph::
Vector, m_ddcomRefSOUT << m_comDesSIN << m_massSIN )
70 , m_initSucceeded(false)
75 addCommand(
"init", makeCommandVoid1(*
this, &
DcmComController::init, docCommandVoid1(
"Initialize the entity.",
"time step")));
81 if(!m_KpSIN.isPlugged())
82 return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
83 if(!m_KiSIN.isPlugged())
84 return SEND_MSG(
"Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
85 if(!m_decayFactorSIN.isPlugged())
86 return SEND_MSG(
"Init failed: signal decayFactor is not plugged", MSG_TYPE_ERROR);
87 if(!m_omegaSIN.isPlugged())
88 return SEND_MSG(
"Init failed: signal omega is not plugged", MSG_TYPE_ERROR);
89 if(!m_massSIN.isPlugged())
90 return SEND_MSG(
"Init failed: signal mass is not plugged", MSG_TYPE_ERROR);
91 if(!m_dcmSIN.isPlugged())
92 return SEND_MSG(
"Init failed: signal dcm is not plugged", MSG_TYPE_ERROR);
93 if(!m_dcmDesSIN.isPlugged())
94 return SEND_MSG(
"Init failed: signal dcmDes is not plugged", MSG_TYPE_ERROR);
95 if(!m_comDesSIN.isPlugged())
96 return SEND_MSG(
"Init failed: signal comDes is not plugged", MSG_TYPE_ERROR);
97 if(!m_ddcomDesSIN.isPlugged())
98 return SEND_MSG(
"Init failed: signal ddcomDes is not plugged", MSG_TYPE_ERROR);
118 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ddcomRef before initialization!");
127 const Vector & Kp = m_KpSIN(iter);
128 const Vector & Ki = m_KiSIN(iter);
129 const double & decayFactor = m_decayFactorSIN(iter);
130 const double & omega = m_omegaSIN(iter);
131 const Vector & dcm = m_dcmSIN(iter);
132 const Vector & dcmDes = m_dcmDesSIN(iter);
133 const Vector & ddcomDes = m_ddcomDesSIN(iter);
135 assert(Kp.size()==3 &&
"Unexpected size of signal Kp");
136 assert(Ki.size()==3 &&
"Unexpected size of signal Ki");
137 assert(dcm.size()==3 &&
"Unexpected size of signal dcm");
138 assert(dcmDes.size()==3 &&
"Unexpected size of signal dcmDes");
139 assert(ddcomDes.size()==3 &&
"Unexpected size of signal ddcomDes");
141 const Eigen::Vector3d dcmError = dcmDes - dcm;
143 const Eigen::Vector3d ddcomRef = ddcomDes + omega * Kp.cwiseProduct(dcmError) + omega * Ki.cwiseProduct(
m_dcmIntegralError);
159 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpRef before initialization!");
167 const double & omega = m_omegaSIN(iter);
168 const Vector & comDes = m_comDesSIN(iter);
170 const Vector & ddcomRef = m_ddcomRefSOUT(iter);
172 assert(comDes.size()==3 &&
"Unexpected size of signal comDes");
174 Eigen::Vector3d zmpRef = comDes - ddcomRef/(omega*omega);
188 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRef before initialization!");
196 const double & mass = m_massSIN(iter);
197 const Vector & comDes = m_comDesSIN(iter);
199 const Vector & ddcomRef = m_ddcomRefSOUT(iter);
201 assert(comDes.size()==3 &&
"Unexpected size of signal comDes");
203 Eigen::Vector3d gravity;
204 gravity << 0.0, 0.0, -9.81;
206 const Eigen::Vector3d forceRef = mass * ( ddcomRef - gravity );
208 Eigen::Matrix<double,6,1> wrenchRef;
209 wrenchRef.head<3>() = forceRef;
210 const Eigen::Vector3d comDes3 = comDes;
211 wrenchRef.tail<3>() = comDes3.cross(wrenchRef.head<3>());
228 os <<
"DcmComController " << getName();
231 getProfiler().report_all(3, os);
233 catch (ExceptionSignal e) {}
void init(const double &dt)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
virtual void display(std::ostream &os) const
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
dynamicgraph::Vector m_dcmIntegralError
true if the entity has been successfully initialized
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DcmComController(const std::string &name)
void resetDcmIntegralError()
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)