Skip to content

Commit

Permalink
Rename xm to xa.
Browse files Browse the repository at this point in the history
  • Loading branch information
david-german-tri committed Apr 14, 2017
1 parent 83e18ac commit 754719a
Show file tree
Hide file tree
Showing 7 changed files with 46 additions and 46 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -74,9 +74,9 @@ GTEST_TEST(PendulumTrajectoryOptimization,
// from x_l, x_r, xdot_l, xdot_r. Check out equation 9 of
// Direct Trajectory Optimization Using Nonlinear Programming and
// Collocation. By C.R. Hargraves and S.W. Paris
const Eigen::Vector2d xm = 0.5 * (x_l + x_r) + h_i / 8 * (xdot_l - xdot_r);
const Eigen::Vector2d xa = 0.5 * (x_l + x_r) + h_i / 8 * (xdot_l - xdot_r);
const Eigen::Vector2d spline_midpoint(state_traj.value((t_l + t_r) / 2));
EXPECT_TRUE(CompareMatrices(xm, spline_midpoint, 1E-10,
EXPECT_TRUE(CompareMatrices(xa, spline_midpoint, 1E-10,
MatrixCompareType::absolute));
}
// TODO(hongkai.dai): Add a test on the optimal cost, when #5685 is merged, so
Expand Down
26 changes: 13 additions & 13 deletions drake/systems/framework/context.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,26 +62,26 @@ class Context {
bool is_stateless() const {
const int nxc = get_continuous_state()->size();
const int nxd = get_num_discrete_state_groups();
const int nxm = get_num_abstract_state_groups();
return nxc == 0 && nxd == 0 && nxm == 0;
const int nxa = get_num_abstract_state_groups();
return nxc == 0 && nxd == 0 && nxa == 0;
}

/// Returns true if the Context has continuous state, but no discrete or
/// abstract state.
bool has_only_continuous_state() const {
const int nxc = get_continuous_state()->size();
const int nxd = get_num_discrete_state_groups();
const int nxm = get_num_abstract_state_groups();
return nxc > 0 && nxd == 0 && nxm == 0;
const int nxa = get_num_abstract_state_groups();
return nxc > 0 && nxd == 0 && nxa == 0;
}

/// Returns true if the Context has discrete state, but no continuous or
/// abstract state.
bool has_only_discrete_state() const {
const int nxc = get_continuous_state()->size();
const int nxd = get_num_discrete_state_groups();
const int nxm = get_num_abstract_state_groups();
return nxd > 0 && nxc == 0 && nxm == 0;
const int nxa = get_num_abstract_state_groups();
return nxd > 0 && nxc == 0 && nxa == 0;
}

/// Sets the continuous state to @p xc, deleting whatever was there before.
Expand Down Expand Up @@ -164,21 +164,21 @@ class Context {
/// Asserts if @p index doesn't exist.
template <typename U>
U& get_mutable_abstract_state(int index) {
AbstractValues* xm = get_mutable_abstract_state();
return xm->get_mutable_value(index).GetMutableValue<U>();
AbstractValues* xa = get_mutable_abstract_state();
return xa->get_mutable_value(index).GetMutableValue<U>();
}

/// Sets the abstract state to @p xm, deleting whatever was there before.
void set_abstract_state(std::unique_ptr<AbstractValues> xm) {
get_mutable_state()->set_abstract_state(std::move(xm));
/// Sets the abstract state to @p xa, deleting whatever was there before.
void set_abstract_state(std::unique_ptr<AbstractValues> xa) {
get_mutable_state()->set_abstract_state(std::move(xa));
}

/// Returns a const reference to the abstract component of the
/// state at @p index. Asserts if @p index doesn't exist.
template <typename U>
const U& get_abstract_state(int index) const {
const AbstractValues* xm = get_state().get_abstract_state();
return xm->get_value(index).GetValue<U>();
const AbstractValues* xa = get_state().get_abstract_state();
return xa->get_value(index).GetValue<U>();
}

// =========================================================================
Expand Down
10 changes: 5 additions & 5 deletions drake/systems/framework/diagram_context.h
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class DiagramState : public State<T> {
std::vector<ContinuousState<T>*> sub_xcs;
sub_xcs.reserve(num_substates());
std::vector<BasicVector<T>*> sub_xds;
std::vector<AbstractValue*> sub_xms;
std::vector<AbstractValue*> sub_xas;
for (State<T>* substate : substates_) {
// Continuous
sub_xcs.push_back(substate->get_mutable_continuous_state());
Expand All @@ -77,9 +77,9 @@ class DiagramState : public State<T> {
substate->get_mutable_discrete_state()->get_data();
sub_xds.insert(sub_xds.end(), xd_data.begin(), xd_data.end());
// Abstract
AbstractValues* xm = substate->get_mutable_abstract_state();
for (int i_xm = 0; i_xm < xm->size(); ++i_xm) {
sub_xms.push_back(&xm->get_mutable_value(i_xm));
AbstractValues* xa = substate->get_mutable_abstract_state();
for (int i_xa = 0; i_xa < xa->size(); ++i_xa) {
sub_xas.push_back(&xa->get_mutable_value(i_xa));
}
}

Expand All @@ -91,7 +91,7 @@ class DiagramState : public State<T> {
this->set_continuous_state(
std::make_unique<DiagramContinuousState<T>>(sub_xcs));
this->set_discrete_state(std::make_unique<DiscreteValues<T>>(sub_xds));
this->set_abstract_state(std::make_unique<AbstractValues>(sub_xms));
this->set_abstract_state(std::make_unique<AbstractValues>(sub_xas));
}

private:
Expand Down
4 changes: 2 additions & 2 deletions drake/systems/framework/leaf_system.h
Original file line number Diff line number Diff line change
Expand Up @@ -143,13 +143,13 @@ class LeafSystem : public System<T> {
// not change.
const int n_xc = context->get_continuous_state()->size();
const int n_xd = context->get_num_discrete_state_groups();
const int n_xm = context->get_num_abstract_state_groups();
const int n_xa = context->get_num_abstract_state_groups();

SetDefaultState(*context, context->get_mutable_state());

DRAKE_DEMAND(n_xc == context->get_continuous_state()->size());
DRAKE_DEMAND(n_xd == context->get_num_discrete_state_groups());
DRAKE_DEMAND(n_xm == context->get_num_abstract_state_groups());
DRAKE_DEMAND(n_xa == context->get_num_abstract_state_groups());

// Set the default parameters, checking that the number of parameters does
// not change.
Expand Down
14 changes: 7 additions & 7 deletions drake/systems/framework/state.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,9 +56,9 @@ class State {
return discrete_state_.get();
}

void set_abstract_state(std::unique_ptr<AbstractValues> xm) {
DRAKE_DEMAND(xm != nullptr);
abstract_state_ = std::move(xm);
void set_abstract_state(std::unique_ptr<AbstractValues> xa) {
DRAKE_DEMAND(xa != nullptr);
abstract_state_ = std::move(xa);
}

const AbstractValues* get_abstract_state() const {
Expand All @@ -71,16 +71,16 @@ class State {
/// state at @p index. Asserts if @p index doesn't exist.
template <typename U>
const U& get_abstract_state(int index) const {
const AbstractValues* xm = get_abstract_state();
return xm->get_value(index).GetValue<U>();
const AbstractValues* xa = get_abstract_state();
return xa->get_value(index).GetValue<U>();
}

/// Returns a mutable pointer to element @p index of the abstract state.
/// Asserts if @p index doesn't exist.
template <typename U>
U& get_mutable_abstract_state(int index) {
AbstractValues* xm = get_mutable_abstract_state();
return xm->get_mutable_value(index).GetMutableValue<U>();
AbstractValues* xa = get_mutable_abstract_state();
return xa->get_mutable_value(index).GetMutableValue<U>();
}

/// Copies the values from another State of the same scalar type into this
Expand Down
22 changes: 11 additions & 11 deletions drake/systems/framework/test/abstract_values_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -23,27 +23,27 @@ class AbstractStateTest : public ::testing::Test {
};

TEST_F(AbstractStateTest, OwnedState) {
AbstractValues xm(std::move(data_));
EXPECT_EQ(42, UnpackIntValue(xm.get_value(0)));
EXPECT_EQ(76, UnpackIntValue(xm.get_value(1)));
AbstractValues xa(std::move(data_));
EXPECT_EQ(42, UnpackIntValue(xa.get_value(0)));
EXPECT_EQ(76, UnpackIntValue(xa.get_value(1)));
}

TEST_F(AbstractStateTest, UnownedState) {
AbstractValues xm(
AbstractValues xa(
std::vector<AbstractValue*>{data_[0].get(), data_[1].get()});
EXPECT_EQ(42, UnpackIntValue(xm.get_value(0)));
EXPECT_EQ(76, UnpackIntValue(xm.get_value(1)));
EXPECT_EQ(42, UnpackIntValue(xa.get_value(0)));
EXPECT_EQ(76, UnpackIntValue(xa.get_value(1)));
}

TEST_F(AbstractStateTest, SingleValueConstructor) {
AbstractValues xm(PackValue<int>(1000));
ASSERT_EQ(1, xm.size());
EXPECT_EQ(1000, UnpackIntValue(xm.get_value(0)));
AbstractValues xa(PackValue<int>(1000));
ASSERT_EQ(1, xa.size());
EXPECT_EQ(1000, UnpackIntValue(xa.get_value(0)));
}

TEST_F(AbstractStateTest, Clone) {
AbstractValues xm(std::move(data_));
std::unique_ptr<AbstractValues> clone = xm.Clone();
AbstractValues xa(std::move(data_));
std::unique_ptr<AbstractValues> clone = xa.Clone();
EXPECT_EQ(42, UnpackIntValue(clone->get_value(0)));
EXPECT_EQ(76, UnpackIntValue(clone->get_value(1)));
}
Expand Down
12 changes: 6 additions & 6 deletions drake/systems/framework/test/leaf_context_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,10 @@ class LeafContextTest : public ::testing::Test {

// Reserve an abstract state with one element, which is not owned.
abstract_state_ = PackValue(42);
std::vector<AbstractValue*> xm;
xm.push_back(abstract_state_.get());
std::vector<AbstractValue*> xa;
xa.push_back(abstract_state_.get());
context_.set_abstract_state(
std::make_unique<AbstractValues>(std::move(xm)));
std::make_unique<AbstractValues>(std::move(xa)));

// Reserve two numeric parameters, of size 3 and size 4.
std::vector<std::unique_ptr<BasicVector<double>>> params;
Expand Down Expand Up @@ -325,9 +325,9 @@ TEST_F(LeafContextTest, SetTimeStateAndParametersFrom) {
target.set_discrete_state(
std::make_unique<DiscreteValues<AutoDiffXd>>(std::move(xd)));

std::vector<std::unique_ptr<AbstractValue>> xm;
xm.push_back(PackValue(76));
target.set_abstract_state(std::make_unique<AbstractValues>(std::move(xm)));
std::vector<std::unique_ptr<AbstractValue>> xa;
xa.push_back(PackValue(76));
target.set_abstract_state(std::make_unique<AbstractValues>(std::move(xa)));

std::vector<std::unique_ptr<BasicVector<AutoDiffXd>>> params;
params.push_back(std::make_unique<BasicVector<AutoDiffXd>>(3));
Expand Down

0 comments on commit 754719a

Please sign in to comment.