# GCC Code Coverage Report

Directory: ./ include/dynamic-graph/tutorial/inverted-pendulum.hh 2024-08-12 12:09:05
Exec Total Coverage
Lines: 6 9 66.7%
Branches: 0 2 0.0%

Line Branch Exec Source
1 /*
3 * Florent Lamiraux
4 *
5 * CNRS
6 *
7 */
8
9 #ifndef DG_TUTORIAL_INVERTED_PENDULUM_HH
10 #define DG_TUTORIAL_INVERTED_PENDULUM_HH
11
12 #include <dynamic-graph/entity.h>
13 #include <dynamic-graph/linear-algebra.h>
14 #include <dynamic-graph/signal-ptr.h>
15
16 namespace dynamicgraph {
17 namespace tutorial {
18
19 /**
20 \brief Inverted Pendulum on a cart
21
22 This class represents the classical inverted pendulum on a cart.
23 The equation of motion is:
24
25 \f{eqnarray*}{
26 \left ( M + m \right ) \ddot x - m l \ddot \theta \cos \theta + m l \dot
27 \theta^2 \sin \theta &=& F\\ m l (-g \sin \theta - \ddot x \cos \theta + l
28 \ddot \theta) &=& 0 \f}
29
30 where
31 \li the state is a vector of dimension 4
32 \f$(x,\theta,\dot{x},\dot{\theta})\f$ represented by signal
33 stateSOUT,
34 \li \f$x\f$ is the position of the cart on an horizontal axis,
35 \f$\theta\f$ is the angle of the pendulum with respect to the
36 vertical axis,
37 \li the input is a vector of dimension 1 \f$(F)\f$ reprensented by signal
38 forceSIN,
39 \li m, M and l are respectively the mass of the pendulum, the mass of the
40 cart and the length of the pendulum.
41
42 A more natural form of the above equation for roboticists is
43 \f[
44 \textbf{M}(\textbf{q})\ddot{\textbf{q}} +
45 \textbf{N}(\textbf{q},\dot{\textbf{q}})\dot{\textbf{q}} +
46 \textbf{G}(\textbf{q}) = \textbf{F}
47 \f]
48 where
49 \f{eqnarray*}
50 \textbf{q} &=& (x, \theta) \\
51 \textbf{M}(\textbf{q}) &=& \left( \begin{array}{cc}
52 M + m & -m\ l\ \cos\theta \\
53 -m\ l\ \cos\theta & m\ l^2 \end{array}\right) \\
54 \textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc}
55 0 & m\ l\ \dot{\theta} \sin\theta \\
56 0 & 0 \end{array}\right)\\
57 \textbf{G}(\textbf{q}) &=& \left( \begin{array}{c}
58 0 \\ -m\ l\ g\ \sin\theta \end{array}\right)\\
59 \textbf{F} &=& \left( \begin{array}{c}
60 F \\ 0 \end{array}\right)
61 \f}
62 In order to make the system intrinsically stable, we add some viscosity
63 by rewriting:
64 \f{eqnarray*}
65 \textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc}
66 \lambda & m\ l\ \dot{\theta} \sin\theta\\
67 0 & \lambda \end{array}\right)
68 \f}
69 where \f$\lambda\f$ is a positive coefficient.
70 */
71
72 class InvertedPendulum : public Entity {
73 public:
74 /**
75 \brief Constructor by name
76 */
77 InvertedPendulum(const std::string& inName);
78
79 ~InvertedPendulum();
80
81 /// Each entity should provide the name of the class it belongs to
82 virtual const std::string& getClassName(void) const { return CLASS_NAME; }
83
84 /// Header documentation of the python class
85 virtual std::string getDocString() const {
86 return "Classical inverted pendulum dynamic model\n";
87 }
88
89 /// Integrate equation of motion over time step given as input
90 void incr(double inTimeStep);
91
92 /**
93 \name Parameters
94 @{
95 */
96 /**
97 \brief Set the mass of the cart
98 */
99 1 void setCartMass(const double& inMass) { cartMass_ = inMass; }
100
101 /**
102 \brief Get the mass of the cart
103 */
104 1 double getCartMass() const { return cartMass_; }
105
106 /**
107 \brief Set the mass of the cart
108 */
109 1 void setPendulumMass(const double& inMass) { pendulumMass_ = inMass; }
110
111 /**
112 \brief Get the mass of the pendulum
113 */
114 1 double getPendulumMass() const { return pendulumMass_; }
115
116 /**
117 \brief Set the length of the cart
118 */
119 1 void setPendulumLength(const double& inLength) { pendulumLength_ = inLength; }
120
121 /**
122 \brief Get the length of the pendulum
123 */
124 1 double getPendulumLength() const { return pendulumLength_; }
125
126 /**
127 @}
128 */
129
130 public:
131 /*
132 \brief Class name
133 */
134 static const std::string CLASS_NAME;
135
136 private:
137 /**
138 \brief Input force acting on the inverted pendulum
139 */
140 SignalPtr<double, int> forceSIN;
141 /**
142 \brief State of the inverted pendulum
143 */
144 Signal< ::dynamicgraph::Vector, int> stateSOUT;
145
146 /// \brief Mass of the cart
147 double cartMass_;
148 /// \brief Mass of the pendulum
149 double pendulumMass_;
150 /// \brief Length of the pendulum
151 double pendulumLength_;
152 /// \brief Viscosity coefficient
153 double viscosity_;
154
155 /**
156 \brief Compute the evolution of the state of the pendulum
157 */
158 ::dynamicgraph::Vector computeDynamics(const ::dynamicgraph::Vector& inState,
159 const double& inControl,
160 double inTimeStep);
161 };
162 } // namespace tutorial
163 } // namespace dynamicgraph
164 #endif
165