19 #include <sot/core/debug.hh> 20 #include <dynamic-graph/factory.h> 21 #include <dynamic-graph/all-commands.h> 22 #include <sot/core/stop-watch.hh> 30 namespace talos_balance
32 namespace dg = ::dynamicgraph;
37 #define PROFILE_POSEQUATERNIONTOMATRIXHOMO_COMPUTATION "PoseQuaternionToMatrixHomo computation " 39 #define INPUT_SIGNALS m_sinSIN 41 #define OUTPUT_SIGNALS m_soutSOUT 49 "PoseQuaternionToMatrixHomo");
57 , CONSTRUCT_SIGNAL_OUT(sout, MatrixHomogeneous,
INPUT_SIGNALS)
75 const Eigen::Map<const Eigen::Quaterniond> q(vect.segment<4>(3).data());
77 s.translation() = vect.head<3>();
78 s.linear() = q.toRotationMatrix();
93 os <<
"PoseQuaternionToMatrixHomo " << getName();
96 getProfiler().report_all(3, os);
98 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")
#define PROFILE_POSEQUATERNIONTOMATRIXHOMO_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PoseQuaternionToMatrixHomo(const std::string &name)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)