GCC Code Coverage Report


Directory: ./
File: src/inverted-pendulum.cpp
Date: 2024-11-12 12:07:01
Exec Total Coverage
Lines: 0 75 0.0%
Branches: 0 126 0.0%

Line Branch Exec Source
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 }
167