Skip to content

Commit

Permalink
Name ports and add @System documentation for InverseDynamicsController (
Browse files Browse the repository at this point in the history
  • Loading branch information
RussTedrake authored Aug 16, 2020
1 parent 042c03b commit 0de592f
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 6 deletions.
10 changes: 5 additions & 5 deletions systems/controllers/inverse_dynamics_controller.cc
Original file line number Diff line number Diff line change
Expand Up @@ -61,11 +61,11 @@ void InverseDynamicsController<T>::SetUp(const VectorX<double>& kp,

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

// Exposes reference state input port.
input_port_index_desired_state_ =
builder->ExportInput(pid_->get_input_port_desired_state());
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.
Expand All @@ -77,12 +77,12 @@ void InverseDynamicsController<T>::SetUp(const VectorX<double>& kp,
} else {
// Exposes reference acceleration input port.
input_port_index_desired_acceleration_ =
builder->ExportInput(adder->get_input_port(1));
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());
builder->ExportOutput(inverse_dynamics.get_output_port_force(), "force");

builder->BuildInto(this);
}
Expand Down
12 changes: 11 additions & 1 deletion systems/controllers/inverse_dynamics_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,16 @@ namespace controllers {
* `q` and `v` stand for the generalized position and velocity, and `vd` is
* the generalized acceleration. `*` indicates reference values.
*
* @system
* name: InverseDynamicsController
* input_ports:
* - estimated_state
* - desired_state
* - desired_acceleration*
* output_ports:
* - force
* @endsystem
*
* This controller always has a BasicVector input port for estimated robot state
* `(q, v)`, a BasicVector input port for reference robot state `(q*, v*)` and
* a BasicVector output port for computed torque `torque`. A constructor flag
Expand All @@ -36,7 +46,7 @@ namespace controllers {
* Note that this class assumes the robot is fully actuated, its position
* and velocity have the same dimension, and it does not have a floating base.
* If violated, the program will abort. This controller was not designed for
* closed loop systems: the controller accounts for neither constraint forces
* closed-loop systems: the controller accounts for neither constraint forces
* nor actuator forces applied at loop constraints. Use on such systems is not
* recommended.
*
Expand Down

0 comments on commit 0de592f

Please sign in to comment.