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

Format

parent 3f9b9bc6
......@@ -182,9 +182,9 @@ A \ref dynamicgraph::command::Command "command" is created by calling a construc
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.
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.
\code
docstring =
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. \code docstring =
"\n"
" Set cart mass\n"
"\n";
......@@ -203,13 +203,14 @@ Setter and getter commands are available through classes templated by the type o
\note
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 should be defined and registered in the class constructor. Commands defined later on will not be reachable by python bindings.
\li commands passed to method Entity::addCommand will be destroyed automatically by Entity class destructor. The user
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
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:
\code
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: \code
dynamicgraph::DefaultCastRegisterer<InvertedPendulum::Vector> IPVectorCast;
\endcode
......
......@@ -25,10 +25,9 @@ The constructor takes
\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 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:
\code
Increment(InvertedPendulum& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::DOUBLE), docstring)
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: \code Increment(InvertedPendulum& entity, const std::string& docstring) : Command(entity,
boost::assign::list_of(Value::DOUBLE), docstring)
{
}
\endcode
......
......@@ -3,15 +3,15 @@
\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>
DYNAMIC_GRAPH_PYTHON_MODULE("tutorial" ${LIBRARY_NAME} wrap)
</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>.
When importing this module, two new python classes deriving from <c>Entity</c> are created:
\li InvertedPendulum, and
\li FeedbackController.
\sa <c>src/simu.py</c> for an example python script.
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>. When importing this module, two new python classes deriving from
<c>Entity</c> are created: \li InvertedPendulum, and \li FeedbackController. \sa <c>src/simu.py</c> for an example
python script.
*/
......@@ -3,9 +3,10 @@
\section intro_dg_tutorial Introduction
This tutorial implements a simple application of the dynamic-graph package with two entities represented by the following classes:
\li dynamicgraph::tutorial::InvertedPendulum: that implements the dynamics of a pendulum on a cart and
\li dynamicgraph::tutorial::FeedbackController: that implements a closed-loop control loop that stabilizes the pendulum.
This tutorial implements a simple application of the dynamic-graph package with two entities represented by the
following classes: \li dynamicgraph::tutorial::InvertedPendulum: that implements the dynamics of a pendulum on a cart
and \li dynamicgraph::tutorial::FeedbackController: that implements a closed-loop control loop that stabilizes the
pendulum.
\section dg_tutorial_inverted_pendulum_ Prerequisite
......@@ -17,8 +18,8 @@ This tutorial requires prior installation of packages:
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_python "build a python module" to create and control this entity in a python
interpreter.
\li how to \ref dg_tutorial_inverted_pendulum_python "build a python module" to create and control this entity in a
python interpreter.
*/
......@@ -13,15 +13,14 @@
#include <dynamic-graph/linear-algebra.h>
namespace dynamicgraph {
namespace tutorial {
/**
namespace tutorial {
/**
\brief Feedback controller for an inverted pendulum
This class implements a feedback control for the inverted pendulum
represented by class InvertedPendulum
*/
class FeedbackController : public Entity
{
*/
class FeedbackController : public Entity {
public:
/**
\brief Constructor by name
......@@ -31,14 +30,11 @@ namespace dynamicgraph {
~FeedbackController();
/// 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
"Feedback controller aimed at maintaining the pendulum vertical\n";
virtual std::string getDocString() const {
return "Feedback controller aimed at maintaining the pendulum vertical\n";
}
/**
\name Parameters
......@@ -47,16 +43,12 @@ namespace dynamicgraph {
/**
\brief Get feedback gain
*/
void setGain (const ::dynamicgraph::Matrix& inGain) {
gain_ = inGain;
}
void setGain(const ::dynamicgraph::Matrix& inGain) { gain_ = inGain; }
/**
\brief Get feedback gain
*/
::dynamicgraph::Matrix getGain () const {
return gain_;
}
::dynamicgraph::Matrix getGain() const { return gain_; }
/**
@}
......@@ -77,16 +69,16 @@ namespace dynamicgraph {
/**
\brief State of the inverted pendulum
*/
SignalPtr < ::dynamicgraph::Vector, int> stateSIN;
SignalPtr< ::dynamicgraph::Vector, int> stateSIN;
/**
\brief Force computed by the control law
*/
SignalTimeDependent < double, int > forceSOUT;
SignalTimeDependent<double, int> forceSOUT;
/// \brief Gain of the controller
::dynamicgraph::Matrix gain_;
};
} // namespace tutorial
};
} // namespace tutorial
} // namespace dynamicgraph
#endif //DG_TUTORIAL_FEEDBACK_CONTROLLER_HH
#endif // DG_TUTORIAL_FEEDBACK_CONTROLLER_HH
......@@ -14,9 +14,9 @@
#include <dynamic-graph/linear-algebra.h>
namespace dynamicgraph {
namespace tutorial {
namespace tutorial {
/**
/**
\brief Inverted Pendulum on a cart
This class represents the classical inverted pendulum on a cart.
......@@ -67,10 +67,9 @@ namespace dynamicgraph {
0 & \lambda \end{array}\right)
\f}
where \f$\lambda\f$ is a positive coefficient.
*/
*/
class InvertedPendulum : public Entity
{
class InvertedPendulum : public Entity {
public:
/**
\brief Constructor by name
......@@ -80,15 +79,10 @@ namespace dynamicgraph {
~InvertedPendulum();
/// 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";
}
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);
......@@ -100,44 +94,32 @@ namespace dynamicgraph {
/**
\brief Set the mass of the cart
*/
void setCartMass (const double& inMass) {
cartMass_ = inMass;
}
void setCartMass(const double& inMass) { cartMass_ = inMass; }
/**
\brief Get the mass of the cart
*/
double getCartMass () const {
return cartMass_;
}
double getCartMass() const { return cartMass_; }
/**
\brief Set the mass of the cart
*/
void setPendulumMass (const double& inMass) {
pendulumMass_ = inMass;
}
void setPendulumMass(const double& inMass) { pendulumMass_ = inMass; }
/**
\brief Get the mass of the pendulum
*/
double getPendulumMass () const {
return pendulumMass_;
}
double getPendulumMass() const { return pendulumMass_; }
/**
\brief Set the length of the cart
*/
void setPendulumLength (const double& inLength) {
pendulumLength_ = inLength;
}
void setPendulumLength(const double& inLength) { pendulumLength_ = inLength; }
/**
\brief Get the length of the pendulum
*/
double getPendulumLength () const {
return pendulumLength_;
}
double getPendulumLength() const { return pendulumLength_; }
/**
@}
......@@ -153,7 +135,7 @@ namespace dynamicgraph {
/**
\brief Input force acting on the inverted pendulum
*/
SignalPtr< double, int > forceSIN;
SignalPtr<double, int> forceSIN;
/**
\brief State of the inverted pendulum
*/
......@@ -171,10 +153,9 @@ namespace dynamicgraph {
/**
\brief Compute the evolution of the state of the pendulum
*/
::dynamicgraph::Vector
computeDynamics(const ::dynamicgraph::Vector& inState,
const double& inControl, double inTimeStep);
};
} // namespace tutorial
::dynamicgraph::Vector computeDynamics(const ::dynamicgraph::Vector& inState, const double& inControl,
double inTimeStep);
};
} // namespace tutorial
} // namespace dynamicgraph
#endif
......@@ -10,23 +10,19 @@
#include <boost/assign/list_of.hpp>
namespace dynamicgraph {
namespace tutorial {
namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
class Increment : public Command
{
namespace tutorial {
namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
class Increment : public Command {
public:
virtual ~Increment() {}
/// Create a command and store it in Entity
/// \param entity Instance of Entity owning of the command
/// \param docstring documentation of the command
Increment(InvertedPendulum& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::DOUBLE), docstring)
{
}
virtual Value doExecute()
{
Increment(InvertedPendulum& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::DOUBLE), docstring) {}
virtual Value doExecute() {
Entity& entity = owner();
InvertedPendulum& ip = static_cast<InvertedPendulum&>(entity);
std::vector<Value> values = getParameterValues();
......@@ -34,8 +30,8 @@ namespace dynamicgraph {
ip.incr(timeStep);
return Value();
}
}; //class Increment
} // namespace command
} // namespace tutorial
}; // class Increment
} // namespace command
} // namespace tutorial
} // namespace dynamicgraph
#endif //DYNAMIC_GRAPH_TUTORIAL_COMMAND_INCREMENT_HH
#endif // DYNAMIC_GRAPH_TUTORIAL_COMMAND_INCREMENT_HH
......@@ -8,13 +8,12 @@
#define DG_TUTORIAL_CONSTANT_HH
namespace dynamicgraph {
namespace tutorial {
class Constant
{
namespace tutorial {
class Constant {
public:
static const double gravity;
};
} // namespace tutorial
};
} // namespace tutorial
} // namespace dynamicgraph
#endif // DG_TUTORIAL_CONSTANT_HH
......@@ -18,16 +18,14 @@ using namespace dynamicgraph::tutorial;
// that will be created when importing the python module.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeedbackController, "FeedbackController");
FeedbackController::FeedbackController(const std::string& inName) :
Entity(inName),
stateSIN(NULL, "FeedbackController("+inName+")::input(vector)::state"),
forceSOUT(stateSIN,
"FeedbackController("+inName+")::output(double)::force"),
gain_(Matrix(4,1))
{
FeedbackController::FeedbackController(const std::string& inName)
: Entity(inName),
stateSIN(NULL, "FeedbackController(" + inName + ")::input(vector)::state"),
forceSOUT(stateSIN, "FeedbackController(" + inName + ")::output(double)::force"),
gain_(Matrix(4, 1)) {
// Register signals into the entity.
signalRegistration (stateSIN);
signalRegistration (forceSOUT);
signalRegistration(stateSIN);
signalRegistration(forceSOUT);
// Set signals as constant to size them
double force = 0.;
......@@ -37,12 +35,10 @@ FeedbackController::FeedbackController(const std::string& inName) :
stateSIN.setConstant(state);
// Define refresh function for output signal
boost::function2<double&, double&,const int&> ftest
= boost::bind(&FeedbackController::computeForceFeedback,
this, _1, _2);
boost::function2<double&, double&, const int&> ftest =
boost::bind(&FeedbackController::computeForceFeedback, this, _1, _2);
forceSOUT.setFunction(boost::bind(&FeedbackController::computeForceFeedback,
this, _1, _2));
forceSOUT.setFunction(boost::bind(&FeedbackController::computeForceFeedback, this, _1, _2));
std::string docstring;
// setGain
docstring =
......@@ -50,9 +46,8 @@ FeedbackController::FeedbackController(const std::string& inName) :
" Set gain of controller\n"
" takes a tuple of 4 floating point numbers as input\n"
"\n";
addCommand(std::string("setGain"),
new ::dynamicgraph::command::Setter<FeedbackController, Matrix>
(*this, &FeedbackController::setGain, docstring));
addCommand(std::string("setGain"), new ::dynamicgraph::command::Setter<FeedbackController, Matrix>(
*this, &FeedbackController::setGain, docstring));
// getGain
docstring =
......@@ -60,26 +55,19 @@ FeedbackController::FeedbackController(const std::string& inName) :
" Get gain of controller\n"
" return a tuple of 4 floating point numbers\n"
"\n";
addCommand(std::string("getGain"),
new ::dynamicgraph::command::Getter<FeedbackController, Matrix>
(*this, &FeedbackController::getGain, docstring));
addCommand(std::string("getGain"), new ::dynamicgraph::command::Getter<FeedbackController, Matrix>(
*this, &FeedbackController::getGain, docstring));
}
FeedbackController::~FeedbackController()
{
}
FeedbackController::~FeedbackController() {}
double& FeedbackController::computeForceFeedback(double& force,
const int& inTime)
{
double& FeedbackController::computeForceFeedback(double& force, const int& inTime) {
const Vector& state = stateSIN(inTime);
if (state.size() != 4)
throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC,
"state signal size is ",
"%d, should be 4.",
state.size());
Vector v (-gain_ * state);
throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC, "state signal size is ",
"%d, should be 4.", state.size());
Vector v(-gain_ * state);
force = v(0);
return force;
}
......@@ -24,15 +24,17 @@ const double Constant::gravity = 9.81;
// that will be created when importing the python module.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(InvertedPendulum, "InvertedPendulum");
InvertedPendulum::InvertedPendulum(const std::string& inName) :
Entity(inName),
forceSIN(NULL, "InvertedPendulum("+inName+")::input(double)::force"),
stateSOUT("InvertedPendulum("+inName+")::output(vector)::state"),
cartMass_(1.0), pendulumMass_(1.0), pendulumLength_(1.0), viscosity_(0.1)
{
InvertedPendulum::InvertedPendulum(const std::string& inName)
: Entity(inName),
forceSIN(NULL, "InvertedPendulum(" + inName + ")::input(double)::force"),
stateSOUT("InvertedPendulum(" + inName + ")::output(vector)::state"),
cartMass_(1.0),
pendulumMass_(1.0),
pendulumLength_(1.0),
viscosity_(0.1) {
// Register signals into the entity.
signalRegistration (forceSIN);
signalRegistration (stateSOUT);
signalRegistration(forceSIN);
signalRegistration(stateSOUT);
// Set signals as constant to size them
Vector state(4);
......@@ -51,80 +53,66 @@ InvertedPendulum::InvertedPendulum(const std::string& inName) :
"\n"
" take one floating point number as input\n"
"\n";
addCommand(std::string("incr"),
new command::Increment(*this, docstring));
addCommand(std::string("incr"), new command::Increment(*this, docstring));
// setCartMass
docstring =
"\n"
" Set cart mass\n"
"\n";
addCommand(std::string("setCartMass"),
new ::dynamicgraph::command::Setter<InvertedPendulum, double>
(*this, &InvertedPendulum::setCartMass, docstring));
addCommand(std::string("setCartMass"), new ::dynamicgraph::command::Setter<InvertedPendulum, double>(
*this, &InvertedPendulum::setCartMass, docstring));
// getCartMass
docstring =
"\n"
" Get cart mass\n"
"\n";
addCommand(std::string("getCartMass"),
new ::dynamicgraph::command::Getter<InvertedPendulum, double>
(*this, &InvertedPendulum::getCartMass, docstring));
addCommand(std::string("getCartMass"), new ::dynamicgraph::command::Getter<InvertedPendulum, double>(
*this, &InvertedPendulum::getCartMass, docstring));
// setPendulumMass
docstring =
"\n"
" Set pendulum mass\n"
"\n";
addCommand(std::string("setPendulumMass"),
new ::dynamicgraph::command::Setter<InvertedPendulum, double>
(*this, &InvertedPendulum::setPendulumMass, docstring));
addCommand(std::string("setPendulumMass"), new ::dynamicgraph::command::Setter<InvertedPendulum, double>(
*this, &InvertedPendulum::setPendulumMass, docstring));
// getPendulumMass
docstring =
"\n"
" Get pendulum mass\n"
"\n";
addCommand(std::string("getPendulumMass"),
new ::dynamicgraph::command::Getter<InvertedPendulum, double>
(*this, &InvertedPendulum::getPendulumMass, docstring));
addCommand(std::string("getPendulumMass"), new ::dynamicgraph::command::Getter<InvertedPendulum, double>(
*this, &InvertedPendulum::getPendulumMass, docstring));
// setPendulumLength
docstring =
"\n"
" Set pendulum length\n"
"\n";
addCommand(std::string("setPendulumLength"),
new ::dynamicgraph::command::Setter<InvertedPendulum, double>
(*this, &InvertedPendulum::setPendulumLength, docstring));
addCommand(std::string("setPendulumLength"), new ::dynamicgraph::command::Setter<InvertedPendulum, double>(
*this, &InvertedPendulum::setPendulumLength, docstring));
// getPendulumLength
docstring =
"\n"
" Get pendulum length\n"
"\n";
addCommand(std::string("getPendulumLength"),
new ::dynamicgraph::command::Getter<InvertedPendulum, double>
(*this, &InvertedPendulum::getPendulumLength, docstring));
addCommand(std::string("getPendulumLength"), new ::dynamicgraph::command::Getter<InvertedPendulum, double>(
*this, &InvertedPendulum::getPendulumLength, docstring));
}
InvertedPendulum::~InvertedPendulum()
{
}
InvertedPendulum::~InvertedPendulum() {}
Vector InvertedPendulum::computeDynamics(const Vector& inState,
const double& inControl,
double inTimeStep)
{
Vector InvertedPendulum::computeDynamics(const Vector& inState, const double& inControl, double inTimeStep) {
if (inState.size() != 4)
throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC,
"state signal size is ",
"%d, should be 4.",
inState.size());
throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC, "state signal size is ",
"%d, should be 4.", inState.size());
double dt = inTimeStep;
double dt2 = dt*dt;
double dt2 = dt * dt;
double g = Constant::gravity;
double x = inState(0);
double th = inState(1);
......@@ -135,34 +123,33 @@ Vector InvertedPendulum::computeDynamics(const Vector& inState,
double M = cartMass_;
double l = pendulumLength_;
double lambda = viscosity_;
double l2 = l*l;
double dth2 = dth*dth;
double l2 = l * l;
double dth2 = dth * dth;
double sth = sin(th);
double cth = cos(th);
double sth2 = sth*sth;
double sth2 = sth * sth;
double b1 = F - m*l*dth2*sth - lambda*dx;
double b2 = m*l*g*sth - lambda*dth;
double b1 = F - m * l * dth2 * sth - lambda * dx;
double b2 = m * l * g * sth - lambda * dth;
double det = m*l2*(M + m*sth2);
double det = m * l2 * (M + m * sth2);
double ddx = (b1*m*l2 + b2*m*l*cth)/det;
double ddth = ((M+m)*b2 + m*l*cth*b1)/det;
double ddx = (b1 * m * l2 + b2 * m * l * cth) / det;
double ddth = ((M + m) * b2 + m * l * cth * b1) / det;
Vector nextState(4);
nextState(0) = x + dx*dt + .5*ddx*dt2;
nextState(1) = th + dth*dt + .5*ddth*dt2;
nextState(2) = dx + dt*ddx;
nextState(3) = dth + dt*ddth;
nextState(0) = x + dx * dt + .5 * ddx * dt2;
nextState(1) = th + dth * dt + .5 * ddth * dt2;
nextState(2) = dx + dt * ddx;
nextState(3) = dth + dt * ddth;
return nextState;
}
void InvertedPendulum::incr(double inTimeStep)
{
void InvertedPendulum::incr(double inTimeStep) {