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_DCMCONTROLLER_ZMPREF_COMPUTATION "DcmController: zmpRef computation " 38 #define PROFILE_DCMCONTROLLER_WRENCHREF_COMPUTATION "DcmController: wrenchRef computation " 40 #define INPUT_SIGNALS m_KpSIN << m_KiSIN << m_decayFactorSIN << m_omegaSIN << m_massSIN << m_comSIN << m_dcmSIN << m_dcmDesSIN << m_zmpDesSIN 42 #define OUTPUT_SIGNALS m_zmpRefSOUT << m_wrenchRefSOUT 59 , CONSTRUCT_SIGNAL_IN(decayFactor, double)
60 , CONSTRUCT_SIGNAL_IN(omega, double)
61 , CONSTRUCT_SIGNAL_IN(mass, double)
67 , CONSTRUCT_SIGNAL_OUT(wrenchRef,
dynamicgraph::
Vector, m_zmpRefSOUT << m_comSIN << m_omegaSIN << m_massSIN)
69 , m_initSucceeded(false)
74 addCommand(
"init", makeCommandVoid1(*
this, &
DcmController::init, docCommandVoid1(
"Initialize the entity.",
"time step")));
80 if(!m_KpSIN.isPlugged())
81 return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
82 if(!m_KiSIN.isPlugged())
83 return SEND_MSG(
"Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
84 if(!m_decayFactorSIN.isPlugged())
85 return SEND_MSG(
"Init failed: signal decayFactor is not plugged", MSG_TYPE_ERROR);
86 if(!m_omegaSIN.isPlugged())
87 return SEND_MSG(
"Init failed: signal omega is not plugged", MSG_TYPE_ERROR);
88 if(!m_massSIN.isPlugged())
89 return SEND_MSG(
"Init failed: signal mass is not plugged", MSG_TYPE_ERROR);
90 if(!m_comSIN.isPlugged())
91 return SEND_MSG(
"Init failed: signal com is not plugged", MSG_TYPE_ERROR);
92 if(!m_dcmSIN.isPlugged())
93 return SEND_MSG(
"Init failed: signal dcm is not plugged", MSG_TYPE_ERROR);
94 if(!m_dcmDesSIN.isPlugged())
95 return SEND_MSG(
"Init failed: signal dcmDes is not plugged", MSG_TYPE_ERROR);
96 if(!m_zmpDesSIN.isPlugged())
97 return SEND_MSG(
"Init failed: signal zmpDes is not plugged", MSG_TYPE_ERROR);
117 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpRef before initialization!");
124 const Vector & Kp = m_KpSIN(iter);
125 const Vector & Ki = m_KiSIN(iter);
126 const double & decayFactor = m_decayFactorSIN(iter);
127 const double & omega = m_omegaSIN(iter);
128 const Vector & dcm = m_dcmSIN(iter);
129 const Vector & dcmDes = m_dcmDesSIN(iter);
130 const Vector & zmpDes = m_zmpDesSIN(iter);
132 assert(Kp.size()==3 &&
"Unexpected size of signal Kp");
133 assert(Ki.size()==3 &&
"Unexpected size of signal Ki");
134 assert(dcm.size()==3 &&
"Unexpected size of signal dcm");
135 assert(dcmDes.size()==3 &&
"Unexpected size of signal dcmDes");
136 assert(zmpDes.size()==3 &&
"Unexpected size of signal zmpDes");
138 Vector dcmError = dcmDes - dcm;
140 Vector zmpRef = zmpDes - (Vector::Constant(3,1,1.0) + Kp/omega).cwiseProduct(dcmError) - Ki.cwiseProduct(
m_dcmIntegralError)/omega;
157 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRef before initialization!");
163 const double & omega = m_omegaSIN(iter);
164 const double & mass = m_massSIN(iter);
165 const Vector & com = m_comSIN(iter);
167 const Vector & zmpRef = m_zmpRefSOUT(iter);
169 assert(com.size()==3 &&
"Unexpected size of signal com");
171 Eigen::Vector3d forceRef = mass*omega*omega*(com-zmpRef);
172 forceRef[2] = mass*9.81;
175 wrenchRef.head<3>() = forceRef;
176 Eigen::Vector3d com3 = com;
177 wrenchRef.tail<3>() = com3.cross(forceRef);
194 os <<
"DcmController " << getName();
197 getProfiler().report_all(3, os);
199 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
dynamicgraph::Vector m_dcmIntegralError
true if the entity has been successfully initialized
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_DCMCONTROLLER_WRENCHREF_COMPUTATION
virtual void display(std::ostream &os) const
#define PROFILE_DCMCONTROLLER_ZMPREF_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DcmController(const std::string &name)
void resetDcmIntegralError()
void init(const double &dt)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)