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_COMADMITTANCECONTROLLER_DDCOMREF_COMPUTATION "ComAdmittanceController: ddcomRef computation " 38 #define PROFILE_COMADMITTANCECONTROLLER_STATEREF_COMPUTATION "ComAdmittanceController: stateRef computation " 39 #define PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION "ComAdmittanceController: dcomRef computation " 40 #define PROFILE_COMADMITTANCECONTROLLER_COMREF_COMPUTATION "ComAdmittanceController: comRef computation " 42 #define INPUT_SIGNALS m_KpSIN << m_zmpSIN << m_zmpDesSIN << m_ddcomDesSIN 44 #define INNER_SIGNALS m_stateRefSINNER 46 #define OUTPUT_SIGNALS m_comRefSOUT << m_dcomRefSOUT << m_ddcomRefSOUT 54 "ComAdmittanceController");
70 , m_initSucceeded(false)
78 addCommand(
"setState", makeCommandVoid2(*
this, &
ComAdmittanceController::setState, docCommandVoid2(
"Set initial reference position and velocity.",
"Initial position",
"Initial velocity")));
83 if(!m_KpSIN.isPlugged())
84 return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
85 if(!m_ddcomDesSIN.isPlugged())
86 return SEND_MSG(
"Init failed: signal ddcomDes is not plugged", MSG_TYPE_ERROR);
87 if(!m_zmpSIN.isPlugged())
88 return SEND_MSG(
"Init failed: signal zmp is not plugged", MSG_TYPE_ERROR);
89 if(!m_zmpDesSIN.isPlugged())
90 return SEND_MSG(
"Init failed: signal zmpDes is not plugged", MSG_TYPE_ERROR);
121 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ddcomRef before initialization!");
129 const Vector & ddcomDes = m_ddcomDesSIN(iter);
130 const Vector & zmp = m_zmpSIN(iter);
131 const Vector & zmpDes = m_zmpDesSIN(iter);
132 const Vector & Kp = m_KpSIN(iter);
134 assert(ddcomDes.size()==3 &&
"Unexpected size of signal ddcomDes");
135 assert(zmp.size()==3 &&
"Unexpected size of signal zmp");
136 assert(zmpDes.size()==3 &&
"Unexpected size of signal zmpDes");
137 assert(Kp.size()==3 &&
"Unexpected size of signal Kp");
139 s = ddcomDes + Kp.cwiseProduct(zmp-zmpDes);
150 SEND_WARNING_STREAM_MSG(
"Cannot compute signal stateRef before initialization!");
158 const Vector & ddcomRef = m_ddcomRefSOUT(iter);
160 assert(ddcomRef.size()==3 &&
"Unexpected size of signal ddcomRef");
162 const Eigen::Vector3d dcomRef =
m_state.tail<3>();
178 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dcomRef before initialization!");
186 const Vector & stateRef = m_stateRefSINNER(iter);
188 assert(stateRef.size()==6 &&
"Unexpected size of signal stateRef");
190 s = stateRef.head<3>();
201 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dcomRef before initialization!");
209 const Vector & stateRef = m_stateRefSINNER(iter);
211 assert(stateRef.size()==6 &&
"Unexpected size of signal stateRef");
213 s = stateRef.tail<3>();
228 os <<
"ComAdmittanceController " << getName();
231 getProfiler().report_all(3, os);
233 catch (ExceptionSignal e) {}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ComAdmittanceController(const std::string &name)
void init(const double &dt)
void setPosition(const dynamicgraph::Vector &)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
virtual void display(std::ostream &os) const
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
dynamicgraph::Vector m_state
true if the entity has been successfully initialized
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
void setState(const dynamicgraph::Vector &, const dynamicgraph::Vector &)
void setVelocity(const dynamicgraph::Vector &)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)