forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
simple_mixed_continuous_and_discrete_time_system.cc
93 lines (77 loc) · 2.69 KB
/
simple_mixed_continuous_and_discrete_time_system.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
// Simple Mixed Continuous-Time/Discrete-Time System Example
//
// This is meant to be a sort of "hello world" example for the
// drake::system classes. It defines a very simple system with both continuous
// and discrete time dynamics, simulates it from a given initial condition, and
// checks the result.
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/leaf_system.h"
namespace drake {
namespace systems {
namespace {
// Simple Discrete Time System
// xd_{n+1} = xd_n³
// xcdot = -xc + xc³
// y = [xd;xc]
class SimpleMixedContinuousTimeDiscreteTimeSystem : public LeafSystem<double> {
public:
SimpleMixedContinuousTimeDiscreteTimeSystem() {
DeclarePeriodicDiscreteUpdateEvent(
1.0, 0.0, &SimpleMixedContinuousTimeDiscreteTimeSystem::Update);
DeclareVectorOutputPort(
"y", BasicVector<double>(2), // xd;xc
&SimpleMixedContinuousTimeDiscreteTimeSystem::CopyStateOut);
DeclareDiscreteState(1); // xd
DeclareContinuousState(1); // xc
}
private:
// xd_{n+1} = xd_n³
void Update(const Context<double>& context,
DiscreteValues<double>* updates) const {
const double xd_n = context.get_discrete_state()[0];
const double xd_np1 = std::pow(xd_n, 3.0);
(*updates)[0] = xd_np1;
}
// xcdot = -xc + xc³
void DoCalcTimeDerivatives(
const Context<double>& context,
ContinuousState<double>* derivatives) const override {
const double xc = context.get_continuous_state()[0];
const double xcdot = -xc + std::pow(xc, 3.0);
(*derivatives)[0] = xcdot;
}
// y = x
void CopyStateOut(const Context<double>& context,
BasicVector<double>* output) const {
const double xd = context.get_discrete_state()[0];
(*output)[0] = xd;
const double xc = context.get_continuous_state()[0];
(*output)[1] = xc;
}
};
int main() {
// Create the simple system.
SimpleMixedContinuousTimeDiscreteTimeSystem system;
// Create the simulator.
Simulator<double> simulator(system);
// Set the initial conditions xd_0, xc(0).
DiscreteValues<double>& xd =
simulator.get_mutable_context().get_mutable_discrete_state();
xd[0] = 0.99; // xd_0
ContinuousState<double>& xc =
simulator.get_mutable_context().get_mutable_continuous_state();
xc[0] = 0.9; // xc(0)
// Simulate for 10 seconds.
simulator.AdvanceTo(10);
// Make sure the simulation converges to the stable fixed point at x=0.
DRAKE_DEMAND(xd[0] < 1.0e-4);
DRAKE_DEMAND(xc[0] < 1.0e-4);
// TODO(russt): make a plot of the resulting trajectory (using vtk?).
return 0;
}
} // namespace
} // namespace systems
} // namespace drake
int main() {
return drake::systems::main();
}