Skip to content

Commit

Permalink
adds annotation to InputPortDescriptor for inputs that are anticipate…
Browse files Browse the repository at this point in the history
…d as random noise/disturbances, as discussed in RobotLocomotion#6374.  pipes optional input through System, LeafSystem, and DiagramBuilder/Diagram, and adds basic unit test coverage.
  • Loading branch information
RussTedrake committed Sep 10, 2017
1 parent b46f931 commit a4bff5b
Show file tree
Hide file tree
Showing 9 changed files with 105 additions and 13 deletions.
3 changes: 2 additions & 1 deletion drake/systems/framework/diagram.h
Original file line number Diff line number Diff line change
Expand Up @@ -1406,7 +1406,8 @@ class Diagram : public System<T>,
// Add this port to our externally visible topology.
const auto& subsystem_descriptor = sys->get_input_port(port_index);
this->DeclareInputPort(subsystem_descriptor.get_data_type(),
subsystem_descriptor.size());
subsystem_descriptor.size(),
subsystem_descriptor.get_random_type());
}

// Exposes the given subsystem output port as an output of the Diagram.
Expand Down
5 changes: 5 additions & 0 deletions drake/systems/framework/diagram_builder.h
Original file line number Diff line number Diff line change
Expand Up @@ -323,6 +323,11 @@ class DiagramBuilder {
}
ThrowIfAlgebraicLoopsExist();

// TODO(russt): Consider automatically exporting any random input ports
// that have not been explicitly exported. This would be especially
// useful if tools like Simulator automatically add RandomSource blocks
// to any open random inputs declared on the system.

