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_SIMPLE_CONTROLLER_6D_DX_REF_COMPUTATION "SimpleController6d: v_ref computation " 39 #define INPUT_SIGNALS m_KpSIN << m_xSIN << m_x_desSIN << m_v_desSIN 41 #define OUTPUT_SIGNALS m_v_refSOUT 49 "SimpleController6d");
61 , m_initSucceeded(false)
74 template <
typename Derived>
92 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_ref before initialization!");
100 const Vector & Kp = m_KpSIN(iter);
102 const MatrixHomogeneous & x = m_xSIN(iter);
103 const MatrixHomogeneous & x_des = m_x_desSIN(iter);
107 const Eigen::Vector3d e_O = 0.5*(x.linear().col(0).cross(x_des.linear().col(0))
108 + x.linear().col(1).cross(x_des.linear().col(1))
109 + x.linear().col(2).cross(x_des.linear().col(2))
112 const Eigen::Matrix3d L = -0.5*(
skew(x_des.linear().col(0))*
skew(x.linear().col(0))
113 +
skew(x_des.linear().col(1))*
skew(x.linear().col(1))
114 +
skew(x_des.linear().col(2))*
skew(x.linear().col(2))
117 Eigen::Matrix<double,6,1> dv_ref;
120 dv_ref.head<3>() = x.linear().transpose() * Kp.head<3>().cwiseProduct(x_des.translation()-x.translation());
122 dv_ref.tail<3>() = x.linear().transpose() * L.inverse() * Kp.tail<3>().cwiseProduct(e_O);
124 if(m_v_desSIN.isPlugged()) {
145 os <<
"SimpleController6d " << getName();
148 getProfiler().report_all(3, os);
150 catch (ExceptionSignal e) {}
virtual void display(std::ostream &os) const
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleController6d(const std::string &name)
Eigen::Matrix3d skew(const Eigen::MatrixBase< Derived > &v)
#define PROFILE_SIMPLE_CONTROLLER_6D_DX_REF_COMPUTATION
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)