GCC Code Coverage Report

Line	Exec	Source
1		/*
2		* Copyright 2010,
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