Skip to content

Commit

Permalink
Add a construtor to allow IDC to own the input plant (RobotLocomotion…
Browse files Browse the repository at this point in the history 
…#14037)
  • Loading branch information
@huihuaTRI
huihuaTRI authored Sep 10, 2020
1 parent 5c5fc08 commit 0cc01f0
Show file tree
Hide file tree
Showing 4 changed files with 165 additions and 102 deletions.
3 changes: 2 additions & 1 deletion bindings/pydrake/systems/controllers_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,8 @@ PYBIND11_MODULE(controllers, m) {
py::arg("robot"), py::arg("kp"), py::arg("ki"), py::arg("kd"),
py::arg("has_reference_acceleration"),
// Keep alive, reference: `self` keeps `robot` alive.
py::keep_alive<1, 2>(), doc.InverseDynamicsController.ctor.doc)
py::keep_alive<1, 2>(),
doc.InverseDynamicsController.ctor.doc_5args_referenced_plant)
.def("set_integral_value",
&InverseDynamicsController<double>::set_integral_value,
doc.InverseDynamicsController.set_integral_value.doc)
Expand Down
83 changes: 46 additions & 37 deletions systems/controllers/inverse_dynamics_controller.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,21 @@ namespace controllers {

template <typename T>
void InverseDynamicsController<T>::SetUp(const VectorX<double>& kp,
const VectorX<double>& ki, const VectorX<double>& kd,
const InverseDynamics<T>& inverse_dynamics, DiagramBuilder<T>* builder) {
const VectorX<double>& ki,
const VectorX<double>& kd) {
DRAKE_DEMAND(multibody_plant_for_control_->is_finalized());

DiagramBuilder<T> builder;
auto inverse_dynamics = builder.template AddSystem<InverseDynamics<T>>(
multibody_plant_for_control_, InverseDynamics<T>::kInverseDynamics);

const int num_positions = multibody_plant_for_control_->num_positions();
const int num_velocities = multibody_plant_for_control_->num_velocities();
const int num_actuators = multibody_plant_for_control_->num_actuators();
const int dim = kp.size();
DRAKE_DEMAND(num_positions == dim);
DRAKE_DEMAND(num_positions == num_velocities);
DRAKE_DEMAND(num_positions == num_actuators);

/*
(vd*)
Expand All @@ -36,55 +48,55 @@ void InverseDynamicsController<T>::SetUp(const VectorX<double>& kp,
*/

// Adds a PID.
pid_ = builder->template AddSystem<PidController<T>>(kp, ki, kd);
pid_ = builder.template AddSystem<PidController<T>>(kp, ki, kd);

// Redirects estimated state input into PID and inverse dynamics.
auto pass_through = builder->template AddSystem<PassThrough<T>>(2 * dim);
auto pass_through = builder.template AddSystem<PassThrough<T>>(2 * dim);

// Adds a adder to do PID's acceleration + reference acceleration.
auto adder = builder->template AddSystem<Adder<T>>(2, dim);
auto adder = builder.template AddSystem<Adder<T>>(2, dim);

// Connects estimated state to PID.
builder->Connect(pass_through->get_output_port(),
pid_->get_input_port_estimated_state());
builder.Connect(pass_through->get_output_port(),
pid_->get_input_port_estimated_state());

// Connects estimated state to inverse dynamics.
builder->Connect(pass_through->get_output_port(),
inverse_dynamics.get_input_port_estimated_state());
builder.Connect(pass_through->get_output_port(),
inverse_dynamics->get_input_port_estimated_state());

// Adds PID's output with reference acceleration
builder->Connect(pid_->get_output_port_control(), adder->get_input_port(0));
builder.Connect(pid_->get_output_port_control(), adder->get_input_port(0));

// Connects desired acceleration to inverse dynamics
builder->Connect(adder->get_output_port(),
inverse_dynamics.get_input_port_desired_acceleration());
builder.Connect(adder->get_output_port(),
inverse_dynamics->get_input_port_desired_acceleration());

// Exposes estimated state input port.
input_port_index_estimated_state_ =
builder->ExportInput(pass_through->get_input_port(), "estimated_state");
builder.ExportInput(pass_through->get_input_port(), "estimated_state");

// Exposes reference state input port.
input_port_index_desired_state_ = builder->ExportInput(
input_port_index_desired_state_ = builder.ExportInput(
pid_->get_input_port_desired_state(), "desired_state");

if (!has_reference_acceleration_) {
// Uses a zero constant source for reference acceleration.
auto zero_feedforward_acceleration =
builder->template AddSystem<ConstantVectorSource<T>>(
builder.template AddSystem<ConstantVectorSource<T>>(
VectorX<T>::Zero(dim));
builder->Connect(zero_feedforward_acceleration->get_output_port(),
adder->get_input_port(1));
builder.Connect(zero_feedforward_acceleration->get_output_port(),
adder->get_input_port(1));
} else {
// Exposes reference acceleration input port.
input_port_index_desired_acceleration_ =
builder->ExportInput(adder->get_input_port(1), "desired_acceleration");
builder.ExportInput(adder->get_input_port(1), "desired_acceleration");
}

// Exposes inverse dynamics' output force port.
output_port_index_control_ =
builder->ExportOutput(inverse_dynamics.get_output_port_force(), "force");
builder.ExportOutput(inverse_dynamics->get_output_port_force(), "force");

builder->BuildInto(this);
builder.BuildInto(this);
}

template <typename T>
Expand All @@ -97,26 +109,23 @@ void InverseDynamicsController<T>::set_integral_value(

template <typename T>
InverseDynamicsController<T>::InverseDynamicsController(
const MultibodyPlant<T>& plant,
const VectorX<double>& kp, const VectorX<double>& ki,
const VectorX<double>& kd, bool has_reference_acceleration)
const MultibodyPlant<T>& plant, const VectorX<double>& kp,
const VectorX<double>& ki, const VectorX<double>& kd,
bool has_reference_acceleration)
: multibody_plant_for_control_(&plant),
has_reference_acceleration_(has_reference_acceleration) {
DRAKE_DEMAND(plant.is_finalized());

DiagramBuilder<T> builder;
auto inverse_dynamics =
builder.template AddSystem<InverseDynamics<T>>(
multibody_plant_for_control_,
InverseDynamics<T>::kInverseDynamics);
SetUp(kp, ki, kd);
}

const int num_positions = multibody_plant_for_control_->num_positions();
const int num_velocities = multibody_plant_for_control_->num_velocities();
const int num_actuators = multibody_plant_for_control_->num_actuators();
DRAKE_DEMAND(num_positions == kp.size());
DRAKE_DEMAND(num_positions == num_velocities);
DRAKE_DEMAND(num_positions == num_actuators);
SetUp(kp, ki, kd, *inverse_dynamics, &builder);
template <typename T>
InverseDynamicsController<T>::InverseDynamicsController(
std::unique_ptr<multibody::MultibodyPlant<T>> plant,
const VectorX<double>& kp, const VectorX<double>& ki,
const VectorX<double>& kd, bool has_reference_acceleration)
: owned_plant_for_control_(std::move(plant)),
multibody_plant_for_control_(owned_plant_for_control_.get()),
has_reference_acceleration_(has_reference_acceleration) {
SetUp(kp, ki, kd);
}

template <typename T>
Expand Down
20 changes: 17 additions & 3 deletions systems/controllers/inverse_dynamics_controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ class InverseDynamicsController : public Diagram<T>,
* - The model is not fully actuated.
* - Vector kp, ki and kd do not all have the same size equal to the number
* of generalized positions.
*
* @pydrake_mkdoc_identifier{5args_referenced_plant}
*/
InverseDynamicsController(
const multibody::MultibodyPlant<T>& plant,
Expand All @@ -90,6 +92,18 @@ class InverseDynamicsController : public Diagram<T>,
const VectorX<double>& kd,
bool has_reference_acceleration);

/**
* Constructs an inverse dynamics controller and takes the ownership of the
* input `plant`.
*
* @pydrake_mkdoc_identifier{5args_owned_plant}
*/
InverseDynamicsController(std::unique_ptr<multibody::MultibodyPlant<T>> plant,
const VectorX<double>& kp,
const VectorX<double>& ki,
const VectorX<double>& kd,
bool has_reference_acceleration);

~InverseDynamicsController() override;

/**
Expand Down Expand Up @@ -138,10 +152,10 @@ class InverseDynamicsController : public Diagram<T>,

private:
void SetUp(const VectorX<double>& kp, const VectorX<double>& ki,
const VectorX<double>& kd,
const controllers::InverseDynamics<T>& inverse_dynamics,
DiagramBuilder<T>* diagram_builder);
const VectorX<double>& kd);

const std::unique_ptr<multibody::MultibodyPlant<T>>
owned_plant_for_control_{};
const multibody::MultibodyPlant<T>* multibody_plant_for_control_{nullptr};
PidController<T>* pid_{nullptr};
const bool has_reference_acceleration_{false};
Expand Down
161 changes: 100 additions & 61 deletions systems/controllers/test/inverse_dynamics_controller_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,82 +14,121 @@ namespace systems {
namespace controllers {
namespace {

class InverseDynamicsControllerTest : public ::testing::Test {
protected:
void SetPidGains(VectorX<double>* kp, VectorX<double>* ki,
VectorX<double>* kd) {
*kp << 1, 2, 3, 4, 5, 6, 7;
*ki << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
*kd = *kp / 2.;
}

void ConfigTestAndCheck(InverseDynamicsController<double>* test_sys,
const VectorX<double>& kp, const VectorX<double>& ki,
const VectorX<double>& kd) {
auto inverse_dynamics_context = test_sys->CreateDefaultContext();
auto output = test_sys->AllocateOutput();
const MultibodyPlant<double>& robot_plant =
*test_sys->get_multibody_plant_for_control();

// Sets current state and reference state and acceleration values.
const int dim = robot_plant.num_positions();
VectorX<double> q(dim), v(dim), q_r(dim), v_r(dim), vd_r(dim);
q << 0.3, 0.2, 0.1, 0, -0.1, -0.2, -0.3;
v = q * 3;

q_r = (q + VectorX<double>::Constant(dim, 0.1)) * 2.;
v_r.setZero();
vd_r << 1, 2, 3, 4, 5, 6, 7;

// Connects inputs.
VectorX<double> state_input(robot_plant.num_positions() +
robot_plant.num_velocities());
state_input << q, v;

VectorX<double> reference_state_input(robot_plant.num_positions() +
robot_plant.num_velocities());
reference_state_input << q_r, v_r;

VectorX<double> reference_acceleration_input(robot_plant.num_velocities());
reference_acceleration_input << vd_r;

test_sys->get_input_port_estimated_state().FixValue(
inverse_dynamics_context.get(), state_input);
test_sys->get_input_port_desired_state().FixValue(
inverse_dynamics_context.get(), reference_state_input);
test_sys->get_input_port_desired_acceleration().FixValue(
inverse_dynamics_context.get(), reference_acceleration_input);

// Sets integrated position error.
VectorX<double> q_int(dim);
q_int << -1, -2, -3, -4, -5, -6, -7;
test_sys->set_integral_value(inverse_dynamics_context.get(), q_int);

// Computes output.
test_sys->CalcOutput(*inverse_dynamics_context, output.get());

// The results should equal to this.
VectorX<double> vd_d = (kp.array() * (q_r - q).array()).matrix() +
(kd.array() * (v_r - v).array()).matrix() +
(ki.array() * q_int.array()).matrix() + vd_r;

auto robot_context = robot_plant.CreateDefaultContext();
VectorX<double> expected_torque = controllers_test::ComputeTorque(
robot_plant, q, v, vd_d, robot_context.get());

// Checks the expected and computed gravity torque.
const BasicVector<double>* output_vector = output->get_vector_data(0);
EXPECT_TRUE(CompareMatrices(expected_torque, output_vector->get_value(),
1e-10, MatrixCompareType::absolute));
}
};

// Tests the computed torque from InverseDynamicsController matches hand
// derived results for the kuka iiwa arm at a given state (q, v), when
// asked to track reference state (q_r, v_r) and reference acceleration (vd_r).
GTEST_TEST(InverseDynamicsControllerTest, TestTorque) {
// This test verifies the case that inverse dynamics controller only references
// the input robot plant.
TEST_F(InverseDynamicsControllerTest, TestTorqueWithReferencedPlant) {
auto robot = std::make_unique<MultibodyPlant<double>>(0.0);
const std::string full_name = drake::FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf");
multibody::Parser(robot.get()).AddModelFromFile(full_name);
robot->WeldFrames(robot->world_frame(),
robot->GetFrameByName("iiwa_link_0"));
robot->WeldFrames(robot->world_frame(), robot->GetFrameByName("iiwa_link_0"));
robot->Finalize();
auto robot_context = robot->CreateDefaultContext();

// Sets pid gains.
const int dim = robot->num_positions();
VectorX<double> kp(dim), ki(dim), kd(dim);
kp << 1, 2, 3, 4, 5, 6, 7;
ki << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
kd = kp / 2.;
SetPidGains(&kp, &ki, &kd);

auto dut = std::make_unique<InverseDynamicsController<double>>(
*robot, kp, ki, kd, true /* expose reference acceleration port */);
auto inverse_dynamics_context = dut->CreateDefaultContext();
auto output = dut->AllocateOutput();
const MultibodyPlant<double>& robot_plant =
*dut->get_multibody_plant_for_control();

// Sets current state and reference state and acceleration values.
VectorX<double> q(dim), v(dim), q_r(dim), v_r(dim), vd_r(dim);
q << 0.3, 0.2, 0.1, 0, -0.1, -0.2, -0.3;
v = q * 3;

q_r = (q + VectorX<double>::Constant(dim, 0.1)) * 2.;
v_r.setZero();
vd_r << 1, 2, 3, 4, 5, 6, 7;

// Connects inputs.
VectorX<double> state_input(robot_plant.num_positions() +
robot_plant.num_velocities());
state_input << q, v;

VectorX<double> reference_state_input(robot_plant.num_positions() +
robot_plant.num_velocities());
reference_state_input << q_r, v_r;

VectorX<double> reference_acceleration_input(robot_plant.num_velocities());
reference_acceleration_input << vd_r;

dut->get_input_port_estimated_state().FixValue(inverse_dynamics_context.get(),
state_input);
dut->get_input_port_desired_state().FixValue(inverse_dynamics_context.get(),
reference_state_input);
dut->get_input_port_desired_acceleration().FixValue(
inverse_dynamics_context.get(), reference_acceleration_input);

// Sets integrated position error.
VectorX<double> q_int(dim);
q_int << -1, -2, -3, -4, -5, -6, -7;
dut->set_integral_value(inverse_dynamics_context.get(), q_int);

// Computes output.
dut->CalcOutput(*inverse_dynamics_context, output.get());

// The results should equal to this.
VectorX<double> vd_d = (kp.array() * (q_r - q).array()).matrix() +
(kd.array() * (v_r - v).array()).matrix() +
(ki.array() * q_int.array()).matrix() + vd_r;

VectorX<double> expected_torque =
controllers_test::ComputeTorque(robot_plant, q, v, vd_d,
robot_context.get());

// Checks the expected and computed gravity torque.
const BasicVector<double>* output_vector = output->get_vector_data(0);
EXPECT_TRUE(CompareMatrices(expected_torque, output_vector->get_value(),
1e-10, MatrixCompareType::absolute));

ConfigTestAndCheck(dut.get(), kp, ki, kd);
}

// Tests the computed torque. This test is similar to the previous test. The
// difference is that the inverse dynamics controller is created by owning the
// input robot plant.
TEST_F(InverseDynamicsControllerTest, TestTorqueWithOwnedPlant) {
auto robot = std::make_unique<MultibodyPlant<double>>(0.0);
const std::string full_name = drake::FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf");
multibody::Parser(robot.get()).AddModelFromFile(full_name);
robot->WeldFrames(robot->world_frame(), robot->GetFrameByName("iiwa_link_0"));
robot->Finalize();

// Sets pid gains.
const int dim = robot->num_positions();
VectorX<double> kp(dim), ki(dim), kd(dim);
SetPidGains(&kp, &ki, &kd);

auto dut = std::make_unique<InverseDynamicsController<double>>(
std::move(robot), kp, ki, kd,
true /* expose reference acceleration port */);

ConfigTestAndCheck(dut.get(), kp, ki, kd);
}


Expand Down

0 comments on commit 0cc01f0

Please sign in to comment.