Create a simple system
Before starting, make sure you understand the basic Drake concept at this link.

Goal

Given a Equation of Motion (EoM) of a particle mass system, create the exact system which moves with external force. Simulate the system and visualize the results.

Analysis

A non-linear system has EoM like this:
x˙=f(t,x,u)y=g(t,x,u)\dot{x} = f(t,x,u) \\ y = g(t,x,u)
Say in our example, the system has EoM like this:
x¨=f(t)/my(t)=x(t)where,x(t)=[x,x˙]\ddot{x} = f(t)/m \\ y(t) = x(t) \\ where, x(t) = [x, \dot{x}]'
The system get external force
f(t)f(t)
from input port, then we compute the system state and write the result observation
y(t)y(t)
to output port.
The
f(t)f(t)
comes from a input source, the output of the system goes to the visualization so we could observe the result.

Code Implementation

Create the system

1
class Particle1dPlant final : public systems::LeafSystem<T> {
2
public:
3
// Disables the built in copy and move constructor.
4
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Particle1dPlant);
5
6
/// Constructor defines a prototype for its continuous state and output port.
7
Particle1dPlant();
8
9
/// Return the OutputPort associated with this Particle1dPlant.
10
/// This method is called when connecting the ports in the diagram builder.
11
const systems::OutputPort<T>& get_output_port() const {
12
return systems::System<T>::get_output_port(0);
13
}
14
/// Returns the current state of this Particle1dPlant as a BasicVector.
15
/// The return value is mutable to allow the calling method to change the
16
/// state (e.g., to set initial values). For example, this method is called
17
/// when building a diagram so initial values can be set by the simulator.
18
/// @param[in] context The Particle1dPlant sub-system context.
19
static systems::BasicVector<T>& get_mutable_state(
20
systems::Context<T>* context) {
21
return get_mutable_state(&context->get_mutable_continuous_state());
22
}
23
24
private:
25
// Casts the continuous state vector from a VectorBase to a BasicVector.
26
static const systems::BasicVector<T>& get_state(
27
const systems::ContinuousState<T>& cstate) {
28
return dynamic_cast<const systems::BasicVector<T>&>(cstate.get_vector());
29
}
30
31
// This method is called in DoCalcTimeDerivative() as a way to update the
32
// state before the time derivatives are calculated.
33
static const systems::BasicVector<T>& get_state(
34
const systems::Context<T>& context) {
35
return get_state(context.get_continuous_state());
36
}
37
38
// Casts the mutable continuous state vector from a VectorBase to BasicVector.
39
static systems::BasicVector<T>& get_mutable_state(
40
systems::ContinuousState<T>* cstate) {
41
return dynamic_cast<systems::BasicVector<T>&>(cstate->get_mutable_vector());
42
}
43
44
// This is the calculator method that assigns values to the state output port.
45
void CopyStateOut(const systems::Context<T>& context,
46
systems::BasicVector<T>* output) const;
47
48
// Method that calculates the state time derivatives.
49
void DoCalcTimeDerivatives(const systems::Context<T>& context,
50
systems::ContinuousState<T>* derivatives) const override;
51
};
Copied!

Build the diagram

1
// Parsing the URDF and constructing a RigidBodyTree from it
2
auto tree =
3
std::make_unique<RigidBodyTree<double>>();
4
parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
5
FindResourceOrThrow("drake/examples/particle1d/particle1d.urdf"),
6
multibody::joints::kFixed, tree.get());
7
8
// Construct and empty diagram, then add a particle plant.
9
systems::DiagramBuilder<double> builder;
10
11
Particle1dPlant<double>* particle_plant =
12
builder.AddSystem<Particle1dPlant<double>>();
13
particle_plant->set_name("RigidBox");
14
15
// To set constant parameters (such as mass) in the particle plant, pass
16
// information from the tree (which read constant parameters in the .urdf).
17
particle_plant->SetConstantParameters(*tree);
18
19
// Add a visualizer block to the diagram, uses the tree and an lcm object.
20
lcm::DrakeLcm lcm;
21
systems::DrakeVisualizer* publisher =
22
builder.AddSystem<systems::DrakeVisualizer>(*tree, &lcm);
23
publisher->set_name("publisher");
24
25
// Within the diagram, connect the particle plant's output port to the
26
// visualizer's input port.
27
builder.Connect(particle_plant->get_output_port(),
28
publisher->get_input_port(0));
29
30
// Build the diagram described by previous call to connect input/export port.
31
auto diagram = builder.Build();
32
33
// Constructs a Simulator that advances this diagram through time.
34
systems::Simulator<double> simulator(*diagram);
35
36
// To set initial values for the simulation:
37
// * Get the Diagram's context.
38
// * Get the part of the Diagram's context associated with particle_plant.
39
// * Get the part of the particle_plant's context associated with state.
40
// * Fill the state with initial values.
41
systems::Context<double>& simulator_context = simulator.get_mutable_context();
42
systems::Context<double>& particle_plant_context =
43
diagram->GetMutableSubsystemContext(*particle_plant, &simulator_context);
44
systems::BasicVector<double>& state =
45
particle_plant->get_mutable_state(&particle_plant_context);
46
47
const double x_init = 0.0;
48
const double xDt_init = 0.0;
49
state.SetAtIndex(0, x_init);
50
state.SetAtIndex(1, xDt_init);
51
52
// Set upper-limit on simulation speed (mostly for visualization).
53
simulator.set_target_realtime_rate(1);
54
55
// Simulate for 10 seconds (default units are SI, with units of seconds).
56
std::cout << "Starting simulation." << std::endl;
57
simulator.StepTo(10.0);
58
std::cout << "Simulation done." << std::endl;
Copied!

Credit

Thanks Manuel Ahumada for providing this example.