#include <sot/talos_balance/ft-wrist-calibration.hh>
Public Types | |
typedef Eigen::Matrix< double, 3, 1 > | Vector3d |
typedef Eigen::Matrix< double, 6, 1 > | Vector6d |
Public Member Functions | |
FtWristCalibration (const std::string &name) | |
void | calibrateWristSensor () |
Command to calibrate the wrist sensors when the robot is in half sitting with the hands aligned. More... | |
DECLARE_SIGNAL_IN (rightWristForceIn, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (leftWristForceIn, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (q, dynamicgraph::Vector) | |
DECLARE_SIGNAL_INNER (rightWeight, dynamicgraph::Vector) | |
DECLARE_SIGNAL_INNER (leftWeight, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (rightWristForceOut, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (leftWristForceOut, dynamicgraph::Vector) | |
virtual void | display (std::ostream &os) const |
void | displayRobotUtil () |
void | init (const std::string &robotRef) |
Initialize. More... | |
void | setLeftHandConf (const double &leftW, const Vector &leftLeverArm) |
void | setRemoveWeight (const bool &removeWeight) |
Set to true if you want to remove the weight from the force. More... | |
void | setRightHandConf (const double &rightW, const Vector &rightLeverArm) |
Commands for setting the hand weight. More... | |
Protected Attributes | |
pinocchio::Data * | m_data |
Pinocchio robot data. More... | |
bool | m_initSucceeded |
true if the entity has been successfully initialized More... | |
Vector6d | m_left_FT_offset |
Offset or bias to be removed from Left FT sensor. More... | |
Vector6d | m_left_FT_offset_calibration_sum |
Variable used during average computation of the offset. More... | |
Vector6d | m_left_weight_calibration_sum |
Variable used during average computation of the weight. More... | |
int | m_leftCalibrationIter = -2 |
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done) More... | |
Vector6d | m_leftHandWeight |
weight of the left hand More... | |
Vector3d | m_leftLeverArm |
left hand lever arm More... | |
pinocchio::FrameIndex | m_leftSensorId |
Id of the joint of the end-effector. More... | |
pinocchio::Model | m_model |
Pinocchio robot model. More... | |
bool | m_removeWeight |
If true, the weight of the end effector is removed from the force. More... | |
Vector6d | m_right_FT_offset |
Offset or bias to be removed from Right FT sensor. More... | |
Vector6d | m_right_FT_offset_calibration_sum |
Variable used during average computation of the offset. More... | |
Vector6d | m_right_weight_calibration_sum |
Variable used during average computation of the weight. More... | |
int | m_rightCalibrationIter = -2 |
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done) More... | |
Vector6d | m_rightHandWeight |
weight of the right hand More... | |
Vector3d | m_rightLeverArm |
right hand lever arm More... | |
pinocchio::FrameIndex | m_rightSensorId |
Id of the force sensor frame. More... | |
RobotUtilShrPtr | m_robot_util |
Robot Util instance to get the sensor frame. More... | |
Definition at line 58 of file ft-wrist-calibration.hh.
typedef Eigen::Matrix<double, 3, 1> Vector3d |
Definition at line 102 of file ft-wrist-calibration.hh.
typedef Eigen::Matrix<double, 6, 1> Vector6d |
Definition at line 101 of file ft-wrist-calibration.hh.
FtWristCalibration | ( | const std::string & | name | ) |
Definition at line 50 of file ft-wrist-calibration.cpp.
References FtWristCalibration::calibrateWristSensor(), FtWristCalibration::init(), INNER_SIGNALS, INPUT_SIGNALS, OUTPUT_SIGNALS, FtWristCalibration::setLeftHandConf(), FtWristCalibration::setRemoveWeight(), and FtWristCalibration::setRightHandConf().
void calibrateWristSensor | ( | ) |
Command to calibrate the wrist sensors when the robot is in half sitting with the hands aligned.
Definition at line 292 of file ft-wrist-calibration.cpp.
References CALIB_ITER_TIME, FtWristCalibration::m_left_FT_offset_calibration_sum, FtWristCalibration::m_leftCalibrationIter, FtWristCalibration::m_right_FT_offset_calibration_sum, and FtWristCalibration::m_rightCalibrationIter.
Referenced by FtWristCalibration::FtWristCalibration().
DECLARE_SIGNAL_IN | ( | rightWristForceIn | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | leftWristForceIn | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | q | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_INNER | ( | rightWeight | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_INNER | ( | leftWeight | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | rightWristForceOut | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | leftWristForceOut | , |
dynamicgraph::Vector | |||
) |
|
virtual |
Definition at line 312 of file ft-wrist-calibration.cpp.
void displayRobotUtil | ( | ) |
void init | ( | const std::string & | robotRef | ) |
Initialize.
Definition at line 96 of file ft-wrist-calibration.cpp.
References FtWristCalibration::m_data, FtWristCalibration::m_initSucceeded, FtWristCalibration::m_left_FT_offset, FtWristCalibration::m_left_FT_offset_calibration_sum, FtWristCalibration::m_left_weight_calibration_sum, FtWristCalibration::m_leftSensorId, FtWristCalibration::m_model, FtWristCalibration::m_right_FT_offset, FtWristCalibration::m_right_FT_offset_calibration_sum, FtWristCalibration::m_right_weight_calibration_sum, FtWristCalibration::m_rightSensorId, and FtWristCalibration::m_robot_util.
Referenced by FtWristCalibration::FtWristCalibration().
void setLeftHandConf | ( | const double & | leftW, |
const Vector & | leftLeverArm | ||
) |
Definition at line 281 of file ft-wrist-calibration.cpp.
References FtWristCalibration::m_initSucceeded, FtWristCalibration::m_leftHandWeight, and FtWristCalibration::m_leftLeverArm.
Referenced by FtWristCalibration::FtWristCalibration().
void setRemoveWeight | ( | const bool & | removeWeight | ) |
Set to true if you want to remove the weight from the force.
[in] | removeWeight | Boolean used to remove the weight |
Definition at line 301 of file ft-wrist-calibration.cpp.
References FtWristCalibration::m_removeWeight.
Referenced by FtWristCalibration::FtWristCalibration().
void setRightHandConf | ( | const double & | rightW, |
const Vector & | rightLeverArm | ||
) |
Commands for setting the hand weight.
Definition at line 270 of file ft-wrist-calibration.cpp.
References FtWristCalibration::m_initSucceeded, FtWristCalibration::m_rightHandWeight, and FtWristCalibration::m_rightLeverArm.
Referenced by FtWristCalibration::FtWristCalibration().
|
protected |
Pinocchio robot data.
Definition at line 110 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::init().
|
protected |
true if the entity has been successfully initialized
Definition at line 132 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), FtWristCalibration::init(), FtWristCalibration::setLeftHandConf(), and FtWristCalibration::setRightHandConf().
|
protected |
Offset or bias to be removed from Left FT sensor.
Definition at line 122 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and FtWristCalibration::init().
|
protected |
Variable used during average computation of the offset.
Definition at line 126 of file ft-wrist-calibration.hh.
Referenced by FtWristCalibration::calibrateWristSensor(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and FtWristCalibration::init().
|
protected |
Variable used during average computation of the weight.
Definition at line 130 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and FtWristCalibration::init().
|
protected |
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done)
Definition at line 118 of file ft-wrist-calibration.hh.
Referenced by FtWristCalibration::calibrateWristSensor(), and dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
weight of the left hand
Definition at line 136 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::setLeftHandConf().
|
protected |
left hand lever arm
Definition at line 140 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::setLeftHandConf().
|
protected |
Id of the joint of the end-effector.
Definition at line 114 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::init().
|
protected |
Pinocchio robot model.
Definition at line 108 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::init().
|
protected |
If true, the weight of the end effector is removed from the force.
Definition at line 142 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and FtWristCalibration::setRemoveWeight().
|
protected |
Offset or bias to be removed from Right FT sensor.
Definition at line 120 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and FtWristCalibration::init().
|
protected |
Variable used during average computation of the offset.
Definition at line 124 of file ft-wrist-calibration.hh.
Referenced by FtWristCalibration::calibrateWristSensor(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and FtWristCalibration::init().
|
protected |
Variable used during average computation of the weight.
Definition at line 128 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and FtWristCalibration::init().
|
protected |
Number of iteration for right calibration (-2 = not calibrated, -1 = caibration done)
Definition at line 116 of file ft-wrist-calibration.hh.
Referenced by FtWristCalibration::calibrateWristSensor(), and dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
weight of the right hand
Definition at line 134 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::setRightHandConf().
|
protected |
right hand lever arm
Definition at line 138 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::setRightHandConf().
|
protected |
Id of the force sensor frame.
Definition at line 112 of file ft-wrist-calibration.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and FtWristCalibration::init().
|
protected |
Robot Util instance to get the sensor frame.
Definition at line 106 of file ft-wrist-calibration.hh.
Referenced by FtWristCalibration::init().