10 #include <sot/core/debug.hh> 11 #include <dynamic-graph/factory.h> 14 #include <dynamic-graph/all-commands.h> 15 #include <sot/core/stop-watch.hh> 18 #define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating 26 namespace talos_balance
33 #define INPUT_SIGNALS m_right_foot_force_inSIN << m_left_foot_force_inSIN 34 #define OUTPUT_SIGNALS m_right_foot_force_outSOUT << m_left_foot_force_outSOUT 52 , CONSTRUCT_SIGNAL_OUT(right_foot_force_out,
dynamicgraph::
Vector, m_right_foot_force_inSIN)
53 , CONSTRUCT_SIGNAL_OUT(left_foot_force_out,
dynamicgraph::
Vector, m_left_foot_force_inSIN)
54 ,m_robot_util(RefVoidRobotUtil())
55 ,m_initSucceeded(false)
63 docCommandVoid1(
"Initialize the entity.",
64 "Robot reference (string)")));
65 addCommand(
"setRightFootWeight",
67 docCommandVoid1(
"Set the weight of the right foot underneath the sensor",
68 "Vector of default forces in Newton")));
69 addCommand(
"setLeftFootWeight",
71 docCommandVoid1(
"Set the weight of the left foot underneath the sensor",
72 "Vector of default forces in Newton")));
73 addCommand(
"calibrateFeetSensor",
75 docCommandVoid0(
"Calibrate the feet senors")));
81 dgADD_OSTREAM_TO_RTLOG (std::cout);
82 std::string localName(robotRef);
84 if (!isNameInRobotUtil(localName))
96 SEND_MSG(
"Entity Initialized",MSG_TYPE_INFO);
108 SEND_WARNING_STREAM_MSG(
"Cannot compute signal sum before initialization!");
114 const Vector & right_foot_force = m_right_foot_force_inSIN(iter);
116 assert(right_foot_force.size() == 6 &&
"Unexpected size of signal right_foot_force_in, should be 6.");
126 SEND_INFO_STREAM_MSG(
"Calibrating ft sensors...");
140 SEND_WARNING_STREAM_MSG(
"Cannot compute signal sum before initialization!");
146 const Vector & left_foot_force = m_left_foot_force_inSIN(iter);
148 assert(left_foot_force.size() == 6 &&
"Unexpected size of signal left_foot_force_in, should be 6.");
171 SEND_WARNING_STREAM_MSG(
"Cannot set right foot weight before initialization!");
181 SEND_WARNING_STREAM_MSG(
"Cannot set left foot weight before initialization!");
189 SEND_WARNING_STREAM_MSG(
"Sampling FT sensor for offset calibration... Robot should be in the air, with horizontal feet.");
204 os <<
"FtCalibration "<<getName();
207 getProfiler().report_all(3, os);
209 catch (ExceptionSignal e) {}
virtual void display(std::ostream &os) const
int m_right_calibration_iter
Vector6d m_right_FT_offset_calibration_sum
Offset or bias to be removed from Left FT sensor.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Vector6d m_left_foot_weight
void setRightFootWeight(const double &rightW)
Commands for setting the feet weight.
AdmittanceControllerEndEffector EntityClassName
void init(const std::string &robotRef)
Initialize.
Vector6d m_right_foot_weight
true if the entity has been successfully initialized
void setLeftFootWeight(const double &leftW)
RobotUtilShrPtr m_robot_util
bool m_initSucceeded
Variable used durring average computation of the offset.
Vector6d m_left_FT_offset
Offset or bias to be removed from Right FT sensor.
Vector6d m_right_FT_offset
Number of iteration left for calibration (-1= not cailbrated, 0=caibration done)
FtCalibration(const std::string &name)
int m_left_calibration_iter
Number of iteration left for calibration (-1= not cailbrated, 0=caibration done)
Vector6d m_left_FT_offset_calibration_sum
Variable used durring average computation of the offset.
void calibrateFeetSensor()
Command to calibrate the foot sensors when the robot is standing in the air with horizontal feet...
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FtCalibration, "FtCalibration")
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)