Skip to content

Commit

Permalink
Stop assigning names to systems which are no better than GetMemoryObj…
Browse files Browse the repository at this point in the history
…ectName.

This came about because two instances of the resulting systems couldn't live in the same diagram.
  • Loading branch information
sammy-tri committed Nov 16, 2017
1 parent a53aac0 commit f1472c2
Show file tree
Hide file tree
Showing 10 changed files with 22 additions and 47 deletions.
1 change: 0 additions & 1 deletion drake/manipulation/perception/optitrack_pose_extractor.cc
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@ OptitrackPoseExtractor::OptitrackPoseExtractor(
&OptitrackPoseExtractor::OutputMeasuredPose)
.get_index()},
X_WO_(X_WO) {
this->set_name("Optitrack pose extractor");
DeclareAbstractState(
systems::AbstractValue::Make<Isometry3<double>>(
Isometry3<double>::Identity()));
Expand Down
1 change: 0 additions & 1 deletion drake/manipulation/perception/pose_smoother.cc
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,6 @@ PoseSmoother::PoseSmoother(double desired_max_linear_velocity,
max_linear_velocity_(desired_max_linear_velocity),
max_angular_velocity_(desired_max_angular_velocity),
is_filter_enabled_(filter_window_size > 1) {
this->set_name("Pose Smoother");
this->DeclareAbstractState(
systems::AbstractValue::Make<InternalState>(
InternalState(filter_window_size)));
Expand Down
1 change: 0 additions & 1 deletion drake/manipulation/planner/robot_plan_interpolator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,6 @@ RobotPlanInterpolator::RobotPlanInterpolator(
double update_interval)
: plan_input_port_(this->DeclareAbstractInputPort().get_index()),
interp_type_(interp_type) {
this->set_name("RobotPlanInterpolator");
parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
model_path, multibody::joints::kFixed, &tree_);
// TODO(sam.creasey) This implementation doesn't know how to
Expand Down
1 change: 0 additions & 1 deletion drake/manipulation/schunk_wsg/schunk_wsg_controller.cc
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,6 @@ SchunkWsgController::SchunkWsgController() {
saturation->get_min_value_port());
builder.ExportOutput(saturation->get_output_port());
builder.BuildInto(this);
set_name("SchunkWsgController");
}

} // namespace schunk_wsg
Expand Down
2 changes: 0 additions & 2 deletions drake/manipulation/schunk_wsg/schunk_wsg_lcm.cc
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@ SchunkWsgTrajectoryGenerator::SchunkWsgTrajectoryGenerator(
this->DeclareVectorOutputPort(
BasicVector<double>(1),
&SchunkWsgTrajectoryGenerator::OutputForce).get_index()) {
this->set_name("SchunkWsgTrajectoryGenerator");
this->DeclareAbstractInputPort();
this->DeclareInputPort(systems::kVectorValued, input_size);
// The update period below matches the polling rate from
Expand Down Expand Up @@ -182,7 +181,6 @@ SchunkWsgStatusSender::
SchunkWsgStatusSender(int input_size,
int position_index, int velocity_index)
: position_index_(position_index), velocity_index_(velocity_index) {
this->set_name("SchunkWsgStatusSender");
this->DeclareInputPort(systems::kVectorValued, input_size);
this->DeclareAbstractOutputPort(&SchunkWsgStatusSender::OutputStatus);
}
Expand Down
3 changes: 0 additions & 3 deletions drake/manipulation/util/sim_diagram_builder.cc
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,6 @@ systems::DrakeVisualizer* SimDiagramBuilder<T>::AddVisualizer(

visualizer_ = builder_.template AddSystem<systems::DrakeVisualizer>(
plant_->get_rigid_body_tree(), lcm);
visualizer_->set_name("visualizer");

return visualizer_;
}

Expand All @@ -25,7 +23,6 @@ systems::RigidBodyPlant<T>* SimDiagramBuilder<T>::AddPlant(
DRAKE_DEMAND(plant_ == nullptr);
plant_ =
builder_.template AddSystem<systems::RigidBodyPlant<T>>(std::move(plant));
plant_->set_name("plant");
return plant_;
}

Expand Down
6 changes: 0 additions & 6 deletions drake/systems/controllers/inverse_dynamics_controller.cc
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@ void InverseDynamicsController<T>::SetUp(const VectorX<double>& kp,
const VectorX<double>& ki,
const VectorX<double>& kd) {
DiagramBuilder<T> builder;
this->set_name("InverseDynamicsController");

const RigidBodyTree<T>& robot = *robot_for_control_;
DRAKE_DEMAND(robot.get_num_positions() == kp.size());
Expand All @@ -43,20 +42,16 @@ void InverseDynamicsController<T>::SetUp(const VectorX<double>& kp,

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

// Adds inverse dynamics.
auto inverse_dynamics =
builder.template AddSystem<InverseDynamics<T>>(robot, false);
inverse_dynamics->set_name("inverse_dynamics");

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

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

// Connects estimated state to PID.
builder.Connect(pass_through->get_output_port(),
Expand Down Expand Up @@ -86,7 +81,6 @@ void InverseDynamicsController<T>::SetUp(const VectorX<double>& kp,
auto zero_feedforward_acceleration =
builder.template AddSystem<ConstantVectorSource<T>>(
VectorX<T>::Zero(robot.get_num_velocities()));
zero_feedforward_acceleration->set_name("zero");
builder.Connect(zero_feedforward_acceleration->get_output_port(),
adder->get_input_port(1));
} else {
Expand Down
10 changes: 0 additions & 10 deletions drake/systems/controllers/pid_controlled_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -58,10 +58,6 @@ void PidControlledSystem<T>::Initialize(
const Eigen::VectorXd& Ki, const Eigen::VectorXd& Kd) {
DRAKE_DEMAND(plant != nullptr);

if (plant->get_name().empty()) {
plant->set_name("plant");
}

DiagramBuilder<T> builder;
plant_ = builder.template AddSystem(std::move(plant));
DRAKE_ASSERT(plant_->get_num_input_ports() >= 1);
Expand All @@ -88,17 +84,13 @@ PidControlledSystem<T>::ConnectController(
auto controller = builder->template AddSystem<PidController<T>>(
feedback_selector,
Kp, Ki, Kd);
controller->set_name("pid_controller");

auto plant_input_adder =
builder->template AddSystem<Adder<T>>(2, plant_input.size());
plant_input_adder->set_name("input_adder");

builder->Connect(plant_output, controller->get_input_port_estimated_state());

builder->Connect(controller->get_output_port_control(),
plant_input_adder->get_input_port(0));

builder->Connect(plant_input_adder->get_output_port(), plant_input);

return ConnectResult{
Expand Down Expand Up @@ -130,8 +122,6 @@ PidControlledSystem<T>::ConnectControllerWithInputSaturation(
DiagramBuilder<T>* builder) {
auto saturation = builder->template AddSystem<Saturation<T>>(
min_plant_input, max_plant_input);
saturation->set_name("saturation");

builder->Connect(saturation->get_output_port(), plant_input);

return
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,6 @@ QpInverseDynamicsSystem::QpInverseDynamicsSystem(
DeclareAbstractOutputPort(lcmt_inverse_dynamics_debug_info(),
&QpInverseDynamicsSystem::CopyOutDebugInfo)
.get_index();

set_name("QpInverseDynamicsSystem");
DeclarePeriodicUnrestrictedUpdate(control_dt_, 0);

abs_state_index_qp_output_ = DeclareAbstractState(
Expand Down
42 changes: 22 additions & 20 deletions drake/systems/controllers/test/pid_controlled_system_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -123,8 +123,6 @@ class PidControlledSystemTest : public ::testing::Test {

auto controller = builder.AddSystem<PidControlledSystem>(
std::move(plant), feedback_selector, Kp_, Ki_, Kd_);
// Check that the controller automatically assigns a name to the plant.
controller->set_name("controller");

builder.Connect(input_source->get_output_port(),
controller->get_input_port(0));
Expand Down Expand Up @@ -154,18 +152,6 @@ class PidControlledSystemTest : public ::testing::Test {
std::unique_ptr<Diagram<double>> diagram_;
};

// Tests that the PidController assigns default names to the plant.
TEST_F(PidControlledSystemTest, DefaultNamesAssigned) {
auto plant = std::make_unique<TestPlantWithMinOutputs>();
auto plant_ptr = plant.get();
const int state_size = plant->get_output_port(0).size();

PidControlledSystem<double> controller(
std::move(plant), MatrixX<double>::Identity(state_size, state_size),
Kp_, Ki_, Kd_);
EXPECT_EQ("plant", plant_ptr->get_name());
}

// Tests that the PidController preserves the names of the plant.
TEST_F(PidControlledSystemTest, ExistingNamesRespected) {
auto plant = std::make_unique<TestPlantWithMinOutputs>();
Expand Down Expand Up @@ -217,22 +203,18 @@ class ConnectControllerTest : public ::testing::Test {
plant_ptr->get_output_port(0).size());

plant_ = builder_.AddSystem(std::move(plant_ptr));
plant_->set_name("plant");

input_source_ = builder_.AddSystem<ConstantVectorSource>(plant_input_);
input_source_->set_name("input");
state_source_ = builder_.AddSystem<ConstantVectorSource>(desired_state_);
state_source_->set_name("state");

builder_.ExportOutput(plant_->get_output_port(0));
}

void ConnectPidPorts(
PidControlledSystem<double>::ConnectResult plant_pid_ports) {
builder_.Connect(input_source_->get_output_port(),
plant_pid_ports.control_input_port);
plant_pid_ports.control_input_port);
builder_.Connect(state_source_->get_output_port(),
plant_pid_ports.state_input_port);
plant_pid_ports.state_input_port);
}

double ComputePidInput() {
Expand Down Expand Up @@ -306,6 +288,26 @@ TEST_F(ConnectControllerTest, SaturatingController) {
EXPECT_EQ(pid_input, calculated_input);
}

// Ensure that multiple plants can coexist in the same diagram (yes,
// this was broken at one point).
TEST_F(ConnectControllerTest, MultipleControllerTest) {
auto plant_pid_ports = PidControlledSystem<double>::ConnectController(
plant_->get_input_port(0), plant_->get_output_port(0),
feedback_selector_, Kp, Ki, Kd, &builder_);

ConnectPidPorts(plant_pid_ports);

// Add another PidControlledSystem and confirm there's nothing that
// prevents two plants from existing in the same diagram.
SetUp();
auto second_plant_pid_ports = PidControlledSystem<double>::ConnectController(
plant_->get_input_port(0), plant_->get_output_port(0),
feedback_selector_, Kp, Ki, Kd, &builder_);

ConnectPidPorts(second_plant_pid_ports);
EXPECT_NO_THROW(builder_.Build());
}

} // namespace
} // namespace controllers
} // namespace systems
Expand Down

0 comments on commit f1472c2

Please sign in to comment.