forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
simple_continuous_time_system.cc
64 lines (51 loc) · 1.62 KB
/
simple_continuous_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
// Simple Continuous Time System Example
//
// This is meant to be a sort of "hello world" example for the
// drake::system classes. It defines a very simple continuous time system,
// 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 Continuous Time System
// xdot = -x + x³
// y = x
class SimpleContinuousTimeSystem : public LeafSystem<double> {
public:
SimpleContinuousTimeSystem() {
auto state_index = DeclareContinuousState(1); // One state variable.
DeclareStateOutputPort("y", state_index);
}
private:
// xdot = -x + x³
void DoCalcTimeDerivatives(
const Context<double>& context,
ContinuousState<double>* derivatives) const override {
const double x = context.get_continuous_state()[0];
const double xdot = -x + std::pow(x, 3.0);
(*derivatives)[0] = xdot;
}
};
int main() {
// Create the simple system.
SimpleContinuousTimeSystem system;
// Create the simulator.
Simulator<double> simulator(system);
// Set the initial conditions x(0).
ContinuousState<double>& state =
simulator.get_mutable_context().get_mutable_continuous_state();
state[0] = 0.9;
// Simulate for 10 seconds.
simulator.AdvanceTo(10);
// Make sure the simulation converges to the stable fixed point at x=0.
DRAKE_DEMAND(state[0] < 1.0e-4);
// TODO(russt): make a plot of the resulting trajectory.
return 0;
}
} // namespace
} // namespace systems
} // namespace drake
int main() {
return drake::systems::main();
}