GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/dynamic-pinocchio/force-compensation.h Lines: 0 1 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 0 - %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * Fran├žois Bleibel,
4
 * Olivier Stasse,
5
 *
6
 * CNRS/AIST
7
 *
8
 */
9
10
#ifndef __SOT_SOTFORCECOMPENSATION_H__
11
#define __SOT_SOTFORCECOMPENSATION_H__
12
13
/* --------------------------------------------------------------------- */
14
/* --- INCLUDE --------------------------------------------------------- */
15
/* --------------------------------------------------------------------- */
16
17
/* Matrix */
18
#include <dynamic-graph/linear-algebra.h>
19
20
/* SOT */
21
#include <dynamic-graph/entity.h>
22
#include <dynamic-graph/signal-ptr.h>
23
#include <dynamic-graph/signal-time-dependent.h>
24
25
#include <sot/core/matrix-geometry.hh>
26
27
/* STD */
28
#include <string>
29
30
/* --------------------------------------------------------------------- */
31
/* --- API ------------------------------------------------------------- */
32
/* --------------------------------------------------------------------- */
33
34
#if defined(WIN32)
35
#if defined(force_compensation_EXPORTS)
36
#define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
37
#else
38
#define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
39
#endif
40
#else
41
#define SOTFORCECOMPENSATION_EXPORT
42
#endif
43
44
namespace dynamicgraph {
45
namespace sot {
46
namespace dg = dynamicgraph;
47
48
/* --------------------------------------------------------------------- */
49
/* --- CLASS ----------------------------------------------------------- */
50
/* --------------------------------------------------------------------- */
51
52
class SOTFORCECOMPENSATION_EXPORT ForceCompensation {
53
 private:
54
  static MatrixRotation I3;
55
56
 protected:
57
  bool usingPrecompensation;
58
59
 public:
60
  ForceCompensation(void);
61
  static MatrixForce& computeHandXworld(
62
      const MatrixRotation& worldRhand,
63
      const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
64
65
  static MatrixForce& computeHandVsensor(const MatrixRotation& sensorRhand,
66
                                         MatrixForce& res);
67
  static MatrixForce& computeSensorXhand(
68
      const MatrixRotation& sensorRhand,
69
      const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
70
  /*   static dynamicgraph::Matrix& computeInertiaSensor( const
71
   * dynamicgraph::Matrix& inertiaJoint, */
72
  /* 					   const MatrixForce& sensorXhand, */
73
  /* 					   dynamicgraph::Matrix& res ); */
74
75
  static dynamicgraph::Vector& computeTorsorCompensated(
76
      const dynamicgraph::Vector& torqueInput,
77
      const dynamicgraph::Vector& torquePrecompensation,
78
      const dynamicgraph::Vector& gravity, const MatrixForce& handXworld,
79
      const MatrixForce& handVsensor, const dynamicgraph::Matrix& gainSensor,
80
      const dynamicgraph::Vector& momentum, dynamicgraph::Vector& res);
81
82
  static dynamicgraph::Vector& crossProduct_V_F(
83
      const dynamicgraph::Vector& velocity, const dynamicgraph::Vector& force,
84
      dynamicgraph::Vector& res);
85
  static dynamicgraph::Vector& computeMomentum(
86
      const dynamicgraph::Vector& velocity,
87
      const dynamicgraph::Vector& acceleration, const MatrixForce& sensorXhand,
88
      const dynamicgraph::Matrix& inertiaJoint, dynamicgraph::Vector& res);
89
90
  static dynamicgraph::Vector& computeDeadZone(
91
      const dynamicgraph::Vector& torqueInput,
92
      const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
93
94
 public:  // CALIBRATION
95
  std::list<dynamicgraph::Vector> torsorList;
96
  std::list<MatrixRotation> rotationList;
97
98
  void clearCalibration(void);
99
  void addCalibrationValue(const dynamicgraph::Vector& torsor,
100
                           const MatrixRotation& worldRhand);
101
102
  dynamicgraph::Vector calibrateTransSensorCom(
103
      const dynamicgraph::Vector& gravity, const MatrixRotation& handRsensor);
104
  dynamicgraph::Vector calibrateGravity(
105
      const MatrixRotation& handRsensor,
106
      bool precompensationCalibration = false,
107
      const MatrixRotation& hand0Rsensor = I3);
108
};
109
110
/* --------------------------------------------------------------------- */
111
/* --- PLUGIN ---------------------------------------------------------- */
112
/* --------------------------------------------------------------------- */
113
114
class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin
115
    : public dg::Entity,
116
      public ForceCompensation {
117
 public:
118
  static const std::string CLASS_NAME;
119
  virtual const std::string& getClassName(void) const { return CLASS_NAME; }
120
  bool calibrationStarted;
121
122
 public: /* --- CONSTRUCTION --- */
123
  ForceCompensationPlugin(const std::string& name);
124
  virtual ~ForceCompensationPlugin(void);
125
126
 public: /* --- SIGNAL --- */
127
  /* --- INPUTS --- */
128
  dg::SignalPtr<dynamicgraph::Vector, int> torsorSIN;
129
  dg::SignalPtr<MatrixRotation, int> worldRhandSIN;
130
131
  /* --- CONSTANTS --- */
132
  dg::SignalPtr<MatrixRotation, int> handRsensorSIN;
133
  dg::SignalPtr<dynamicgraph::Vector, int> translationSensorComSIN;
134
  dg::SignalPtr<dynamicgraph::Vector, int> gravitySIN;
135
  dg::SignalPtr<dynamicgraph::Vector, int> precompensationSIN;
136
  dg::SignalPtr<dynamicgraph::Matrix, int> gainSensorSIN;
137
  dg::SignalPtr<dynamicgraph::Vector, int> deadZoneLimitSIN;
138
  dg::SignalPtr<dynamicgraph::Vector, int> transSensorJointSIN;
139
  dg::SignalPtr<dynamicgraph::Matrix, int> inertiaJointSIN;
140
141
  dg::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
142
  dg::SignalPtr<dynamicgraph::Vector, int> accelerationSIN;
143
144
  /* --- INTERMEDIATE OUTPUTS --- */
145
  dg::SignalTimeDependent<MatrixForce, int> handXworldSOUT;
146
  dg::SignalTimeDependent<MatrixForce, int> handVsensorSOUT;
147
  dg::SignalPtr<dynamicgraph::Vector, int> torsorDeadZoneSIN;
148
149
  dg::SignalTimeDependent<MatrixForce, int> sensorXhandSOUT;
150
  // dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
151
  dg::SignalTimeDependent<dynamicgraph::Vector, int> momentumSOUT;
152
  dg::SignalPtr<dynamicgraph::Vector, int> momentumSIN;
153
154
  /* --- OUTPUTS --- */
155
  dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorCompensatedSOUT;
156
  dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorDeadZoneSOUT;
157
158
  typedef int sotDummyType;
159
  dg::SignalTimeDependent<sotDummyType, int> calibrationTrigerSOUT;
160
161
 public: /* --- COMMANDLINE --- */
162
  sotDummyType& calibrationTriger(sotDummyType& dummy, int time);
163
};
164
165
}  // namespace sot
166
}  // namespace dynamicgraph
167
168
#endif  // #ifndef __SOT_SOTFORCECOMPENSATION_H__