19 #include <sot/core/debug.hh> 20 #include <sot/core/stop-watch.hh> 21 #include <dynamic-graph/factory.h> 22 #include <dynamic-graph/command-bind.h> 23 #include <dynamic-graph/all-commands.h> 29 namespace talos_balance
31 namespace dg = ::dynamicgraph;
35 #define PROFILE_ANKLEADMITTANCECONTROLLER_DRP_COMPUTATION \ 36 "AnkleAdmittanceController: dRP computation " 38 #define INPUT_SIGNALS m_gainsXYSIN << m_wrenchSIN << m_pRefSIN 40 #define OUTPUT_SIGNALS m_dRPSOUT << m_vDesSOUT 48 "AnkleAdmittanceController");
60 , m_initSucceeded(false)
70 if(!m_gainsXYSIN.isPlugged())
71 return SEND_MSG(
"Init failed: signal gainsXY is not plugged", MSG_TYPE_ERROR);
72 if(!m_wrenchSIN.isPlugged())
73 return SEND_MSG(
"Init failed: signal wrench is not plugged", MSG_TYPE_ERROR);
74 if(!m_pRefSIN.isPlugged())
75 return SEND_MSG(
"Init failed: signal pRef is not plugged", MSG_TYPE_ERROR);
88 SEND_WARNING_STREAM_MSG(
"Can't compute dqRef before initialization!");
96 const Eigen::Vector3d pRef = m_pRefSIN(iter);
97 const Vector & wrench = m_wrenchSIN(iter);
98 const Vector & gainsXY = m_gainsXYSIN(iter);
100 assert(pRef.size() == 3 &&
"Unexpected size of signal pRef");
101 assert(wrench.size() == 6 &&
"Unexpected size of signal wrench");
102 assert(gainsXY.size() == 2 &&
"Unexpected size of signal gainsXY");
104 const Eigen::Vector3d error = wrench.tail<3>() - pRef.cross(wrench.head<3>());
107 dRP[0] = gainsXY[0] * error[0];
108 dRP[1] = gainsXY[1] * error[1];
121 SEND_WARNING_STREAM_MSG(
"Can't compute vDes before initialization!");
127 const Vector & dRP = m_dRPSOUT(iter);
129 s.head<3>().setZero();
130 s.segment<2>(3) = dRP;
144 os <<
"AnkleAdmittanceController " << getName();
147 getProfiler().report_all(3, os);
149 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
#define PROFILE_ANKLEADMITTANCECONTROLLER_DRP_COMPUTATION
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AnkleAdmittanceController(const std::string &name)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)
virtual void display(std::ostream &os) const