auto blueprint = std::make_unique<typename Diagram<T>::Blueprint>();
blueprint->input_port_ids = input_port_ids_;
blueprint->output_port_ids = output_port_ids_;
Expand Down
18 changes: 16 additions & 2 deletions drake/systems/framework/input_port_descriptor.h
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,23 @@ class InputPortDescriptor {
/// @param data_type Whether the port described is vector or abstract valued.
/// @param size If the port described is vector-valued, the number of
/// elements, or kAutoSize if determined by connections.
/// @param random_type Defines the anticipated distribution if the port is
/// intended to model a random-source "noise" or
/// "disturbance" input (default: kNotRandom).
InputPortDescriptor(const System<T>* system, int index,
PortDataType data_type, int size)
: system_(system), index_(index), data_type_(data_type), size_(size) {
PortDataType data_type, int size,
RandomPortType random_type = kNotRandom)
: system_(system),
index_(index),
data_type_(data_type),
size_(size),
random_type_(random_type) {
if (size_ == kAutoSize) {
DRAKE_ABORT_MSG("Auto-size ports are not yet implemented.");
}
if (is_random() && data_type_ != kVectorValued) {
DRAKE_ABORT_MSG("Random input ports must be vector valued.");
}
}

/// @name Basic Concepts
Expand Down Expand Up @@ -57,12 +68,15 @@ class InputPortDescriptor {
int get_index() const { return index_; }
PortDataType get_data_type() const { return data_type_; }
int size() const { return size_; }
bool is_random() const { return random_type_ != kNotRandom; }
RandomPortType get_random_type() const { return random_type_; }

private:
const System<T>* const system_;
const int index_;
const PortDataType data_type_;
const int size_;
const RandomPortType random_type_;
};

} // namespace systems
Expand Down
9 changes: 6 additions & 3 deletions drake/systems/framework/leaf_system.h
Original file line number Diff line number Diff line change
Expand Up @@ -707,13 +707,16 @@ class LeafSystem : public System<T> {
/// This is the best way to declare LeafSystem input ports that require
/// subclasses of BasicVector. The port's size will be model_vector.size(),
/// and LeafSystem's default implementation of DoAllocateInputVector will be
/// model_vector.Clone().
/// model_vector.Clone(). If the port is intended to model a random noise or
/// disturbance input, @p random_type can (optionally) be used to label it
/// as such.
const InputPortDescriptor<T>& DeclareVectorInputPort(
const BasicVector<T>& model_vector) {
const BasicVector<T>& model_vector,
RandomPortType random_type = kNotRandom) {
const int size = model_vector.size();
const int next_index = this->get_num_input_ports();
model_input_values_.AddVectorModel(next_index, model_vector.Clone());
return this->DeclareInputPort(kVectorValued, size);
return this->DeclareInputPort(kVectorValued, size, random_type);
}

// Avoid shadowing out the no-arg DeclareAbstractInputPort().
Expand Down
9 changes: 6 additions & 3 deletions drake/systems/framework/system.h
Original file line number Diff line number Diff line change
Expand Up @@ -1267,11 +1267,14 @@ class System {
System() {}

/// Adds a port with the specified @p type and @p size to the input topology.
/// If the port is intended to model a random noise or disturbance input,
/// @p random_type can (optionally) be used to label it as such.
/// @return descriptor of declared port.
const InputPortDescriptor<T>& DeclareInputPort(PortDataType type, int size) {
const InputPortDescriptor<T>& DeclareInputPort(
PortDataType type, int size, RandomPortType random_type = kNotRandom) {
int port_index = get_num_input_ports();
input_ports_.push_back(
std::make_unique<InputPortDescriptor<T>>(this, port_index, type, size));
input_ports_.push_back(std::make_unique<InputPortDescriptor<T>>(
this, port_index, type, size, random_type));
return *input_ports_.back();
}

Expand Down
12 changes: 12 additions & 0 deletions drake/systems/framework/system_common.h
Original file line number Diff line number Diff line change
Expand Up @@ -12,5 +12,17 @@ typedef enum {
kAbstractValued = 1,
} PortDataType;

/// Some input ports may be specifically labeled as "random"; these are
/// intended to model random noise/disturbance inputs to systems and
/// are *often*, but not exclusively, wired up to RandomSource systems.
typedef enum {
kNotRandom = 0,
kUniformRandom = 1, /// Anticipated vector elements are independent and
/// uniformly distributed ∈ [0,1].
kGaussianRandom = 2, /// Anticipated vector elements are independent and
/// drawn from a mean-zero, unit-variance normal
/// distribution.
} RandomPortType;

} // namespace systems
} // namespace drake
22 changes: 22 additions & 0 deletions drake/systems/framework/test/diagram_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -830,6 +830,28 @@ GTEST_TEST(PortDependentFeedthroughTest, DetectFeedthrough) {
EXPECT_TRUE(diagram->HasDirectFeedthrough(0, 0));
}

// A system with a random source inputs.
class RandomInputSystem : public LeafSystem<double> {
public:
RandomInputSystem() {
this->DeclareInputPort(kVectorValued, 1);
this->DeclareInputPort(kVectorValued, 1, kUniformRandom);
this->DeclareInputPort(kVectorValued, 1, kGaussianRandom);
}
};

GTEST_TEST(RandomInputSystemTest, RandomInputTest) {
DiagramBuilder<double> builder;
auto random = builder.AddSystem<RandomInputSystem>();
builder.ExportInput(random->get_input_port(0));
builder.ExportInput(random->get_input_port(1));
builder.ExportInput(random->get_input_port(2));
auto diagram = builder.Build();
EXPECT_EQ(diagram->get_input_port(0).get_random_type(), kNotRandom);
EXPECT_EQ(diagram->get_input_port(1).get_random_type(), kUniformRandom);
EXPECT_EQ(diagram->get_input_port(2).get_random_type(), kGaussianRandom);
}

// A vector with a scalar configuration and scalar velocity.
class SecondOrderStateVector : public BasicVector<double> {
public:
Expand Down
22 changes: 21 additions & 1 deletion drake/systems/framework/test/leaf_system_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -380,6 +380,8 @@ class DeclaredModelPortsSystem : public LeafSystem<double> {
this->DeclareInputPort(kVectorValued, 1);
this->DeclareVectorInputPort(MyVector2d());
this->DeclareAbstractInputPort(Value<int>(22));
this->DeclareVectorInputPort(MyVector2d(), kUniformRandom);
this->DeclareVectorInputPort(MyVector2d(), kGaussianRandom);

// Output port 0 uses a BasicVector base class model.
this->DeclareVectorOutputPort(BasicVector<double>(3),
Expand Down Expand Up @@ -443,12 +445,14 @@ class DeclaredModelPortsSystem : public LeafSystem<double> {
GTEST_TEST(ModelLeafSystemTest, ModelPortsTopology) {
DeclaredModelPortsSystem dut;

ASSERT_EQ(dut.get_num_input_ports(), 3);
ASSERT_EQ(dut.get_num_input_ports(), 5);
ASSERT_EQ(dut.get_num_output_ports(), 4);

const InputPortDescriptor<double>& in0 = dut.get_input_port(0);
const InputPortDescriptor<double>& in1 = dut.get_input_port(1);
const InputPortDescriptor<double>& in2 = dut.get_input_port(2);
const InputPortDescriptor<double>& in3 = dut.get_input_port(3);
const InputPortDescriptor<double>& in4 = dut.get_input_port(4);

const OutputPort<double>& out0 = dut.get_output_port(0);
const OutputPort<double>& out1 = dut.get_output_port(1);
Expand All @@ -458,6 +462,8 @@ GTEST_TEST(ModelLeafSystemTest, ModelPortsTopology) {
EXPECT_EQ(in0.get_data_type(), kVectorValued);
EXPECT_EQ(in1.get_data_type(), kVectorValued);
EXPECT_EQ(in2.get_data_type(), kAbstractValued);
EXPECT_EQ(in3.get_data_type(), kVectorValued);
EXPECT_EQ(in4.get_data_type(), kVectorValued);

EXPECT_EQ(out0.get_data_type(), kVectorValued);
EXPECT_EQ(out1.get_data_type(), kVectorValued);
Expand All @@ -466,10 +472,24 @@ GTEST_TEST(ModelLeafSystemTest, ModelPortsTopology) {

EXPECT_EQ(in0.size(), 1);
EXPECT_EQ(in1.size(), 2);
EXPECT_EQ(in3.size(), 2);
EXPECT_EQ(in4.size(), 2);

EXPECT_EQ(out0.size(), 3);
EXPECT_EQ(out1.size(), 4);
EXPECT_EQ(out3.size(), 2);

EXPECT_FALSE(in0.is_random());
EXPECT_FALSE(in1.is_random());
EXPECT_FALSE(in2.is_random());
EXPECT_TRUE(in3.is_random());
EXPECT_TRUE(in4.is_random());

EXPECT_EQ(in0.get_random_type(), kNotRandom);
EXPECT_EQ(in1.get_random_type(), kNotRandom);
EXPECT_EQ(in2.get_random_type(), kNotRandom);
EXPECT_EQ(in3.get_random_type(), kUniformRandom);
EXPECT_EQ(in4.get_random_type(), kGaussianRandom);
}

// Tests that the model values specified in Declare{...} are actually used by
Expand Down
18 changes: 15 additions & 3 deletions drake/systems/framework/test/system_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -392,6 +392,8 @@ class ValueIOTestSystem : public System<T> {
}));

this->DeclareInputPort(kVectorValued, 1);
this->DeclareInputPort(kVectorValued, 1, kUniformRandom);
this->DeclareInputPort(kVectorValued, 1, kGaussianRandom);
this->CreateOutputPort(std::make_unique<LeafOutputPort<T>>(*this,
1, // Vector size.
[](const Context<T>&) {
Expand Down Expand Up @@ -428,8 +430,8 @@ class ValueIOTestSystem : public System<T> {

BasicVector<T>* DoAllocateInputVector(
const InputPortDescriptor<T>& descriptor) const override {
// Should only get called for the second input.
EXPECT_EQ(descriptor.get_index(), 1);
// Should not get called for the first (abstract) input.
EXPECT_GE(descriptor.get_index(), 1);
return new TestTypedVector<T>();
}

Expand Down Expand Up @@ -556,7 +558,7 @@ class SystemIOTest : public ::testing::Test {
TEST_F(SystemIOTest, SystemValueIOTest) {
test_sys_.CalcOutput(*context_, output_.get());

EXPECT_EQ(context_->get_num_input_ports(), 2);
EXPECT_EQ(context_->get_num_input_ports(), 4);
EXPECT_EQ(output_->get_num_ports(), 2);

EXPECT_EQ(output_->get_data(0)->GetValue<std::string>(),
Expand All @@ -578,6 +580,16 @@ TEST_F(SystemIOTest, SystemValueIOTest) {
EXPECT_NE(dynamic_cast<const TestTypedVector<double>*>(
test_sys_.EvalVectorInput(*context_, 1)),
nullptr);

EXPECT_FALSE(test_sys_.get_input_port(0).is_random());
EXPECT_FALSE(test_sys_.get_input_port(1).is_random());
EXPECT_TRUE(test_sys_.get_input_port(2).is_random());
EXPECT_TRUE(test_sys_.get_input_port(3).is_random());

EXPECT_EQ(test_sys_.get_input_port(0).get_random_type(), kNotRandom);
EXPECT_EQ(test_sys_.get_input_port(1).get_random_type(), kNotRandom);
EXPECT_EQ(test_sys_.get_input_port(2).get_random_type(), kUniformRandom);
EXPECT_EQ(test_sys_.get_input_port(3).get_random_type(), kGaussianRandom);
}

// Tests that FixInputPortsFrom allocates ports of the same dimension as the
Expand Down

0 comments on commit a4bff5b

Please sign in to comment.