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_EULERTOQUAT_COMPUTATION "EulerToQuat computation " 39 #define INPUT_SIGNALS m_eulerSIN 41 #define OUTPUT_SIGNALS m_quaternionSOUT 62 addCommand(
"init", makeCommandVoid0(*
this, &
EulerToQuat::init, docCommandVoid0(
"Initialize the entity.")));
72 const size_t sz = input.size();
73 if((
size_t)(s.size())!=(sz+1))
78 const Eigen::Vector3d & euler = input.segment<3>(3);
80 const double roll = euler[0];
81 const double pitch = euler[1];
82 const double yaw = euler[2];
84 Eigen::Quaterniond quat;
85 quat = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
86 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
87 * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
89 s.head<3>() = input.head<3>();
91 s.segment<4>(3) = quat.coeffs();
94 s.tail(sz-6) = input.tail(sz-6);
109 os <<
"EulerToQuat " << getName();
112 getProfiler().report_all(3, os);
114 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
virtual void display(std::ostream &os) const
#define PROFILE_EULERTOQUAT_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EulerToQuat(const std::string &name)
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)