Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/dynamic-graph/tutorial/inverted-pendulum.hh	Lines:	6	9	66.7 %
Date:	2024-04-12 12:04:38	Branches:	0	2	0.0 %


/*
 * Copyright 2010,
 * Florent Lamiraux
 *
 * CNRS
 *
 */

#ifndef DG_TUTORIAL_INVERTED_PENDULUM_HH
#define DG_TUTORIAL_INVERTED_PENDULUM_HH

#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-ptr.h>

namespace dynamicgraph {
namespace tutorial {

/**
   \brief Inverted Pendulum on a cart

   This class represents the classical inverted pendulum on a cart.
   The equation of motion is:

   \f{eqnarray*}{
   \left ( M + m \right ) \ddot x - m l \ddot \theta \cos \theta + m l \dot
   \theta^2 \sin \theta &=& F\\ m l (-g \sin \theta - \ddot x \cos \theta + l
   \ddot \theta) &=& 0 \f}

   where
   \li the state is a vector of dimension 4
       \f$(x,\theta,\dot{x},\dot{\theta})\f$ represented by signal
       stateSOUT,
   \li \f$x\f$ is the position of the cart on an horizontal axis,
       \f$\theta\f$ is the angle of the pendulum with respect to the
        vertical axis,
   \li the input is a vector of dimension 1 \f$(F)\f$ reprensented by signal
       forceSIN,
   \li m, M and l are respectively the mass of the pendulum, the mass of the
       cart and the length of the pendulum.

   A more natural form of the above equation for roboticists is
   \f[
   \textbf{M}(\textbf{q})\ddot{\textbf{q}} +
   \textbf{N}(\textbf{q},\dot{\textbf{q}})\dot{\textbf{q}} +
   \textbf{G}(\textbf{q}) = \textbf{F}
   \f]
   where
   \f{eqnarray*}
   \textbf{q} &=& (x, \theta) \\
   \textbf{M}(\textbf{q}) &=& \left( \begin{array}{cc}
   M + m & -m\ l\ \cos\theta \\
   -m\ l\ \cos\theta & m\ l^2 \end{array}\right) \\
   \textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc}
   0 & m\ l\ \dot{\theta} \sin\theta \\
   0 & 0 \end{array}\right)\\
   \textbf{G}(\textbf{q}) &=& \left( \begin{array}{c}
   0 \\ -m\ l\ g\ \sin\theta \end{array}\right)\\
   \textbf{F} &=& \left( \begin{array}{c}
   F \\ 0 \end{array}\right)
   \f}
   In order to make the system intrinsically stable, we add some viscosity
   by rewriting:
   \f{eqnarray*}
   \textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc}
   \lambda & m\ l\ \dot{\theta} \sin\theta\\
   0 & \lambda \end{array}\right)
   \f}
   where \f$\lambda\f$ is a positive coefficient.
*/

class InvertedPendulum : public Entity {
 public:
  /**
     \brief Constructor by name
  */
  InvertedPendulum(const std::string& inName);

  ~InvertedPendulum();

  /// Each entity should provide the name of the class it belongs to
  virtual const std::string& getClassName(void) const { return CLASS_NAME; }

  /// Header documentation of the python class
  virtual std::string getDocString() const {
    return "Classical inverted pendulum dynamic model\n";
  }

  /// Integrate equation of motion over time step given as input
  void incr(double inTimeStep);

  /**
      \name Parameters
      @{
  */
  /**
     \brief Set the mass of the cart
  */
  void setCartMass(const double& inMass) { cartMass_ = inMass; }

  /**
     \brief Get the mass of the cart
  */
  double getCartMass() const { return cartMass_; }

  /**
     \brief Set the mass of the cart
  */
  void setPendulumMass(const double& inMass) { pendulumMass_ = inMass; }

  /**
     \brief Get the mass of the pendulum
  */
  double getPendulumMass() const { return pendulumMass_; }

  /**
     \brief Set the length of the cart
  */
  void setPendulumLength(const double& inLength) { pendulumLength_ = inLength; }

  /**
     \brief Get the length of the pendulum
  */
  double getPendulumLength() const { return pendulumLength_; }

  /**
     @}
  */

 public:
  /*
    \brief Class name
  */
  static const std::string CLASS_NAME;

 private:
  /**
     \brief Input force acting on the inverted pendulum
  */
  SignalPtr<double, int> forceSIN;
  /**
     \brief State of the inverted pendulum
  */
  Signal< ::dynamicgraph::Vector, int> stateSOUT;

  /// \brief Mass of the cart
  double cartMass_;
  /// \brief Mass of the pendulum
  double pendulumMass_;
  /// \brief Length of the pendulum
  double pendulumLength_;
  /// \brief Viscosity coefficient
  double viscosity_;

  /**
     \brief Compute the evolution of the state of the pendulum
  */
  ::dynamicgraph::Vector computeDynamics(const ::dynamicgraph::Vector& inState,
                                         const double& inControl,
                                         double inTimeStep);
};
}  // namespace tutorial
}  // namespace dynamicgraph
#endif


Generated by: GCOVR (Version 4.2)

Line	Branch	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