-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
print_symbolic_dynamics.cc
85 lines (72 loc) · 2.89 KB
/
print_symbolic_dynamics.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
#include <iostream>
#include "drake/common/find_resource.h"
#include "drake/common/symbolic/expression.h"
#include "drake/examples/pendulum/pendulum_plant.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/plant/multibody_plant.h"
// A simple example of extracting the symbolic dynamics of the pendulum system,
// and printing them to std::out.
using drake::symbolic::Expression;
using drake::symbolic::Variable;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using drake::systems::System;
namespace drake {
namespace examples {
namespace pendulum {
namespace {
// Obtains the dynamics using PendulumPlant (a System that directly models the
// dynamics).
VectorX<Expression> PendulumPlantDynamics() {
// Load the Pendulum.urdf into a symbolic PendulumPlant.
PendulumPlant<Expression> symbolic_plant;
// Obtain the symbolic dynamics.
auto context = symbolic_plant.CreateDefaultContext();
symbolic_plant.get_input_port().FixValue(context.get(),
Expression(Variable("tau")));
context->get_mutable_continuous_state_vector().SetAtIndex(
0, Variable("theta"));
context->get_mutable_continuous_state_vector().SetAtIndex(
1, Variable("thetadot"));
const auto& derivatives = symbolic_plant.EvalTimeDerivatives(*context);
return derivatives.CopyToVector();
}
// Obtains the dynamics using MultibodyPlant configured to model a pendulum.
VectorX<Expression> MultibodyPlantDynamics() {
// Load the Pendulum.urdf into a symbolic MultibodyPlant.
const char* const urdf_path = "drake/examples/pendulum/Pendulum.urdf";
MultibodyPlant<double> plant(0.0);
Parser parser(&plant);
parser.AddModelFromFile(FindResourceOrThrow(urdf_path));
plant.Finalize();
auto symbolic_plant_ptr = System<double>::ToSymbolic(plant);
const MultibodyPlant<Expression>& symbolic_plant = *symbolic_plant_ptr;
// Obtain the symbolic dynamics.
auto context = symbolic_plant.CreateDefaultContext();
symbolic_plant.get_actuation_input_port().FixValue(
context.get(), Expression(Variable("tau")));
symbolic_plant.SetPositionsAndVelocities(
context.get(), Vector2<Expression>(
Variable("theta"), Variable("thetadot")));
const auto& derivatives = symbolic_plant.EvalTimeDerivatives(*context);
return derivatives.CopyToVector();
}
int main() {
std::cout << "PendulumPlantDynamics:\n";
auto dynamics = PendulumPlantDynamics();
std::cout << "d/dt theta = " << dynamics[0] << "\n";
std::cout << "d/dt thetadot = " << dynamics[1] << "\n";
std::cout << "\n";
std::cout << "MultibodyPlantDynamics:\n";
dynamics = MultibodyPlantDynamics();
std::cout << "d/dt theta = " << dynamics[0] << "\n";
std::cout << "d/dt thetadot = " << dynamics[1] << "\n";
return 0;
}
} // namespace
} // namespace pendulum
} // namespace examples
} // namespace drake
int main() {
return drake::examples::pendulum::main();
}