sot-talos-balance  1.7.0
ft-wrist-calibration.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  * LAAS-CNRS
4  * F. Bailly
5  * T. Flayol
6  * F. Risbourg
7  */
8 
9 #ifndef __sot_talos_balance_ft_wrist_calibration_H__
10 #define __sot_talos_balance_ft_wrist_calibration_H__
11 
12 /* --------------------------------------------------------------------- */
13 /* --- API ------------------------------------------------------------- */
14 /* --------------------------------------------------------------------- */
15 
16 #if defined (WIN32)
17 # if defined (__sot_talos_balance_ft_wrist_calibration_H__)
18 # define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllexport)
19 # else
20 # define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllimport)
21 # endif
22 #else
23 # define SOTFTWRISTCALIBRATION_EXPORT
24 #endif
25 
26 
27 /* --------------------------------------------------------------------- */
28 /* --- INCLUDE --------------------------------------------------------- */
29 /* --------------------------------------------------------------------- */
30 
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>
36 #include <map>
37 #include "boost/assign.hpp"
38 
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>
46 
47 
49 
50 namespace dynamicgraph {
51  namespace sot {
52  namespace talos_balance {
53 
54  /* --------------------------------------------------------------------- */
55  /* --- CLASS ----------------------------------------------------------- */
56  /* --------------------------------------------------------------------- */
57 
59  :public::dynamicgraph::Entity
60  {
61  //typedef FtCalibration EntityClassName;
62  DYNAMIC_GRAPH_ENTITY_DECL();
63 
64  public:
65  /* --- CONSTRUCTOR ---- */
66  FtWristCalibration( const std::string & name);
68  void init(const std::string & robotRef);
69 
70  /* --- SIGNALS --- */
71  DECLARE_SIGNAL_IN(rightWristForceIn, dynamicgraph::Vector);
72  DECLARE_SIGNAL_IN(leftWristForceIn, dynamicgraph::Vector);
73  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
74  DECLARE_SIGNAL_INNER(rightWeight, dynamicgraph::Vector);
75  DECLARE_SIGNAL_INNER(leftWeight, dynamicgraph::Vector);
76  DECLARE_SIGNAL_OUT(rightWristForceOut, dynamicgraph::Vector);
77  DECLARE_SIGNAL_OUT(leftWristForceOut, dynamicgraph::Vector);
78 
79  /* --- COMMANDS --- */
80 
82  void setRightHandConf(const double &rightW, const Vector &rightLeverArm);
83  void setLeftHandConf(const double &leftW, const Vector &leftLeverArm);
84 
86  void calibrateWristSensor();
87 
93  void setRemoveWeight(const bool &removeWeight);
94 
95  void displayRobotUtil();
96 
97  /* --- ENTITY INHERITANCE --- */
98  virtual void display( std::ostream& os ) const;
99 
100  /* --- TYPEDEFS ---- */
101  typedef Eigen::Matrix<double, 6, 1> Vector6d;
102  typedef Eigen::Matrix<double, 3, 1> Vector3d;
103 
104  protected:
106  RobotUtilShrPtr m_robot_util;
108  pinocchio::Model m_model;
110  pinocchio::Data *m_data;
112  pinocchio::FrameIndex m_rightSensorId;
114  pinocchio::FrameIndex m_leftSensorId;
116  int m_rightCalibrationIter = -2;
118  int m_leftCalibrationIter = -2;
138  Vector3d m_rightLeverArm;
140  Vector3d m_leftLeverArm;
143 
144 
145  }; // class FtWristCalibration
146 
147  } // namespace talos_balance
148  } // namespace sot
149 } // namespace dynamicgraph
150 
151 
152 
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< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
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.
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.
pinocchio::FrameIndex m_rightSensorId
Id of the force sensor frame.
pinocchio::FrameIndex m_leftSensorId
Id of the joint of the end-effector.
pinocchio::Model m_model
Pinocchio robot model.
#define SOTFTWRISTCALIBRATION_EXPORT
bool m_initSucceeded
true if the entity has been successfully initialized