9 #ifndef __sot_talos_balance_ft_wrist_calibration_H__ 10 #define __sot_talos_balance_ft_wrist_calibration_H__ 17 # if defined (__sot_talos_balance_ft_wrist_calibration_H__) 18 # define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllexport) 20 # define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllimport) 23 # define SOTFTWRISTCALIBRATION_EXPORT 31 #include <pinocchio/fwd.hpp> 32 #include <dynamic-graph/signal-helper.h> 33 #include <dynamic-graph/real-time-logger.h> 34 #include <sot/core/matrix-geometry.hh> 35 #include <sot/core/robot-utils.hh> 37 #include "boost/assign.hpp" 39 #include <pinocchio/parsers/urdf.hpp> 40 #include <pinocchio/multibody/model.hpp> 41 #include <pinocchio/spatial/se3.hpp> 42 #include <pinocchio/spatial/motion.hpp> 43 #include <pinocchio/algorithm/kinematics.hpp> 44 #include <pinocchio/algorithm/frames.hpp> 45 #include <pinocchio/algorithm/center-of-mass.hpp> 52 namespace talos_balance {
59 :public::dynamicgraph::Entity
62 DYNAMIC_GRAPH_ENTITY_DECL();
68 void init(
const std::string & robotRef);
82 void setRightHandConf(
const double &rightW,
const Vector &rightLeverArm);
83 void setLeftHandConf(
const double &leftW,
const Vector &leftLeverArm);
86 void calibrateWristSensor();
93 void setRemoveWeight(
const bool &removeWeight);
95 void displayRobotUtil();
98 virtual void display( std::ostream& os )
const;
116 int m_rightCalibrationIter = -2;
118 int m_leftCalibrationIter = -2;
153 #endif // #ifndef __sot_talos_balance_ft_wrist_calibration_H__ pinocchio::Data * m_data
Pinocchio robot data.
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.
Vector6d m_right_weight_calibration_sum
Variable used during average computation of the weight.
Eigen::Matrix< double, 3, 1 > Vector3d
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
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.
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.
Eigen::Matrix< double, 6, 1 > Vector6d
Vector3d m_leftLeverArm
left hand lever arm
pinocchio::Model m_model
Pinocchio robot model.
#define SOTFTWRISTCALIBRATION_EXPORT
bool m_initSucceeded
true if the entity has been successfully initialized