LQR on Cart Pole
Linear Quadratic Regulator (LQR) is an optimal control method. Cart-Pole is a canonical model with one prismatic joint connecting the ground and cart and one revolute joint connect the cart and bar.
Cart-Pole tracking state

A brief explanation of LQR

LQR controller is a gain matrix K, which maps income system state to control. The gain matrix K comes from the solution of Riccati Equation.
Optimal control is all about minimizing cost. We formulate the state tracking control problem into a cost minimizing problem, which is equal to solve the Riccati Equation. What's amazing is, the solution of Riccati Equation gives us the optimal controller that generates the minimum cost.
So given the system and cost definition, we could compute the optimal controller K.
To learn more about Cart-Pole model and LQR, find the notes from Russ Tedrake's Underactauted Robotics.

Get the code

Get the code from my repo, we need to modify the original SDF file to get a fully actuated Cart Pole. And we need a fixed DARE solver in Drake.
1
<sdf version='1.6'>
2
<model name='CartPole'>
3
<!-- This sdf file produces a model with the default parameters as
4
documented in cart_pole_params.named_vector.
5
They MUST be kept in sync. -->
6
<link name='Cart'>
7
<pose>0 0 0 0 0 0</pose>
8
<inertial>
9
<mass>10.0</mass>
10
<!-- For this model case, with the cart not having any rotational
11
degrees of freedom, the values of the inertia matrix do not
12
participate in the model. Therefore we just set them to zero
13
(or near to zero since sdformat does not allow exact zeroes
14
for inertia values). -->
15
<inertia>
16
<ixx>1.0e-20</ixx><iyy>1.0e-20</iyy><izz>1.0e-20</izz>
17
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
18
</inertia>
19
</inertial>
20
21
<visual name='cart_visual'>
22
<pose>0 0 0 0 0 0</pose>
23
<geometry>
24
<box>
25
<size>0.24 0.12 0.12</size>
26
</box>
27
</geometry>
28
</visual>
29
</link>
30
31
<link name='Pole'>
32
<!-- The pole is modeled as a point mass at the end of a pole. -->
33
<!-- The length of the pole is 0.5 meters. -->
34
<pose>0 0 -0.5 0 0 0</pose>
35
<inertial>
36
<mass>1.0</mass>
37
<!-- A point mass has zero rotational inertia.
38
We must specify small values since otherwise sdformat throws an
39
exception. -->
40
<inertia>
41
<ixx>1.0e-20</ixx><iyy>1.0e-20</iyy><izz>1.0e-20</izz>
42
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
43
</inertia>
44
</inertial>
45
46
<visual name='pole_point_mass'>
47
<pose>0 0 0 0 0 0</pose>
48
<geometry>
49
<sphere>
50
<radius>0.05</radius>
51
</sphere>
52
</geometry>
53
</visual>
54
<visual name='pole_rod'>
55
<pose>0 0 0.25 0 0 0</pose>
56
<geometry>
57
<cylinder>
58
<radius>0.025</radius>
59
<length>0.5</length>
60
</cylinder>
61
</geometry>
62
</visual>
63
</link>
64
65
<joint name='CartSlider' type='prismatic'>
66
<parent>world</parent>
67
<child>Cart</child>
68
<axis>
69
<xyz>1.0 0.0 0.0</xyz>
70
<limit>
71
<!-- The pole pin joint is not actuated. -->
72
<effort>1.0</effort>
73
</limit>
74
</axis>
75
</joint>
76
77
<joint name='PolePin' type='revolute'>
78
<!-- Pose of the joint frame in the pole's frame (located at the point
79
mass) -->
80
<pose>0 0 0.5 0 0 0</pose>
81
<parent>Cart</parent>
82
<child>Pole</child>
83
<axis>
84
<xyz>0.0 -1.0 0.0</xyz>
85
<limit>
86
<!-- The pole pin joint is not actuated. -->
87
<effort>1.0</effort>
88
</limit>
89
</axis>
90
</joint>
91
</model>
92
</sdf>
Copied!
cart_pole_lqr.cc
1
///
2
/// This file use a fully actuated cart pole model to track a specific state
3
///
4
5
#include <gflags/gflags.h>
6
7
#include <drake/systems/framework/event.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/controllers/linear_quadratic_regulator.h"
18
#include "drake/systems/framework/diagram.h"
19
#include "drake/systems/framework/diagram_builder.h"
20
#include "drake/systems/framework/framework_common.h"
21
#include "drake/systems/primitives/affine_system.h"
22
#include "drake/systems/primitives/constant_vector_source.h"
23
#include "drake/systems/primitives/linear_system.h"
24
25
DEFINE_double(target_realtime_rate, 1.0,
26
"Rate at which to run the simulation, relative to realtime");
27
DEFINE_double(simulation_time, 10, "How long to simulate the pendulum");
28
DEFINE_double(max_time_step, 1.0e-3,
29
"Simulation time step used for integrator.");
30
31
namespace drake {
32
namespace examples {
33
namespace multibody {
34
namespace cart_pole {
35
namespace {
36
37
// Fixed path to double pendulum SDF model.
38
static const char* const kCartPoleSdfPath =
39
"drake/examples/multibody/cart_pole/cart_pole.sdf";
40
41
//
42
// Main function for demo.
43
//
44
void DoMain() {
45
DRAKE_DEMAND(FLAGS_simulation_time > 0);
46
logging::HandleSpdlogGflags();
47
48
systems::DiagramBuilder<double> builder;
49
50
geometry::SceneGraph<double>& scene_graph =
51
*builder.AddSystem<geometry::SceneGraph>();
52
scene_graph.set_name("scene_graph");
53
54
// Load and parse double pendulum SDF from file into a tree.
55
drake::multibody::MultibodyPlant<double>* cp =
56
builder.AddSystem<drake::multibody::MultibodyPlant<double>>(
57
FLAGS_max_time_step);
58
cp->set_name("cart_pole");
59
cp->RegisterAsSourceForSceneGraph(&scene_graph);
60
61
drake::multibody::Parser parser(cp);
62
const std::string sdf_path = FindResourceOrThrow(kCartPoleSdfPath);
63
drake::multibody::ModelInstanceIndex plant_model_instance_index =
64
parser.AddModelFromFile(sdf_path);
65
(void)plant_model_instance_index;
66
67
// Now the plant is complete.
68
cp->Finalize();
69
70
// Create LQR Controller.
71
auto cp_context = cp->CreateDefaultContext();
72
const int CartPole_actuation_port = 3;
73
// Set nominal torque to zero.
74
Eigen::VectorXd u0 = Eigen::VectorXd::Zero(2);
75
cp_context->FixInputPort(CartPole_actuation_port, u0);
76
77
// Set nominal state to the upright fixed point.
78
Eigen::VectorXd x0 = Eigen::VectorXd::Zero(4);
79
x0[0] = 1;
80
x0[1] = M_PI;
81
cp_context->SetDiscreteState(x0);
82
83
// Setup LQR Cost matrices (penalize position error 10x more than velocity
84
// to roughly address difference in units, using sqrt(g/l) as the time
85
// constant.
86
Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(4, 4);
87
Q(0, 0) = 10;
88
Q(1, 1) = 10;
89
Eigen::MatrixXd R = Eigen::MatrixXd::Identity(2, 2);
90
Eigen::MatrixXd N;
91
auto lqr = builder.AddSystem(systems::controllers::LinearQuadraticRegulator(
92
*cp, *cp_context, Q, R, N, CartPole_actuation_port));
93
94
builder.Connect(cp->get_state_output_port(), lqr->get_input_port());
95
builder.Connect(lqr->get_output_port(), cp->get_actuation_input_port());
96
97
// Connect plant with scene_graph to get collision information.
98
DRAKE_DEMAND(!!cp->get_source_id());
99
builder.Connect(
100
cp->get_geometry_poses_output_port(),
101
scene_graph.get_source_pose_port(cp->get_source_id().value()));
102
builder.Connect(scene_graph.get_query_output_port(),
103
cp->get_geometry_query_input_port());
104
105
geometry::ConnectDrakeVisualizer(&builder, scene_graph);
106
107
auto diagram = builder.Build();
108
std::unique_ptr<systems::Context<double>> diagram_context =
109
diagram->CreateDefaultContext();
110
111
// Create plant_context to set velocity.
112
systems::Context<double>& plant_context =
113
diagram->GetMutableSubsystemContext(*cp, diagram_context.get());
114
// Set init position.
115
Eigen::VectorXd positions = Eigen::VectorXd::Zero(2);
116
positions[0] = 0.0;
117
positions[1] = 0.0;
118
cp->SetPositions(&plant_context, positions);
119
120
systems::Simulator<double> simulator(*diagram, std::move(diagram_context));
121
simulator.set_publish_every_time_step(true);
122
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
123
simulator.Initialize();
124
simulator.AdvanceTo(FLAGS_simulation_time);
125
}
126
127
} // namespace
128
} // namespace cart_pole
129
} // namespace multibody
130
} // namespace examples
131
} // namespace drake
132
133
int main(int argc, char** argv) {
134
gflags::SetUsageMessage(
135
"Using LQR controller to control "
136
"the fully actuated cart pole model!");
137
gflags::ParseCommandLineFlags(&argc, &argv, true);
138
drake::examples::multibody::cart_pole::DoMain();
139
return 0;
140
}
Copied!
Add the following lines to BUILD.bazel.
BUILD.bazel
1
drake_cc_binary(
2
name = "cart_pole_lqr",
3
srcs = ["cart_pole_lqr.cc"],
4
data = ["cart_pole.sdf"],
5
deps = [
6
"//common:find_resource",
7
"//common:text_logging_gflags",
8
"//geometry:geometry_visualization",
9
"//multibody/parsing",
10
"//multibody/plant",
11
"//systems/analysis:simulator",
12
"//systems/controllers",
13
"//systems/framework:diagram",
14
"//systems/framework:event_collection",
15
"//systems/primitives",
16
"@gflags",
17
],
18
)
Copied!

Run the demo

To get the demo working, a DARE solver is required. We could get the solver by:
1
git remote add weiqiao https://github.com/weiqiao/drake.git
2
git fetch weiqiao
3
git cherry-pick e777b31f04ec1d176a33d018669f62e9d3924e72
4
git cherry-pick aee6342eb01b142f5f109c6dba8eafa8f5994601
5
git cherry-pick a6f90a80f6842a67fe596c748e5b71eb56a7500a
Copied!
Then run the demo:
1
bazel run //examples/multibody/cart_pole:cart_pole_lqr
Copied!

The code explained

Implementation notes

LQR Controller in Drake is an affine system. Because LQR is about error dynamics, so the gain matrix K of LQR has to get the state error, and output the control error. With affine system, LQR could get state directly and output control directly, the conversion to state error and convert back to control is handled within the affine system.
Indexing is very important in Drake. A correct indexing is required to get the system work correctly. LQR is using Drake Index on control rather than generalized tree index.