1 |
|
|
/* |
2 |
|
|
* Copyright 2010 CNRS |
3 |
|
|
* |
4 |
|
|
* Florent Lamiraux |
5 |
|
|
*/ |
6 |
|
|
|
7 |
|
|
#include "dynamic-graph/tutorial/inverted-pendulum.hh" |
8 |
|
|
|
9 |
|
|
#include <dynamic-graph/command-getter.h> |
10 |
|
|
#include <dynamic-graph/command-setter.h> |
11 |
|
|
#include <dynamic-graph/factory.h> |
12 |
|
|
|
13 |
|
|
#include <boost/format.hpp> |
14 |
|
|
#include <boost/numeric/ublas/io.hpp> |
15 |
|
|
|
16 |
|
|
#include "command-increment.hh" |
17 |
|
|
#include "constant.hh" |
18 |
|
|
|
19 |
|
|
using namespace dynamicgraph; |
20 |
|
|
using namespace dynamicgraph::tutorial; |
21 |
|
|
|
22 |
|
|
const double Constant::gravity = 9.81; |
23 |
|
|
|
24 |
|
|
// Register new Entity type in the factory |
25 |
|
|
// Note that the second argument is the type name of the python class |
26 |
|
|
// that will be created when importing the python module. |
27 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(InvertedPendulum, "InvertedPendulum"); |
28 |
|
|
|
29 |
|
|
InvertedPendulum::InvertedPendulum(const std::string& inName) |
30 |
|
|
: Entity(inName), |
31 |
|
|
forceSIN(NULL, "InvertedPendulum(" + inName + ")::input(double)::force"), |
32 |
|
|
stateSOUT("InvertedPendulum(" + inName + ")::output(vector)::state"), |
33 |
|
|
cartMass_(1.0), |
34 |
|
|
pendulumMass_(1.0), |
35 |
|
|
pendulumLength_(1.0), |
36 |
|
|
viscosity_(0.1) { |
37 |
|
|
// Register signals into the entity. |
38 |
|
|
signalRegistration(forceSIN); |
39 |
|
|
signalRegistration(stateSOUT); |
40 |
|
|
|
41 |
|
|
// Set signals as constant to size them |
42 |
|
|
Vector state(4); |
43 |
|
|
state.fill(0.); |
44 |
|
|
double input = 0.; |
45 |
|
|
stateSOUT.setConstant(state); |
46 |
|
|
forceSIN.setConstant(input); |
47 |
|
|
|
48 |
|
|
// Commands |
49 |
|
|
std::string docstring; |
50 |
|
|
|
51 |
|
|
// Incr |
52 |
|
|
docstring = |
53 |
|
|
"\n" |
54 |
|
|
" Integrate dynamics for time step provided as input\n" |
55 |
|
|
"\n" |
56 |
|
|
" take one floating point number as input\n" |
57 |
|
|
"\n"; |
58 |
|
|
addCommand(std::string("incr"), new command::Increment(*this, docstring)); |
59 |
|
|
|
60 |
|
|
// setCartMass |
61 |
|
|
docstring = |
62 |
|
|
"\n" |
63 |
|
|
" Set cart mass\n" |
64 |
|
|
"\n"; |
65 |
|
|
addCommand(std::string("setCartMass"), |
66 |
|
|
new ::dynamicgraph::command::Setter<InvertedPendulum, double>( |
67 |
|
|
*this, &InvertedPendulum::setCartMass, docstring)); |
68 |
|
|
|
69 |
|
|
// getCartMass |
70 |
|
|
docstring = |
71 |
|
|
"\n" |
72 |
|
|
" Get cart mass\n" |
73 |
|
|
"\n"; |
74 |
|
|
addCommand(std::string("getCartMass"), |
75 |
|
|
new ::dynamicgraph::command::Getter<InvertedPendulum, double>( |
76 |
|
|
*this, &InvertedPendulum::getCartMass, docstring)); |
77 |
|
|
|
78 |
|
|
// setPendulumMass |
79 |
|
|
docstring = |
80 |
|
|
"\n" |
81 |
|
|
" Set pendulum mass\n" |
82 |
|
|
"\n"; |
83 |
|
|
addCommand(std::string("setPendulumMass"), |
84 |
|
|
new ::dynamicgraph::command::Setter<InvertedPendulum, double>( |
85 |
|
|
*this, &InvertedPendulum::setPendulumMass, docstring)); |
86 |
|
|
|
87 |
|
|
// getPendulumMass |
88 |
|
|
docstring = |
89 |
|
|
"\n" |
90 |
|
|
" Get pendulum mass\n" |
91 |
|
|
"\n"; |
92 |
|
|
addCommand(std::string("getPendulumMass"), |
93 |
|
|
new ::dynamicgraph::command::Getter<InvertedPendulum, double>( |
94 |
|
|
*this, &InvertedPendulum::getPendulumMass, docstring)); |
95 |
|
|
|
96 |
|
|
// setPendulumLength |
97 |
|
|
docstring = |
98 |
|
|
"\n" |
99 |
|
|
" Set pendulum length\n" |
100 |
|
|
"\n"; |
101 |
|
|
addCommand(std::string("setPendulumLength"), |
102 |
|
|
new ::dynamicgraph::command::Setter<InvertedPendulum, double>( |
103 |
|
|
*this, &InvertedPendulum::setPendulumLength, docstring)); |
104 |
|
|
|
105 |
|
|
// getPendulumLength |
106 |
|
|
docstring = |
107 |
|
|
"\n" |
108 |
|
|
" Get pendulum length\n" |
109 |
|
|
"\n"; |
110 |
|
|
addCommand(std::string("getPendulumLength"), |
111 |
|
|
new ::dynamicgraph::command::Getter<InvertedPendulum, double>( |
112 |
|
|
*this, &InvertedPendulum::getPendulumLength, docstring)); |
113 |
|
|
} |
114 |
|
|
|
115 |
|
|
InvertedPendulum::~InvertedPendulum() {} |
116 |
|
|
|
117 |
|
|
Vector InvertedPendulum::computeDynamics(const Vector& inState, |
118 |
|
|
const double& inControl, |
119 |
|
|
double inTimeStep) { |
120 |
|
|
if (inState.size() != 4) |
121 |
|
|
throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC, |
122 |
|
|
"state signal size is ", |
123 |
|
|
"%d, should be 4.", inState.size()); |
124 |
|
|
|
125 |
|
|
double dt = inTimeStep; |
126 |
|
|
double dt2 = dt * dt; |
127 |
|
|
double g = Constant::gravity; |
128 |
|
|
double x = inState(0); |
129 |
|
|
double th = inState(1); |
130 |
|
|
double dx = inState(2); |
131 |
|
|
double dth = inState(3); |
132 |
|
|
double F = inControl; |
133 |
|
|
double m = pendulumMass_; |
134 |
|
|
double M = cartMass_; |
135 |
|
|
double l = pendulumLength_; |
136 |
|
|
double lambda = viscosity_; |
137 |
|
|
double l2 = l * l; |
138 |
|
|
double dth2 = dth * dth; |
139 |
|
|
double sth = sin(th); |
140 |
|
|
double cth = cos(th); |
141 |
|
|
double sth2 = sth * sth; |
142 |
|
|
|
143 |
|
|
double b1 = F - m * l * dth2 * sth - lambda * dx; |
144 |
|
|
double b2 = m * l * g * sth - lambda * dth; |
145 |
|
|
|
146 |
|
|
double det = m * l2 * (M + m * sth2); |
147 |
|
|
|
148 |
|
|
double ddx = (b1 * m * l2 + b2 * m * l * cth) / det; |
149 |
|
|
double ddth = ((M + m) * b2 + m * l * cth * b1) / det; |
150 |
|
|
|
151 |
|
|
Vector nextState(4); |
152 |
|
|
nextState(0) = x + dx * dt + .5 * ddx * dt2; |
153 |
|
|
nextState(1) = th + dth * dt + .5 * ddth * dt2; |
154 |
|
|
nextState(2) = dx + dt * ddx; |
155 |
|
|
nextState(3) = dth + dt * ddth; |
156 |
|
|
|
157 |
|
|
return nextState; |
158 |
|
|
} |
159 |
|
|
|
160 |
|
|
void InvertedPendulum::incr(double inTimeStep) { |
161 |
|
|
int t = stateSOUT.getTime(); |
162 |
|
|
Vector nextState = computeDynamics(stateSOUT(t), forceSIN(t), inTimeStep); |
163 |
|
|
stateSOUT.setConstant(nextState); |
164 |
|
|
stateSOUT.setTime(t + 1); |
165 |
|
|
forceSIN(t + 1); |
166 |
|
|
} |