11 #include <sot/core/debug.hh> 12 #include <dynamic-graph/all-commands.h> 13 #include <dynamic-graph/factory.h> 14 #include <sot/core/stop-watch.hh> 15 #include "pinocchio/algorithm/frames.hpp" 16 #include "pinocchio/algorithm/center-of-mass.hpp" 22 namespace talos_balance
24 namespace dg = ::dynamicgraph;
29 using boost::math::normal;
32 #define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation" 33 #define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation" 34 #define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation" 36 #define INPUT_SIGNALS m_qSIN << m_vSIN 37 #define OUTPUT_SIGNALS m_cSOUT << m_dcSOUT 61 docCommandVoid2(
"Initialize the entity.",
63 "URDF file path (string)")));
71 std::string localName(robotRef);
72 if (isNameInRobotUtil(localName))
75 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
79 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
83 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename,
84 pinocchio::JointModelFreeFlyer(),
m_model);
86 catch (
const std::exception& e)
88 std::cout << e.what();
89 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
104 SEND_WARNING_STREAM_MSG(
"Cannot compute signal com before initialization!");
109 const Vector & dc = m_dcSOUT(iter);
119 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dcom before initialization!");
124 const Vector & q = m_qSIN(iter);
125 const Vector & v = m_vSIN(iter);
133 os <<
"DcmEstimator " << getName();
136 getProfiler().report_all(3, os);
138 catch (ExceptionSignal e) {}
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
void init(const double &dt, const std::string &urdfFile)
double m_dt
robot velocities according to pinocchio convention
RobotUtilShrPtr m_robot_util
true if the entity has been successfully initialized
virtual void display(std::ostream &os) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DcmEstimator(const std::string &name)
pinocchio::Model m_model
sampling time step
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)