GCC Code Coverage Report

Directory: ./
File: src/inverted-pendulum.cpp
Date: 2024-12-12 12:09:11
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