PID control of double pendulum
We use a double pendulum URDF as model. The task of the PID controller is to do state feedback control and keep the double pendulum at the top. To get a sense of how the model look like, try the command:
1
bazel run //examples/double_pendulum:double_pendulum_demo
Copied!
The code in the tutorial is also available on my github, so you could also checkout my code directly if you get stuck.
In this tutorial, we will go through:
  1. 1.
    Create workspace and compile.
  2. 2.
    Import the double pendulum URDF as a MultibodyPlant.
  3. 3.
    Create a PID controller, and connect it to MultibodyPlant.
  4. 4.
    Simulate and visualize.

Run the demo

Create workspace

Create workspace and files.
1
cd drake
2
mkdir -p examples/double_pendulum_pid
Copied!

Add BUILD.bazel

To make the work space compile, we add the following to BUILD.bazel
BUILD.bazel
1
load(
2
"@drake//tools/skylark:drake_cc.bzl",
3
"drake_cc_binary",
4
)
5
6
load("//tools/install:install_data.bzl", "install_data")
7
8
drake_cc_binary(
9
name = "run_double_pendulum_pid_exe",
10
srcs = [
11
"run_double_pendulum_pid.cc",
12
],
13
data = [
14
":models",
15
"//tools:drake_visualizer",
16
],
17
deps = [
18
"//common:essential",
19
"//common:find_resource",
20
"//common:text_logging_gflags",
21
"//geometry:geometry_visualization",
22
"//lcm",
23
"//multibody/parsing",
24
"//multibody/plant",
25
"//systems/analysis",
26
"//systems/controllers",
27
"//systems/framework",
28
"@sdformat",
29
],
30
)
31
32
install_data()
Copied!

Add source file

