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;
38 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUSUM_COMPUTATION "CoupledAdmittanceController: tauSum computation " 39 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUDIFF_COMPUTATION "CoupledAdmittanceController: tauDiff computation " 40 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUDESSUM_COMPUTATION "CoupledAdmittanceController: tauDesSum computation " 41 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUDESDIFF_COMPUTATION "CoupledAdmittanceController: tauDesDiff computation " 42 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFSUM_COMPUTATION "CoupledAdmittanceController: dqRefSum computation " 43 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFDIFF_COMPUTATION "CoupledAdmittanceController: dqReDiff computation " 44 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFL_COMPUTATION "CoupledAdmittanceController: dqRefL computation " 45 #define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFR_COMPUTATION "CoupledAdmittanceController: dqRefR computation " 47 #define INPUT_SIGNALS m_kSumSIN << m_kDiffSIN << m_tauLSIN << m_tauRSIN << m_tauDesLSIN << m_tauDesRSIN 49 #define INNER_SIGNALS m_tauSumSINNER << m_tauDiffSINNER << m_tauDesSumSINNER << m_tauDesDiffSINNER << m_dqRefSumSINNER << m_dqRefDiffSINNER 51 #define OUTPUT_SIGNALS m_dqRefLSOUT << m_dqRefRSOUT 59 "CoupledAdmittanceController");
91 const Vector & tauL = m_tauLSIN(iter);
92 const Vector & tauR = m_tauRSIN(iter);
105 const Vector & tauL = m_tauLSIN(iter);
106 const Vector & tauR = m_tauRSIN(iter);
119 const Vector & tauDesL = m_tauDesLSIN(iter);
120 const Vector & tauDesR = m_tauDesRSIN(iter);
122 s = tauDesL + tauDesR;
133 const Vector & tauDesL = m_tauDesLSIN(iter);
134 const Vector & tauDesR = m_tauDesRSIN(iter);
136 s = tauDesL - tauDesR;
147 const Vector & tau = m_tauSumSINNER(iter);
148 const Vector & tauDes = m_tauDesSumSINNER(iter);
149 const Vector & k = m_kSumSIN(iter);
151 s = k.cwiseProduct(tauDes - tau);
162 const Vector & tau = m_tauDiffSINNER(iter);
163 const Vector & tauDes = m_tauDesDiffSINNER(iter);
164 const Vector & k = m_kDiffSIN(iter);
166 s = k.cwiseProduct(tauDes - tau);
177 const Vector & dqRefSum = m_dqRefSumSINNER(iter);
178 const Vector & dqRefDiff = m_dqRefDiffSINNER(iter);
180 s = dqRefSum + dqRefDiff;
191 const Vector & dqRefSum = m_dqRefSumSINNER(iter);
192 const Vector & dqRefDiff = m_dqRefDiffSINNER(iter);
194 s = dqRefSum - dqRefDiff;
209 os <<
"CoupledAdmittanceController " << getName();
212 getProfiler().report_all(3, os);
214 catch (ExceptionSignal e) {}
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFSUM_COMPUTATION
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUDESDIFF_COMPUTATION
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUDESSUM_COMPUTATION
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFL_COMPUTATION
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
virtual void display(std::ostream &os) const
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFR_COMPUTATION
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_DQREFDIFF_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CoupledAdmittanceController(const std::string &name)
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUDIFF_COMPUTATION
#define PROFILE_COUPLED_ADMITTANCECONTROLLER_TAUSUM_COMPUTATION
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)