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_STATETRANSFORMATION_Q_COMPUTATION "State transformation q computation " 38 #define PROFILE_STATETRANSFORMATION_V_COMPUTATION "State transformation v computation " 40 #define INPUT_SIGNALS m_referenceFrameSIN << m_q_inSIN << m_v_inSIN 42 #define OUTPUT_SIGNALS m_qSOUT << m_vSOUT 50 "StateTransformation");
57 , CONSTRUCT_SIGNAL_IN(referenceFrame, MatrixHomogeneous)
62 , m_initSucceeded(false)
82 const size_t sz = input.size();
83 if((
size_t)(s.size())!=sz)
88 const MatrixHomogeneous & referenceFrame = m_referenceFrameSIN(iter);
91 const Eigen::Vector3d & euler = input.segment<3>(3);
93 const double roll = euler[0];
94 const double pitch = euler[1];
95 const double yaw = euler[2];
97 Eigen::Quaterniond quat;
98 quat = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
99 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
100 * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
103 M.linear() = quat.toRotationMatrix();
104 M.translation() = input.head<3>();
107 MatrixHomogeneous res = referenceFrame*M;
110 s.head<3>() = res.translation();
112 s.segment<3>(3) = res.linear().eulerAngles(2, 1, 0).reverse();
115 s.tail(sz-6) = input.tail(sz-6);
125 const size_t sz = input.size();
126 if((
size_t)(s.size())!=sz)
131 const MatrixHomogeneous & referenceFrame = m_referenceFrameSIN(iter);
134 s.head<3>() = referenceFrame.linear() * input.head<3>();
136 s.segment<3>(3) = referenceFrame.linear() * input.segment<3>(3);
139 s.tail(sz-6) = input.tail(sz-6);
154 os <<
"StateTransformation " << getName();
157 getProfiler().report_all(3, os);
159 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)