And add the run_double_pendulum_pid.cc file
run_double_pendulum_pid.cc
1
///
2
/// @brief An SDF based double pendulum example.
3
///
4
5
#include <gflags/gflags.h>
6
7
#include <drake/systems/controllers/pid_controlled_system.h>
8
#include "drake/common/drake_assert.h"
9
#include "drake/common/find_resource.h"
10
#include "drake/common/text_logging_gflags.h"
11
#include "drake/geometry/geometry_visualization.h"
12
#include "drake/geometry/scene_graph.h"
13
#include "drake/lcm/drake_lcm.h"
14
#include "drake/multibody/parsing/parser.h"
15
#include "drake/multibody/plant/multibody_plant.h"
16
#include "drake/systems/analysis/simulator.h"
17
#include "drake/systems/framework/diagram.h"
18
#include "drake/systems/framework/diagram_builder.h"
19
#include "drake/systems/primitives/constant_vector_source.h"
20
21
DEFINE_double(target_realtime_rate, 1.0,
22
"Rate at which to run the simulation, relative to realtime");
23
DEFINE_double(simulation_time, 10, "How long to simulate the pendulum");
24
DEFINE_double(max_time_step, 1.0e-3,
25
"Simulation time step used for integrator.");
26
DEFINE_double(Kp_, 10.0, "Kp");
27
DEFINE_double(Ki_, 0.0, "Kp");
28
DEFINE_double(Kd_, 0.0, "Kp");
29
30
namespace drake {
31
namespace examples {
32
namespace double_pendulum {
33
namespace {
34
35
// Fixed path to double pendulum SDF model.
36
static const char* const kDoublePendulumSdfPath =
37
"drake/examples/double_pendulum/models/double_pendulum.sdf";
38
39
//
40
// Main function for demo.
41
//
42
void DoMain() {
43
DRAKE_DEMAND(FLAGS_simulation_time > 0);
44
45
systems::DiagramBuilder<double> builder;
46
47
geometry::SceneGraph<double>& scene_graph =
48
*builder.AddSystem<geometry::SceneGraph>();
49
scene_graph.set_name("scene_graph");
50
51
// Load and parse double pendulum SDF from file into a tree.
52
multibody::MultibodyPlant<double>* dp =
53
builder.AddSystem<multibody::MultibodyPlant<double>>(FLAGS_max_time_step);
54
dp->set_name("plant");
55
dp->RegisterAsSourceForSceneGraph(&scene_graph);
56
57
multibody::Parser parser(dp);
58
const std::string sdf_path = FindResourceOrThrow(kDoublePendulumSdfPath);
59
multibody::ModelInstanceIndex plant_model_instance_index =
60
parser.AddModelFromFile(sdf_path);
61
(void)plant_model_instance_index;
62
63
// Weld the base link to world frame with no rotation.
64
const auto& root_link = dp->GetBodyByName("base");
65
dp->AddJoint<multibody::WeldJoint>("weld_base", dp->world_body(), nullopt,
66
root_link, nullopt,
67
Isometry3<double>::Identity());
68
dp->AddJointActuator("a1", dp->GetJointByName("upper_joint"));
69
dp->AddJointActuator("a2", dp->GetJointByName("lower_joint"));
70
71
// Now the plant is complete.
72
dp->Finalize();
73
74
// Create PID Controller.
75
const Eigen::VectorXd Kp =
76
Eigen::VectorXd::Ones(dp->num_positions()) * FLAGS_Kp_;
77
const Eigen::VectorXd Ki =
78
Eigen::VectorXd::Ones(dp->num_positions()) * FLAGS_Ki_;
79
const Eigen::VectorXd Kd =
80
Eigen::VectorXd::Ones(dp->num_positions()) * FLAGS_Kd_;
81
const auto* const pid =
82
builder.AddSystem<systems::controllers::PidController<double>>(Kp, Ki,
83
Kd);
84
builder.Connect(dp->get_state_output_port(),
85
pid->get_input_port_estimated_state());
86
builder.Connect(pid->get_output_port_control(),
87
dp->get_actuation_input_port());
88
// Set PID desired states.
89
auto desired_base_source =
90
builder.AddSystem<systems::ConstantVectorSource<double>>(
91
Eigen::VectorXd::Zero(dp->num_multibody_states()));
92
builder.Connect(desired_base_source->get_output_port(),
93
pid->get_input_port_desired_state());
94
95
// Connect plant with scene_graph to get collision information.
96
DRAKE_DEMAND(!!dp->get_source_id());
97
builder.Connect(
98
dp->get_geometry_poses_output_port(),
99
scene_graph.get_source_pose_port(dp->get_source_id().value()));
100
builder.Connect(scene_graph.get_query_output_port(),
101
dp->get_geometry_query_input_port());
102
103
geometry::ConnectDrakeVisualizer(&builder, scene_graph);
104
105
auto diagram = builder.Build();
106
std::unique_ptr<systems::Context<double>> diagram_context =
107
diagram->CreateDefaultContext();
108
109
// Create plant_context to set velocity.
110
systems::Context<double>& plant_context =
111
diagram->GetMutableSubsystemContext(*dp, diagram_context.get());
112
// Set init position.
113
Eigen::VectorXd positions = Eigen::VectorXd::Zero(2);
114
positions[0] = 0.1;
115
positions[1] = 0.4;
116
dp->SetPositions(&plant_context, positions);
117
118
systems::Simulator<double> simulator(*diagram, std::move(diagram_context));
119
simulator.set_publish_every_time_step(true);
120
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
121
simulator.Initialize();
122
simulator.AdvanceTo(FLAGS_simulation_time);
123
}
124
125
} // namespace
126
} // namespace double_pendulum
127
} // namespace examples
128
} // namespace drake
129
130
int main(int argc, char** argv) {
131
gflags::SetUsageMessage(
132
"Simple sdformat usage example, just"
133
"make sure drake-visualizer is running!");
134
gflags::ParseCommandLineFlags(&argc, &argv, true);
135
drake::examples::double_pendulum::DoMain();
136
return 0;
137
}
Copied!

Simulate and visualize

1
bazel-bin/tools/drake_visualizer &
2
bazel run //examples/double_pendulum_pid:run_double_pendulum_pid_exe -- --Kp_=1000 --Ki_=5 --Kd_=100
Copied!

The Code Explained

Try with your own model

You could bring your own simple URDF/SDF (with 1 or 2 degree of freedom) and try doing a position control with the PID controller.