sot-talos-balance  1.5.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 <dynamic-graph/signal-helper.h>
32 #include <dynamic-graph/real-time-logger.h>
33 #include <sot/core/matrix-geometry.hh>
34 #include <sot/core/robot-utils.hh>
35 #include <map>
36 #include "boost/assign.hpp"
37 
38 #include <pinocchio/parsers/urdf.hpp>
39 #include <pinocchio/multibody/model.hpp>
40 #include <pinocchio/spatial/se3.hpp>
41 #include <pinocchio/spatial/motion.hpp>
42 #include <pinocchio/algorithm/kinematics.hpp>
43 #include <pinocchio/algorithm/frames.hpp>
44 #include <pinocchio/algorithm/center-of-mass.hpp>
45 
46 
48 
49 namespace dynamicgraph {
50  namespace sot {
51  namespace talos_balance {
52 
53  /* --------------------------------------------------------------------- */
54  /* --- CLASS ----------------------------------------------------------- */
55  /* --------------------------------------------------------------------- */
56 
58  :public::dynamicgraph::Entity
59  {
60  //typedef FtCalibration EntityClassName;
61  DYNAMIC_GRAPH_ENTITY_DECL();
62 
63  public:
64  /* --- CONSTRUCTOR ---- */
65  FtWristCalibration( const std::string & name);
67  void init(const std::string & robotRef);
68 
69  /* --- SIGNALS --- */
70  DECLARE_SIGNAL_IN(rightWristForceIn, dynamicgraph::Vector);
71  DECLARE_SIGNAL_IN(leftWristForceIn, dynamicgraph::Vector);
72  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
73  DECLARE_SIGNAL_INNER(rightWeight, dynamicgraph::Vector);
74  DECLARE_SIGNAL_INNER(leftWeight, dynamicgraph::Vector);
75  DECLARE_SIGNAL_OUT(rightWristForceOut, dynamicgraph::Vector);
76  DECLARE_SIGNAL_OUT(leftWristForceOut, dynamicgraph::Vector);
77 
78  /* --- COMMANDS --- */
79 
81  void setRightHandConf(const double &rightW, const Vector &rightLeverArm);
82  void setLeftHandConf(const double &leftW, const Vector &leftLeverArm);
83 
85  void calibrateWristSensor();
86 
92  void setRemoveWeight(const bool &removeWeight);
93 
94  void displayRobotUtil();
95 
96  /* --- ENTITY INHERITANCE --- */
97  virtual void display( std::ostream& os ) const;
98 
99  /* --- TYPEDEFS ---- */
100  typedef Eigen::Matrix<double, 6, 1> Vector6d;
101  typedef Eigen::Matrix<double, 3, 1> Vector3d;
102 
103  protected:
105  RobotUtilShrPtr m_robot_util;
107  pinocchio::Model m_model;
109  pinocchio::Data *m_data;
111  pinocchio::FrameIndex m_rightSensorId;
113  pinocchio::FrameIndex m_leftSensorId;
115  int m_rightCalibrationIter = -2;
117  int m_leftCalibrationIter = -2;
137  Vector3d m_rightLeverArm;
139  Vector3d m_leftLeverArm;
142 
143 
144  }; // class FtWristCalibration
145 
146  } // namespace talos_balance
147  } // namespace sot
148 } // namespace dynamicgraph
149 
150 
151 
152 #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