11 #include <sot/core/debug.hh> 12 #include <dynamic-graph/factory.h> 15 #include <dynamic-graph/all-commands.h> 16 #include <sot/core/stop-watch.hh> 19 #define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating 27 namespace talos_balance
34 #define INPUT_SIGNALS m_rightWristForceInSIN << m_leftWristForceInSIN << m_qSIN 35 #define INNER_SIGNALS m_rightWeightSINNER << m_leftWeightSINNER 36 #define OUTPUT_SIGNALS m_rightWristForceOutSOUT << m_leftWristForceOutSOUT 44 "FtWristCalibration");
59 , m_robot_util(RefVoidRobotUtil())
64 , m_initSucceeded(false)
65 , m_removeWeight(false)
73 docCommandVoid1(
"Initialize the entity.",
74 "Robot reference (string)")));
75 addCommand(
"setRightHandConf",
77 docCommandVoid2(
"Set the data of the right hand",
78 "Vector of default forces in Newton",
79 "Vector of the weight lever arm")));
80 addCommand(
"setLeftHandConf",
82 docCommandVoid2(
"Set the data of the left hand",
83 "Vector of default forces in Newton",
84 "Vector of the weight lever arm")));
85 addCommand(
"calibrateWristSensor",
87 docCommandVoid0(
"Calibrate the wrist sensors")));
89 addCommand(
"setRemoveWeight",
91 docCommandVoid1(
"set RemoveWeight",
"desired removeWeight")));
98 dgADD_OSTREAM_TO_RTLOG (std::cout);
99 std::string localName(robotRef);
100 if (!isNameInRobotUtil(localName))
115 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
122 SEND_MSG(
"Entity Initialized", MSG_TYPE_INFO);
134 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rightWeight before initialization!");
138 const Vector &q = m_qSIN(iter);
139 assert(q.size() ==
m_model.nq &&
"Unexpected size of signal q");
152 weight.head<3>() = sensorPlacement.rotation().transpose() *
m_rightHandWeight.head<3>();
153 weight.tail<3>() = sensorPlacement.rotation().transpose() *
m_rightHandWeight.tail<3>();
164 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rightWeight before initialization!");
168 const Vector &q = m_qSIN(iter);
169 assert(q.size() ==
m_model.nq &&
"Unexpected size of signal q");
181 weight.head<3>() = sensorPlacement.rotation().transpose() *
m_leftHandWeight.head<3>();
182 weight.tail<3>() = sensorPlacement.rotation().transpose() *
m_leftHandWeight.tail<3>();
193 SEND_WARNING_STREAM_MSG(
"Cannot compute signal sum before initialization!");
196 const Vector & rightWristForce = m_rightWristForceInSIN(iter);
197 assert(rightWristForce.size() == 6 &&
"Unexpected size of signal rightWristForceIn, should be 6.");
198 const Vector & rightWeight = m_rightWeightSINNER(iter);
199 assert(rightWeight.size() == 6 &&
"Unexpected size of signal rightWeight, should be 6.");
210 SEND_INFO_STREAM_MSG(
"Calibrating ft sensors...");
229 SEND_WARNING_STREAM_MSG(
"Cannot compute signal sum before initialization!");
232 const Vector & leftWristForce = m_leftWristForceInSIN(iter);
233 assert(leftWristForce.size() == 6 &&
"Unexpected size of signal leftWristForceIn, should be 6.");
234 const Vector & leftWeight = m_leftWeightSINNER(iter);
235 assert(leftWeight.size() == 6 &&
"Unexpected size of signal leftWeight, should be 6.");
266 SEND_WARNING_STREAM_MSG(
"Cannot set right hand weight before initialization!");
277 SEND_WARNING_STREAM_MSG(
"Cannot set left hand weight before initialization!");
281 m_leftLeverArm << leftLeverArm[0],leftLeverArm[1],leftLeverArm[2];
286 SEND_WARNING_STREAM_MSG(
"Sampling FT sensor for offset calibration... Robot should be in the air, with horizontal hand.");
306 os <<
"FtWristCalibration "<<getName();
309 getProfiler().report_all(3, os);
311 catch (ExceptionSignal e) {}
pinocchio::Data * m_data
Pinocchio robot data.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FtWristCalibration, "FtWristCalibration")
Vector6d m_left_FT_offset_calibration_sum
Variable used during average computation of the offset.
Vector6d m_right_FT_offset
Offset or bias to be removed from Right FT sensor.
void setLeftHandConf(const double &leftW, const Vector &leftLeverArm)
Vector6d m_right_weight_calibration_sum
Variable used during average computation of the weight.
void init(const std::string &robotRef)
Initialize.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
bool m_removeWeight
If true, the weight of the end effector is removed from the force.
Vector6d m_right_FT_offset_calibration_sum
Variable used during average computation of the offset.
Vector3d m_rightLeverArm
right hand lever arm
RobotUtilShrPtr m_robot_util
Robot Util instance to get the sensor frame.
Vector6d m_left_FT_offset
Offset or bias to be removed from Left FT sensor.
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
FtWristCalibration(const std::string &name)
void setRemoveWeight(const bool &removeWeight)
Set to true if you want to remove the weight from the force.
Vector6d m_left_weight_calibration_sum
Variable used during average computation of the weight.
Vector6d m_leftHandWeight
weight of the left hand
Vector6d m_rightHandWeight
weight of the right hand
pinocchio::FrameIndex m_rightSensorId
Id of the force sensor frame.
pinocchio::FrameIndex m_leftSensorId
Id of the joint of the end-effector.
virtual void display(std::ostream &os) const
Vector3d m_leftLeverArm
left hand lever arm
pinocchio::Model m_model
Pinocchio robot model.
void calibrateWristSensor()
Command to calibrate the wrist sensors when the robot is in half sitting with the hands aligned...
int m_leftCalibrationIter
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done) ...
int m_rightCalibrationIter
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done) ...
bool m_initSucceeded
true if the entity has been successfully initialized
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)
void setRightHandConf(const double &rightW, const Vector &rightLeverArm)
Commands for setting the hand weight.