GCC Code Coverage Report

Directory: ./
File: include/sot/dynamic-pinocchio/force-compensation.h
Date: 2025-01-28 11:15:54
Exec Total Coverage
Lines: 0 1 0.0%
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__
169