Inverse Dynamics Control on fully-actuated KUKA arm
Control KUKA back to home position with inverse dynamics controller

Why Inverse Dynamics Control, why not PID?

Why not use PID controller? That's a good question. If the robot's joints are decouples and using PID controller for each joint is the way to go. All you need is model-free parameters that corresponds to each joint. In my case, I am simulating a humanoid robot with two arms, the joints and links are deeply coupled that model-free method does not work well here.
The Inverse Dynamics Controller is a system block that takes in the desired acceleration and spit out whatever the generalized control force is. Using Inverse Dynamics Controller is easy in Drake because MultibodyPlant does all the inverse dynamics calculation for you.

Code

Create a folder drake/examples/kuka_iiwa_arm_idc/
1
cd drake
2
mkdir -p examples/kuka_iiwa_arm_idc
Copied!
Put the following file into the folder.
run_kuka_idc_demo.cc
1
/// @file
2
///
3
/// This demo sets up a kuka arm simulation.
4
/// An inverse dynamics controller was created to control the robot to
5
/// the desired system state.
6
7
#include <gflags/gflags.h>
8
9
#include "drake/common/drake_assert.h"
10
#include "drake/common/find_resource.h"
11
#include "drake/common/text_logging_gflags.h"
12
#include "drake/common/type_safe_index.h"
13
#include "drake/geometry/geometry_visualization.h"
14
#include "drake/geometry/scene_graph.h"
15
#include "drake/lcm/drake_lcm.h"
16
#include "drake/multibody/parsing/parser.h"
17
#include "drake/multibody/plant/multibody_plant.h"
18
#include "drake/multibody/tree/weld_joint.h"
19
#include "drake/systems/analysis/simulator.h"
20
#include "drake/systems/controllers/inverse_dynamics_controller.h"
21
#include "drake/systems/framework/diagram.h"
22
#include "drake/systems/framework/diagram_builder.h"
23
#include "drake/systems/primitives/constant_vector_source.h"
24
25
namespace drake {
26
namespace examples {
27
namespace kuka {
28
29
using drake::multibody::MultibodyPlant;
30
31
DEFINE_double(simulation_time, 5,
32
"Desired duration of the simulation in seconds");
33
DEFINE_double(max_time_step, 1.0e-3,
34
"Simulation time step used for integrator.");
35
DEFINE_double(target_realtime_rate, 1,
36
"Desired rate relative to real time. See documentation for "
37
"Simulator::set_target_realtime_rate() for details.");
38
39
DEFINE_bool(add_gravity, true,
40
"Indicator for whether terrestrial gravity"
41
" (9.81 m/s²) is included or not.");
42
43
DEFINE_double(Kp, 10.0, "Kp");
44
DEFINE_double(Ki, 0.1, "Ki");
45
DEFINE_double(Kd, 5.0, "Kd");
46
47
void DoMain() {
48
DRAKE_DEMAND(FLAGS_simulation_time > 0);
49
50
systems::DiagramBuilder<double> builder;
51
52
geometry::SceneGraph<double>& scene_graph =
53
*builder.AddSystem<geometry::SceneGraph>();
54
scene_graph.set_name("scene_graph");
55
56
MultibodyPlant<double>& plant =
57
*builder.AddSystem<MultibodyPlant>(FLAGS_max_time_step);
58
plant.RegisterAsSourceForSceneGraph(&scene_graph);
59
std::string full_name = FindResourceOrThrow(
60
"drake/manipulation/models/"
61
"iiwa_description/sdf/iiwa14_no_collision.sdf");
62
63
multibody::ModelInstanceIndex plant_index =
64
multibody::Parser(&plant).AddModelFromFile(full_name);
65
66
// Weld the hand to the world frame.
67
const auto& joint_arm_root = plant.GetBodyByName("iiwa_link_0");
68
plant.AddJoint<multibody::WeldJoint>("weld_arm", plant.world_body(), nullopt,
69
joint_arm_root, nullopt,
70
Isometry3<double>::Identity());
71
72
if (!FLAGS_add_gravity) {
73
plant.mutable_gravity_field().set_gravity_vector(Eigen::Vector3d::Zero());
74
}
75
76
// Now the model is complete.
77
plant.Finalize();
78
79
// Defind desired states, here we choose 0 positions and 0 velocities.
80
VectorX<double> desired_state =
81
VectorX<double>::Zero(plant.num_multibody_states());
82
auto desired_state_source =
83
builder.AddSystem<systems::ConstantVectorSource<double>>(desired_state);
84
desired_state_source->set_name("constant_source");
85
86
drake::log()->info(
87
"positions numbers: " + std::to_string(plant.num_positions()) +
88
", velocities numbers: " + std::to_string(plant.num_velocities()) +
89
", actuators numbers: " + std::to_string(plant.num_actuators()));
90
91
// Create inverse dynamics controller.
92
const int U = plant.num_actuators();
93
auto IDC =
94
builder
95
.AddSystem<systems::controllers::InverseDynamicsController<double>>(
96
plant, Eigen::VectorXd::Ones(U) * FLAGS_Kp,
97
Eigen::VectorXd::Ones(U) * FLAGS_Ki,
98
Eigen::VectorXd::Ones(U) * FLAGS_Kd, false);
99
builder.Connect(IDC->get_output_port_control(),
100
plant.get_applied_generalized_force_input_port());
101
builder.Connect(plant.get_state_output_port(),
102
IDC->get_input_port_estimated_state());
103
builder.Connect(desired_state_source->get_output_port(),
104
IDC->get_input_port_desired_state());
105
106
// Constant zero load for plant actuation_input_port.
107
auto constant_zero_torque =
108
builder.AddSystem<systems::ConstantVectorSource<double>>(
109
Eigen::VectorXd::Zero(plant.num_actuators()));
110
builder.Connect(constant_zero_torque->get_output_port(),
111
plant.get_actuation_input_port());
112
113
DRAKE_DEMAND(!!plant.get_source_id());
114
builder.Connect(
115
plant.get_geometry_poses_output_port(),
116
scene_graph.get_source_pose_port(plant.get_source_id().value()));
117
builder.Connect(scene_graph.get_query_output_port(),
118
plant.get_geometry_query_input_port());
119
120
geometry::ConnectDrakeVisualizer(&builder, scene_graph);
121
std::unique_ptr<systems::Diagram<double>> diagram = builder.Build();
122
123
// Create a context for this system.
124
std::unique_ptr<systems::Context<double>> diagram_context =
125
diagram->CreateDefaultContext();
126
diagram->SetDefaultContext(diagram_context.get());
127
128
systems::Context<double>& plant_context =
129
diagram->GetMutableSubsystemContext(plant, diagram_context.get());
130
Eigen::VectorXd q = plant.GetPositions(plant_context, plant_index);
131
q[0] = 0.1;
132
q[1] = 0.5;
133
q[2] = 0.3;
134
plant.SetPositions(&plant_context, q);
135
136
// Set up simulator.
137
systems::Simulator<double> simulator(*diagram, std::move(diagram_context));
138
simulator.set_publish_every_time_step(true);
139
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
140
simulator.Initialize();
141
simulator.AdvanceTo(FLAGS_simulation_time);
142
}
143
144
} // namespace kuka
145
} // namespace examples
146
} // namespace drake
147
148
int main(int argc, char* argv[]) {
149
gflags::SetUsageMessage(
150
"bazel run"
151
"//examples/kuka_iiwa_arm_idc:run_kuka_idc");
152
gflags::ParseCommandLineFlags(&argc, &argv, true);
153
drake::examples::kuka::DoMain();
154
return 0;
155
}
Copied!
Add BUILD.bazel
BUILD.bazel
1
load(
2
"@drake//tools/skylark:drake_cc.bzl",
3
"drake_cc_binary",
4
)
5
6
package(
7
default_visibility = [":__subpackages__"],
8
)
9
10
drake_cc_binary(
11
name = "run_kuka_idc",
12
srcs = ["run_kuka_idc_demo.cc"],
13
data = [
14
"//manipulation/models/iiwa_description:models",
15
],
16
deps = [
17
"//common:find_resource",
18
"//common:text_logging_gflags",
19
"//common:type_safe_index",
20
"//geometry:geometry_visualization",
21
"//lcm",
22
"//multibody/parsing",
23
"//multibody/plant",
24
"//systems/analysis",
25
"//systems/controllers",
26
"//systems/primitives:constant_vector_source",
27
"@gflags",
28
],
29
)
Copied!

Run demo

1
bazel-bin/tools/drake_visualizer &
2
bazel run //examples/kuka_iiwa_arm_idc:run_kuka_idc
Copied!
The complete code is here.
1
bazel-bin/tools/drake_visualizer &
2
bazel run //examples/kinova_jaco_arm:run_setpose_jaco_demo
Copied!