Commit 4729f7e5 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Format

parent 3f9b9bc6
...@@ -74,7 +74,7 @@ Setters and getters will enable us later to control parameters through commands. ...@@ -74,7 +74,7 @@ Setters and getters will enable us later to control parameters through commands.
double getPendulumLength () const { double getPendulumLength () const {
return pendulumLength_; return pendulumLength_;
} }
\endcode \endcode
The name of the class is stored as a static member The name of the class is stored as a static member
...@@ -160,14 +160,14 @@ We set input and output signal as constant with a given value ...@@ -160,14 +160,14 @@ We set input and output signal as constant with a given value
\endcode \endcode
The following lines of code define and register commands into the entity. The following lines of code define and register commands into the entity.
A \ref dynamicgraph::command::Command "command" is created by calling a constructor with A \ref dynamicgraph::command::Command "command" is created by calling a constructor with
\li a string: the name of the command, \li a string: the name of the command,
\li a pointer to a newly created command and \li a pointer to a newly created command and
\li a string documenting the command: \li a string documenting the command:
\code \code
std::string docstring; std::string docstring;
// Incr // Incr
docstring = docstring =
"\n" "\n"
...@@ -182,34 +182,35 @@ A \ref dynamicgraph::command::Command "command" is created by calling a construc ...@@ -182,34 +182,35 @@ A \ref dynamicgraph::command::Command "command" is created by calling a construc
In this example, command::Increment is a command specific to our class In this example, command::Increment is a command specific to our class
InvertedPendulum. This new command is explained in page \ref dg_tutorial_inverted_pendulum_command. InvertedPendulum. This new command is explained in page \ref dg_tutorial_inverted_pendulum_command.
Setter and getter commands are available through classes templated by the type of entity using the command and the type of the parameter. Be aware that only a prespecified set of types are supported for commands, see class dynamicgraph::command::Value. Setter and getter commands are available through classes templated by the type of entity using the command and the type
\code of the parameter. Be aware that only a prespecified set of types are supported for commands, see class
docstring = dynamicgraph::command::Value. \code docstring =
"\n" "\n"
" Set cart mass\n" " Set cart mass\n"
"\n"; "\n";
addCommand(std::string("setCartMass"), addCommand(std::string("setCartMass"),
new ::dynamicgraph::command::Setter<InvertedPendulum, double> new ::dynamicgraph::command::Setter<InvertedPendulum, double>
(*this, &InvertedPendulum::setCartMass, docstring)); (*this, &InvertedPendulum::setCartMass, docstring));
docstring = docstring =
"\n" "\n"
" Get cart mass\n" " Get cart mass\n"
"\n"; "\n";
addCommand(std::string("getCartMass"), addCommand(std::string("getCartMass"),
new ::dynamicgraph::command::Getter<InvertedPendulum, double> new ::dynamicgraph::command::Getter<InvertedPendulum, double>
(*this, &InvertedPendulum::getCartMass, docstring)); (*this, &InvertedPendulum::getCartMass, docstring));
\endcode \endcode
\note \note
It is important to notice that It is important to notice that
\li commands passed to method Entity::addCommand will be destroyed automatically by Entity class destructor. The user should therefore not destroy them, \li commands passed to method Entity::addCommand will be destroyed automatically by Entity class destructor. The user
\li commands should be defined and registered in the class constructor. Commands defined later on will not be reachable by python bindings. should therefore not destroy them, \li commands should be defined and registered in the class constructor. Commands
defined later on will not be reachable by python bindings.
\subsection dg_tutorial_inverted_pendulum_cxx_implementation_newtypes Registering new types: advanced feature \subsection dg_tutorial_inverted_pendulum_cxx_implementation_newtypes Registering new types: advanced feature
Signals are templated by the type of data they convey. In this example, we hae defined our own class of vectors InvertedPendulum::Vector. In order to be able to create signals with this type, we need to register the new type: Signals are templated by the type of data they convey. In this example, we hae defined our own class of vectors
\code InvertedPendulum::Vector. In order to be able to create signals with this type, we need to register the new type: \code
dynamicgraph::DefaultCastRegisterer<InvertedPendulum::Vector> IPVectorCast; dynamicgraph::DefaultCastRegisterer<InvertedPendulum::Vector> IPVectorCast;
\endcode \endcode
......
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
Command (dynamicgraph::command::Command) are objects that encapsulate an Command (dynamicgraph::command::Command) are objects that encapsulate an
action to be performed on an entity. action to be performed on an entity.
In this page, we define a new command that will call method In this page, we define a new command that will call method
InvertedPendulum::incr. The source code is in InvertedPendulum::incr. The source code is in
<c>src/command-increment.hh</c>. <c>src/command-increment.hh</c>.
...@@ -25,12 +25,11 @@ The constructor takes ...@@ -25,12 +25,11 @@ The constructor takes
\li a reference to a InvertedPendulum and calls parent class, \li a reference to a InvertedPendulum and calls parent class,
\li a vector a types specifying the number and types of input arguments of the command and \li a vector a types specifying the number and types of input arguments of the command and
\li a string documenting the command. \li a string documenting the command.
In this case, there is only one argument of type <c>double</c>. Note the use of <c>boost::assign::list_of</c> to build a vector in one command line: In this case, there is only one argument of type <c>double</c>. Note the use of <c>boost::assign::list_of</c> to build
\code a vector in one command line: \code Increment(InvertedPendulum& entity, const std::string& docstring) : Command(entity,
Increment(InvertedPendulum& entity, const std::string& docstring) : boost::assign::list_of(Value::DOUBLE), docstring)
Command(entity, boost::assign::list_of(Value::DOUBLE), docstring) {
{ }
}
\endcode \endcode
We then define the action of the command in virtual method <c>doExecute</c>. We then define the action of the command in virtual method <c>doExecute</c>.
...@@ -38,7 +37,7 @@ We need to get a reference to the object on which the command will act. Note ...@@ -38,7 +37,7 @@ We need to get a reference to the object on which the command will act. Note
that we can straightfowardly statically cast in <c>InvertedPendulum</c> the that we can straightfowardly statically cast in <c>InvertedPendulum</c> the
Entity object returned by method <c>owner</c>: Entity object returned by method <c>owner</c>:
\code \code
virtual Value doExecute() virtual Value doExecute()
{ {
Entity& entity = owner(); Entity& entity = owner();
InvertedPendulum& ip = static_cast<InvertedPendulum&>(entity); InvertedPendulum& ip = static_cast<InvertedPendulum&>(entity);
......
...@@ -3,15 +3,15 @@ ...@@ -3,15 +3,15 @@
\section dg_tutorial_inverted_pendulum_python_intro Introduction \section dg_tutorial_inverted_pendulum_python_intro Introduction
Generating python bindings for new Entity classes is straightforward. We only need to add the following lines into file <c>src/CMakeLists.txt</c>: Generating python bindings for new Entity classes is straightforward. We only need to add the following lines into file
<c>src/CMakeLists.txt</c>:
<code> <code>
DYNAMIC_GRAPH_PYTHON_MODULE("tutorial" ${LIBRARY_NAME} wrap) DYNAMIC_GRAPH_PYTHON_MODULE("tutorial" ${LIBRARY_NAME} wrap)
</code> </code>
This will create and install a python module called <c>dynamic_graph.tutorial</c>, linked with library <c>${LIBRARY_NAME} (libdynamic-graph-tutorial.so)</c>. This will create and install a python module called <c>dynamic_graph.tutorial</c>, linked with library
When importing this module, two new python classes deriving from <c>Entity</c> are created: <c>${LIBRARY_NAME} (libdynamic-graph-tutorial.so)</c>. When importing this module, two new python classes deriving from
\li InvertedPendulum, and <c>Entity</c> are created: \li InvertedPendulum, and \li FeedbackController. \sa <c>src/simu.py</c> for an example
\li FeedbackController. python script.
\sa <c>src/simu.py</c> for an example python script.
*/ */
...@@ -3,9 +3,10 @@ ...@@ -3,9 +3,10 @@
\section intro_dg_tutorial Introduction \section intro_dg_tutorial Introduction
This tutorial implements a simple application of the dynamic-graph package with two entities represented by the following classes: This tutorial implements a simple application of the dynamic-graph package with two entities represented by the
\li dynamicgraph::tutorial::InvertedPendulum: that implements the dynamics of a pendulum on a cart and following classes: \li dynamicgraph::tutorial::InvertedPendulum: that implements the dynamics of a pendulum on a cart
\li dynamicgraph::tutorial::FeedbackController: that implements a closed-loop control loop that stabilizes the pendulum. and \li dynamicgraph::tutorial::FeedbackController: that implements a closed-loop control loop that stabilizes the
pendulum.
\section dg_tutorial_inverted_pendulum_ Prerequisite \section dg_tutorial_inverted_pendulum_ Prerequisite
...@@ -17,8 +18,8 @@ This tutorial requires prior installation of packages: ...@@ -17,8 +18,8 @@ This tutorial requires prior installation of packages:
This tutorial show: This tutorial show:
\li how to \ref dg_tutorial_inverted_pendulum_cxx "create a new entity" with signals and commands, \li how to \ref dg_tutorial_inverted_pendulum_cxx "create a new entity" with signals and commands,
\li how to \ref dg_tutorial_inverted_pendulum_python "build a python module" to create and control this entity in a python \li how to \ref dg_tutorial_inverted_pendulum_python "build a python module" to create and control this entity in a
interpreter. python interpreter.
*/ */
...@@ -13,80 +13,72 @@ ...@@ -13,80 +13,72 @@
#include <dynamic-graph/linear-algebra.h> #include <dynamic-graph/linear-algebra.h>
namespace dynamicgraph { namespace dynamicgraph {
namespace tutorial { namespace tutorial {
/** /**
\brief Feedback controller for an inverted pendulum \brief Feedback controller for an inverted pendulum
This class implements a feedback control for the inverted pendulum This class implements a feedback control for the inverted pendulum
represented by class InvertedPendulum represented by class InvertedPendulum
*/ */
class FeedbackController : public Entity class FeedbackController : public Entity {
{ public:
public: /**
/** \brief Constructor by name
\brief Constructor by name */
*/ FeedbackController(const std::string& inName);
FeedbackController(const std::string& inName);
~FeedbackController(); ~FeedbackController();
/// Each entity should provide the name of the class it belongs to /// Each entity should provide the name of the class it belongs to
virtual const std::string& getClassName (void) const { virtual const std::string& getClassName(void) const { return CLASS_NAME; }
return CLASS_NAME;
}
/// Header documentation of the python class /// Header documentation of the python class
virtual std::string getDocString () const { virtual std::string getDocString() const {
return return "Feedback controller aimed at maintaining the pendulum vertical\n";
"Feedback controller aimed at maintaining the pendulum vertical\n"; }
} /**
/** \name Parameters
\name Parameters @{
@{ */
*/ /**
/** \brief Get feedback gain
\brief Get feedback gain */
*/ void setGain(const ::dynamicgraph::Matrix& inGain) { gain_ = inGain; }
void setGain (const ::dynamicgraph::Matrix& inGain) {
gain_ = inGain;
}
/** /**
\brief Get feedback gain \brief Get feedback gain
*/ */
::dynamicgraph::Matrix getGain () const { ::dynamicgraph::Matrix getGain() const { return gain_; }
return gain_;
}
/** /**
@} @}
*/ */
protected: protected:
/* /*
\brief Class name \brief Class name
*/ */
static const std::string CLASS_NAME; static const std::string CLASS_NAME;
private: private:
/** /**
Compute the control law Compute the control law
*/ */
double& computeForceFeedback(double& force, const int& inTime); double& computeForceFeedback(double& force, const int& inTime);
/** /**
\brief State of the inverted pendulum \brief State of the inverted pendulum
*/ */
SignalPtr < ::dynamicgraph::Vector, int> stateSIN; SignalPtr< ::dynamicgraph::Vector, int> stateSIN;
/** /**
\brief Force computed by the control law \brief Force computed by the control law
*/ */
SignalTimeDependent < double, int > forceSOUT; SignalTimeDependent<double, int> forceSOUT;
/// \brief Gain of the controller /// \brief Gain of the controller
::dynamicgraph::Matrix gain_; ::dynamicgraph::Matrix gain_;
}; };
} // namespace tutorial } // namespace tutorial
} // namespace dynamicgraph } // namespace dynamicgraph
#endif //DG_TUTORIAL_FEEDBACK_CONTROLLER_HH #endif // DG_TUTORIAL_FEEDBACK_CONTROLLER_HH
...@@ -14,167 +14,148 @@ ...@@ -14,167 +14,148 @@
#include <dynamic-graph/linear-algebra.h> #include <dynamic-graph/linear-algebra.h>
namespace dynamicgraph { namespace dynamicgraph {
namespace tutorial { namespace tutorial {
/** /**
\brief Inverted Pendulum on a cart \brief Inverted Pendulum on a cart
This class represents the classical inverted pendulum on a cart. This class represents the classical inverted pendulum on a cart.
The equation of motion is: The equation of motion is:
\f{eqnarray*}{ \f{eqnarray*}{
\left ( M + m \right ) \ddot x - m l \ddot \theta \cos \theta + m l \dot \theta^2 \sin \theta &=& F\\ \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 m l (-g \sin \theta - \ddot x \cos \theta + l \ddot \theta) &=& 0
\f} \f}
where where
\li the state is a vector of dimension 4 \li the state is a vector of dimension 4
\f$(x,\theta,\dot{x},\dot{\theta})\f$ represented by signal \f$(x,\theta,\dot{x},\dot{\theta})\f$ represented by signal
stateSOUT, stateSOUT,
\li \f$x\f$ is the position of the cart on an horizontal axis, \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 \f$\theta\f$ is the angle of the pendulum with respect to the
vertical axis, vertical axis,
\li the input is a vector of dimension 1 \f$(F)\f$ reprensented by signal \li the input is a vector of dimension 1 \f$(F)\f$ reprensented by signal
forceSIN, forceSIN,
\li m, M and l are respectively the mass of the pendulum, the mass of the \li m, M and l are respectively the mass of the pendulum, the mass of the
cart and the length of the pendulum. cart and the length of the pendulum.
A more natural form of the above equation for roboticists is A more natural form of the above equation for roboticists is
\f[ \f[
\textbf{M}(\textbf{q})\ddot{\textbf{q}} + \textbf{M}(\textbf{q})\ddot{\textbf{q}} +
\textbf{N}(\textbf{q},\dot{\textbf{q}})\dot{\textbf{q}} + \textbf{N}(\textbf{q},\dot{\textbf{q}})\dot{\textbf{q}} +
\textbf{G}(\textbf{q}) = \textbf{F} \textbf{G}(\textbf{q}) = \textbf{F}
\f] \f]
where where
\f{eqnarray*} \f{eqnarray*}
\textbf{q} &=& (x, \theta) \\ \textbf{q} &=& (x, \theta) \\
\textbf{M}(\textbf{q}) &=& \left( \begin{array}{cc} \textbf{M}(\textbf{q}) &=& \left( \begin{array}{cc}
M + m & -m\ l\ \cos\theta \\ M + m & -m\ l\ \cos\theta \\
-m\ l\ \cos\theta & m\ l^2 \end{array}\right) \\ -m\ l\ \cos\theta & m\ l^2 \end{array}\right) \\
\textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc} \textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc}
0 & m\ l\ \dot{\theta} \sin\theta \\ 0 & m\ l\ \dot{\theta} \sin\theta \\
0 & 0 \end{array}\right)\\ 0 & 0 \end{array}\right)\\
\textbf{G}(\textbf{q}) &=& \left( \begin{array}{c} \textbf{G}(\textbf{q}) &=& \left( \begin{array}{c}
0 \\ -m\ l\ g\ \sin\theta \end{array}\right)\\ 0 \\ -m\ l\ g\ \sin\theta \end{array}\right)\\
\textbf{F} &=& \left( \begin{array}{c} \textbf{F} &=& \left( \begin{array}{c}
F \\ 0 \end{array}\right) F \\ 0 \end{array}\right)
\f} \f}
In order to make the system intrinsically stable, we add some viscosity In order to make the system intrinsically stable, we add some viscosity
by rewriting: by rewriting:
\f{eqnarray*} \f{eqnarray*}
\textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc} \textbf{N}(\textbf{q},\dot{\textbf{q}}) &=& \left( \begin{array}{cc}
\lambda & m\ l\ \dot{\theta} \sin\theta\\ \lambda & m\ l\ \dot{\theta} \sin\theta\\
0 & \lambda \end{array}\right) 0 & \lambda \end{array}\right)
\f} \f}
where \f$\lambda\f$ is a positive coefficient. where \f$\lambda\f$ is a positive coefficient.
*/ */
class InvertedPendulum : public Entity class InvertedPendulum : public Entity {
{ public:
public: /**
/** \brief Constructor by name
\brief Constructor by name */
*/ InvertedPendulum(const std::string& inName);
InvertedPendulum(const std::string& inName);
~InvertedPendulum();
~InvertedPendulum();
/// Each entity should provide the name of the class it belongs to
/// Each entity should provide the name of the class it belongs to virtual const std::string& getClassName(void) const { return CLASS_NAME; }
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"; }
/// Header documentation of the python class /// Integrate equation of motion over time step given as input
virtual std::string getDocString () const { void incr(double inTimeStep);
return
"Classical inverted pendulum dynamic model\n"; /**
} \name Parameters
@{
/// Integrate equation of motion over time step given as input */
void incr(double inTimeStep); /**
\brief Set the mass of the cart
/** */
\name Parameters void setCartMass(const double& inMass) { cartMass_ = inMass; }
@{
*/ /**
/** \brief Get the mass of the cart
\brief Set the mass of the cart */
*/ double getCartMass() const { return cartMass_; }
void setCartMass (const double& inMass) {
cartMass_ = inMass; /**
} \brief Set the mass of the cart
*/
/** void setPendulumMass(const double& inMass) { pendulumMass_ = inMass; }
\brief Get the mass of the cart
*/ /**
double getCartMass () const { \brief Get the mass of the pendulum
return cartMass_; */
} double getPendulumMass() const { return pendulumMass_; }
/** /**
\brief Set the mass of the cart \brief Set the length of the cart
*/ */
void setPendulumMass (const double& inMass) { void setPendulumLength(const double& inLength) { pendulumLength_ = inLength; }
pendulumMass_ = inMass;
} /**
\brief Get the length of the pendulum
/** */
\brief Get the mass of the pendulum double getPendulumLength() const { return pendulumLength_; }
*/
double getPendulumMass () const { /**
return pendulumMass_; @}
} */
/** protected:
\brief Set the length of the cart /*
*/ \brief Class name
void setPendulumLength (const double& inLength) { */
pendulumLength_ = inLength; static const std::string CLASS_NAME;
}
private:
/** /**
\brief Get the length of the pendulum \brief Input force acting on the inverted pendulum
*/ */
double getPendulumLength () const { SignalPtr<double, int> forceSIN;
return pendulumLength_; /**
} \brief State of the inverted pendulum
*/
/** Signal< ::dynamicgraph::Vector, int> stateSOUT;
@}
*/ /// \brief Mass of the cart
double cartMass_;
protected: /// \brief Mass of the pendulum
/* double pendulumMass_;
\brief Class name /// \brief Length of the pendulum
*/ double pendulumLength_;
static const std::string CLASS_NAME; /// \brief Viscosity coefficient
double viscosity_;
private:
/** /**
\brief Input force acting on the inverted pendulum \brief Compute the evolution of the state of the pendulum
*/ */
SignalPtr< double, int > forceSIN; ::dynamicgraph::Vector computeDynamics(const ::dynamicgraph::Vector& inState, const double& inControl,
/**