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_SIMPLEREFERENCEFRAME_DCM_COMPUTATION "SimpleReferenceFrame: dcm computation " 39 #define INPUT_SIGNALS m_footLeftSIN << m_footRightSIN << m_resetSIN 41 #define OUTPUT_SIGNALS m_referenceFrameSOUT 49 "SimpleReferenceFrame");
56 , CONSTRUCT_SIGNAL_IN(footLeft, MatrixHomogeneous)
57 , CONSTRUCT_SIGNAL_IN(footRight, MatrixHomogeneous)
58 , CONSTRUCT_SIGNAL_IN(reset, bool)
59 , CONSTRUCT_SIGNAL_OUT(referenceFrame, MatrixHomogeneous, m_footLeftSIN << m_footRightSIN << m_resetSIN)
61 , m_initSucceeded(false)
67 addCommand(
"init", makeCommandVoid1(*
this, &
SimpleReferenceFrame::init, docCommandVoid1(
"Initialize the entity.",
"Robot name")));
75 std::string localName(robotName);
76 if (isNameInRobotUtil(localName))
82 SEND_ERROR_STREAM_MSG(
"You should have a robotUtil pointer initialized before");
86 catch (
const std::exception& e)
88 SEND_ERROR_STREAM_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename);
105 SEND_WARNING_STREAM_MSG(
"Cannot compute signal referenceFrame before initialization!");
109 const MatrixHomogeneous & footLeft = m_footLeftSIN(iter);
110 const MatrixHomogeneous & footRight = m_footRightSIN(iter);
111 const bool reset = m_resetSIN.isPlugged() ? m_resetSIN(iter) :
false;
115 Eigen::Vector3d centerTranslation = ( footLeft.translation() + footRight.translation() )/2 +
m_rightFootSoleXYZ;
116 centerTranslation[2] = 0;
136 os <<
"SimpleReferenceFrame " << getName();
139 getProfiler().report_all(3, os);
141 catch (ExceptionSignal e) {}
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
void init(const std::string &robotName)
Vector m_rightFootSoleXYZ
MatrixHomogeneous m_referenceFrame
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleReferenceFrame(const std::string &name)
RobotUtilShrPtr m_robot_util
virtual void display(std::ostream &os) const
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)