feedback-controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010 CNRS
3  *
4  * Florent Lamiraux
5  */
6 
8 
11 #include <dynamic-graph/factory.h>
12 
13 #include "constant.hh"
14 
15 using namespace dynamicgraph;
16 using namespace dynamicgraph::tutorial;
17 
18 // Register new Entity type in the factory
19 // Note that the second argument is the type name of the python class
20 // that will be created when importing the python module.
22 
23 FeedbackController::FeedbackController(const std::string& inName)
24  : Entity(inName),
25  stateSIN(NULL,
26  "FeedbackController(" + inName + ")::input(vector)::state"),
27  forceSOUT(stateSIN,
28  "FeedbackController(" + inName + ")::output(double)::force"),
29  gain_(Matrix(4, 1)) {
30  // Register signals into the entity.
33 
34  // Set signals as constant to size them
35  double force = 0.;
36  Vector state(4);
37  state.fill(0.);
38  forceSOUT.setConstant(force);
39  stateSIN.setConstant(state);
40 
41  // Define refresh function for output signal
42  boost::function2<double&, double&, const int&> ftest =
43  boost::bind(&FeedbackController::computeForceFeedback, this, _1, _2);
44 
46  boost::bind(&FeedbackController::computeForceFeedback, this, _1, _2));
47  std::string docstring;
48  // setGain
49  docstring =
50  "\n"
51  " Set gain of controller\n"
52  " takes a tuple of 4 floating point numbers as input\n"
53  "\n";
54  addCommand(std::string("setGain"),
55  new ::dynamicgraph::command::Setter<FeedbackController, Matrix>(
56  *this, &FeedbackController::setGain, docstring));
57 
58  // getGain
59  docstring =
60  "\n"
61  " Get gain of controller\n"
62  " return a tuple of 4 floating point numbers\n"
63  "\n";
64  addCommand(std::string("getGain"),
65  new ::dynamicgraph::command::Getter<FeedbackController, Matrix>(
66  *this, &FeedbackController::getGain, docstring));
67 }
68 
70 
72  const int& inTime) {
73  const Vector& state = stateSIN(inTime);
74 
75  if (state.size() != 4)
77  "state signal size is ",
78  "%d, should be 4.", state.size());
79  Vector v(-gain_ * state);
80  force = v(0);
81  return force;
82 }
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MyEntity, "MyEntity")
SignalTimeDependent< double, int > forceSOUT
Force computed by the control law.
Eigen::VectorXd Vector
void signalRegistration(const SignalArray< int > &signals)
virtual void setConstant(const T &t)
::dynamicgraph::Matrix getGain() const
Get feedback gain.
Feedback controller for an inverted pendulum.
void setGain(const ::dynamicgraph::Matrix &inGain)
Get feedback gain.
virtual void setConstant(const T &t)
::dynamicgraph::Matrix gain_
Gain of the controller.
double & computeForceFeedback(double &force, const int &inTime)
FeedbackController(const std::string &inName)
Constructor by name.
Eigen::MatrixXd Matrix
v
void addCommand(const std::string &name, command::Command *command)
SignalPtr< ::dynamicgraph::Vector, int > stateSIN
State of the inverted pendulum.
virtual void setFunction(boost::function2< T &, T &, Time > t, Mutex *mutexref=NULL)


dynamic-graph-tutorial
Author(s): Nicolas Mansard, Olivier Stasse
autogenerated on Sun Jun 25 2023 02:37:04