GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/core/device.hh Lines: 1 3 33.3 %
Date: 2023-03-13 12:09:37 Branches: 0 0 - %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * Florent Lamiraux
4
 *
5
 * CNRS
6
 *
7
 */
8
9
#ifndef SOT_DEVICE_HH
10
#define SOT_DEVICE_HH
11
12
/* --------------------------------------------------------------------- */
13
/* --- INCLUDE --------------------------------------------------------- */
14
/* --------------------------------------------------------------------- */
15
16
#include <pinocchio/fwd.hpp>
17
18
/* -- MaaL --- */
19
#include <dynamic-graph/linear-algebra.h>
20
21
/* SOT */
22
#include <dynamic-graph/all-signals.h>
23
#include <dynamic-graph/entity.h>
24
25
#include <sot/core/matrix-geometry.hh>
26
27
#include "sot/core/api.hh"
28
#include "sot/core/periodic-call.hh"
29
30
namespace dynamicgraph {
31
namespace sot {
32
33
/// Define the type of input expected by the robot
34
enum ControlInput {
35
  CONTROL_INPUT_NO_INTEGRATION = 0,
36
  CONTROL_INPUT_ONE_INTEGRATION = 1,
37
  CONTROL_INPUT_TWO_INTEGRATION = 2,
38
  CONTROL_INPUT_SIZE = 3
39
};
40
41
const std::string ControlInput_s[] = {"noInteg", "oneInteg", "twoInteg"};
42
43
/* --------------------------------------------------------------------- */
44
/* --- CLASS ----------------------------------------------------------- */
45
/* --------------------------------------------------------------------- */
46
47
class SOT_CORE_EXPORT Device : public Entity {
48
 public:
49
  static const std::string CLASS_NAME;
50
1
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
51
52
  enum ForceSignalSource {
53
    FORCE_SIGNAL_RLEG,
54
    FORCE_SIGNAL_LLEG,
55
    FORCE_SIGNAL_RARM,
56
    FORCE_SIGNAL_LARM
57
  };
58
59
 protected:
60
  dynamicgraph::Vector state_;
61
  dynamicgraph::Vector velocity_;
62
  bool sanityCheck_;
63
  dynamicgraph::Vector vel_control_;
64
  ControlInput controlInputType_;
65
  bool withForceSignals[4];
66
  PeriodicCall periodicCallBefore_;
67
  PeriodicCall periodicCallAfter_;
68
  double timestep_;
69
70
  /// \name Robot bounds used for sanity checks
71
  /// \{
72
  Vector upperPosition_;
73
  Vector upperVelocity_;
74
  Vector upperTorque_;
75
  Vector lowerPosition_;
76
  Vector lowerVelocity_;
77
  Vector lowerTorque_;
78
  /// \}
79
 public:
80
  /* --- CONSTRUCTION --- */
81
  Device(const std::string &name);
82
  /* --- DESTRUCTION --- */
83
  virtual ~Device();
84
85
  virtual void setStateSize(const unsigned int &size);
86
  virtual void setState(const dynamicgraph::Vector &st);
87
  void setVelocitySize(const unsigned int &size);
88
  virtual void setVelocity(const dynamicgraph::Vector &vel);
89
  virtual void setSecondOrderIntegration();
90
  virtual void setNoIntegration();
91
  virtual void setControlInputType(const std::string &cit);
92
  virtual void increment(const double &dt = 5e-2);
93
94
  /// \name Sanity check parameterization
95
  /// \{
96
  void setSanityCheck(const bool &enableCheck);
97
  void setPositionBounds(const Vector &lower, const Vector &upper);
98
  void setVelocityBounds(const Vector &lower, const Vector &upper);
99
  void setTorqueBounds(const Vector &lower, const Vector &upper);
100
  /// \}
101
102
  PeriodicCall &periodicCallBefore() { return periodicCallBefore_; }
103
  PeriodicCall &periodicCallAfter() { return periodicCallAfter_; }
104
105
 public: /* --- DISPLAY --- */
106
  virtual void display(std::ostream &os) const;
107
  virtual void cmdDisplay();
108
  SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
109
                                                  const Device &r) {
110
    r.display(os);
111
    return os;
112
  }
113
114
 public: /* --- SIGNALS --- */
115
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> controlSIN;
116
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> attitudeSIN;
117
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> zmpSIN;
118
119
  /// \name Device current state.
120
  /// \{
121
  dynamicgraph::Signal<dynamicgraph::Vector, int> stateSOUT;
122
  dynamicgraph::Signal<dynamicgraph::Vector, int> velocitySOUT;
123
  dynamicgraph::Signal<MatrixRotation, int> attitudeSOUT;
124
  /*! \brief The current state of the robot from the command viewpoint. */
125
  dynamicgraph::Signal<dynamicgraph::Vector, int> motorcontrolSOUT;
126
  dynamicgraph::Signal<dynamicgraph::Vector, int> previousControlSOUT;
127
  /*! \brief The ZMP reference send by the previous controller. */
128
  dynamicgraph::Signal<dynamicgraph::Vector, int> ZMPPreviousControllerSOUT;
129
  /// \}
130
131
  /// \name Real robot current state
132
  /// This corresponds to the real encoders values and take into
133
  /// account the stabilization step. Therefore, this usually
134
  /// does *not* match the state control input signal.
135
  /// \{
136
  /// Motor positions
137
  dynamicgraph::Signal<dynamicgraph::Vector, int> robotState_;
138
  /// Motor velocities
139
  dynamicgraph::Signal<dynamicgraph::Vector, int> robotVelocity_;
140
  /// The force torque sensors
141
  dynamicgraph::Signal<dynamicgraph::Vector, int> *forcesSOUT[4];
142
  /// Motor torques
143
  /// \todo why pseudo ?
144
  dynamicgraph::Signal<dynamicgraph::Vector, int> pseudoTorqueSOUT;
145
  /// \}
146
147
 protected:
148
  /// Compute roll pitch yaw angles of freeflyer joint.
149
  void integrateRollPitchYaw(dynamicgraph::Vector &state,
150
                             const dynamicgraph::Vector &control, double dt);
151
  /// Store Position of free flyer joint
152
  MatrixHomogeneous ffPose_;
153
  /// Compute the new position, from the current control.
154
  ///
155
  /// When sanity checks are enabled, this checks that the control is within
156
  /// bounds. There are three cases, depending on what the control is:
157
  /// - position: checks that the position is within bounds,
158
  /// - velocity: checks that the velocity and the future position are
159
  ///             within bounds,
160
  /// - acceleration: checks that the acceleration, the future velocity and
161
  ///                 position are within bounds.
162
  ///                 \todo in order to check the acceleration, we need
163
  ///                 pinocchio and the contact forces in order to estimate
164
  ///                 the joint torques for the given acceleration.
165
  virtual void integrate(const double &dt);
166
167
 protected:
168
  /// Get freeflyer pose
169
  const MatrixHomogeneous &freeFlyerPose() const;
170
171
 public:
172
  virtual void setRoot(const dynamicgraph::Matrix &root);
173
174
  virtual void setRoot(const MatrixHomogeneous &worldMwaist);
175
176
 private:
177
  // Intermediate variable to avoid dynamic allocation
178
  dynamicgraph::Vector forceZero6;
179
};
180
}  // namespace sot
181
}  // namespace dynamicgraph
182
183
#endif /* #ifndef SOT_DEVICE_HH */