GCC Code Coverage Report

Directory: ./
File: include/sot/core/device.hh
Date: 2024-08-13 12:13:25
Exec Total Coverage
Lines: 1 3 33.3%
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 */
184