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_QUATTOEULER_COMPUTATION "QuatToEuler computation " 39 #define INPUT_SIGNALS m_quaternionSIN 41 #define OUTPUT_SIGNALS m_eulerSOUT 62 addCommand(
"init", makeCommandVoid0(*
this, &
QuatToEuler::init, docCommandVoid0(
"Initialize the entity.")));
75 const Eigen::Map<const Eigen::Quaterniond> quat(input.segment<4>(3).data());
77 size_t sz = input.size();
80 s.head<3>() = input.head<3>();
82 Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
88 s.tail(sz-7) = input.tail(sz-7);
103 os <<
"QuatToEuler " << getName();
106 getProfiler().report_all(3, os);
108 catch (ExceptionSignal e) {}
virtual void display(std::ostream &os) const
#define PROFILE_QUATTOEULER_COMPUTATION
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW QuatToEuler(const std::string &name)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)