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!");
140 const Vector &q = m_qSIN(iter);
141 assert(q.size() ==
m_model.nq &&
"Unexpected size of signal q");
147 Eigen::Vector3d leverArm = sensorPlacement.rotation() *
m_rightLeverArm;
152 Eigen::Matrix<double,6,1> weight;
154 weight.head<3>() = sensorPlacement.rotation().transpose() *
m_rightHandWeight.head<3>();
155 weight.tail<3>() = sensorPlacement.rotation().transpose() *
m_rightHandWeight.tail<3>();
166 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rightWeight before initialization!");
172 const Vector &q = m_qSIN(iter);
173 assert(q.size() ==
m_model.nq &&
"Unexpected size of signal q");
178 Eigen::Vector3d leverArm = sensorPlacement.rotation() *
m_leftLeverArm;
183 Eigen::Matrix<double,6,1> weight;
185 weight.head<3>() = sensorPlacement.rotation().transpose() *
m_leftHandWeight.head<3>();
186 weight.tail<3>() = sensorPlacement.rotation().transpose() *
m_leftHandWeight.tail<3>();
197 SEND_WARNING_STREAM_MSG(
"Cannot compute signal sum before initialization!");
202 const Vector & rightWristForce = m_rightWristForceInSIN(iter);
203 assert(rightWristForce.size() == 6 &&
"Unexpected size of signal rightWristForceIn, should be 6.");
204 const Vector & rightWeight = m_rightWeightSINNER(iter);
205 assert(rightWeight.size() == 6 &&
"Unexpected size of signal rightWeight, should be 6.");
216 SEND_INFO_STREAM_MSG(
"Calibrating ft sensors...");
235 SEND_WARNING_STREAM_MSG(
"Cannot compute signal sum before initialization!");
240 const Vector & leftWristForce = m_leftWristForceInSIN(iter);
241 assert(leftWristForce.size() == 6 &&
"Unexpected size of signal leftWristForceIn, should be 6.");
242 const Vector & leftWeight = m_leftWeightSINNER(iter);
243 assert(leftWeight.size() == 6 &&
"Unexpected size of signal leftWeight, should be 6.");
274 SEND_WARNING_STREAM_MSG(
"Cannot set right hand weight before initialization!");
285 SEND_WARNING_STREAM_MSG(
"Cannot set left hand weight before initialization!");
289 m_leftLeverArm << leftLeverArm[0],leftLeverArm[1],leftLeverArm[2];
294 SEND_WARNING_STREAM_MSG(
"Sampling FT sensor for offset calibration... Robot should be in the air, with horizontal hand.");
314 os <<
"FtWristCalibration "<<getName();
317 getProfiler().report_all(3, os);
319 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.