Skip to content

Commit

Permalink
remove all methods in direct trajectory optimization that accept a co…
Browse files Browse the repository at this point in the history 
…st/constraint without an explicit declaration of the associated variables. these methods assumed the user understood the correct variable order in the mathematical program, which is far too fragile
  • Loading branch information
@RussTedrake
RussTedrake committed Aug 15, 2017
1 parent c04db7f commit 04fa63a
Show file tree
Hide file tree
Showing 11 changed files with 177 additions and 469 deletions.
10 changes: 4 additions & 6 deletions drake/automotive/test/trajectory_optimization_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,10 @@ GTEST_TEST(TrajectoryOptimizationTest, SimpleCarDircolTest) {
kTrajectoryTimeUpperBound);

// Input limits (note that the steering limit imposed by SimpleCar is larger).
DrivingCommand<double> lower_limit, upper_limit;
lower_limit.set_steering_angle(-M_PI_2);
lower_limit.set_acceleration(-std::numeric_limits<double>::infinity());
upper_limit.set_steering_angle(M_PI_2);
upper_limit.set_acceleration(std::numeric_limits<double>::infinity());
prog.AddInputBounds(lower_limit.get_value(), upper_limit.get_value());
DrivingCommand<symbolic::Expression> input;
input.SetFromVector(prog.input().cast<symbolic::Expression>());
prog.AddConstraintToAllKnotPoints(input.steering_angle() <= M_PI_2);
prog.AddConstraintToAllKnotPoints(-M_PI_2 <= input.steering_angle());

// Ensure that time intervals are (relatively) evenly spaced.
prog.AddTimeIntervalBounds(kTrajectoryTimeLowerBound / (kNumTimeSamples - 1),
Expand Down
2 changes: 1 addition & 1 deletion drake/examples/acrobot/acrobot_run_swing_up_traj_optimization.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ int do_main() {
systems::DircolTrajectoryOptimization dircol_traj(
acrobot.get(), *context, kNumTimeSamples, kTrajectoryTimeLowerBound,
kTrajectoryTimeUpperBound);
AddSwingUpTrajectoryParams(kNumTimeSamples, x0, xG, &dircol_traj);
AddSwingUpTrajectoryParams(x0, xG, &dircol_traj);

const double timespan_init = 4;
auto traj_init_x =
Expand Down
73 changes: 10 additions & 63 deletions drake/examples/acrobot/acrobot_swing_up.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,76 +22,23 @@ using drake::solvers::LinearEqualityConstraint;
namespace drake {
namespace examples {
namespace acrobot {
namespace {

/**
* Define a function to be evaluated as the running cost for an
* acrobot trajectory (using the solvers::FunctionTraits style
* interface).
*/
class AcrobotRunningCost {
public:
static size_t numInputs() {
return AcrobotStateVectorIndices::kNumCoordinates + 2;
}
static size_t numOutputs() { return 1; }

template <typename ScalarType>
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
void eval(const VecIn<ScalarType>& x, VecOut<ScalarType>& y) const {
DRAKE_ASSERT(static_cast<size_t>(x.rows()) == numInputs());
DRAKE_ASSERT(static_cast<size_t>(y.rows()) == numOutputs());

// u represents the input vector.
const auto u = x.tail(1);
const double R = 10; // From PendulumPlant.m, arbitrary AFAICT
y = (R * u) * u;
}
};

/**
* Define a function to be evaluated as the final cost for an
* acrobot trajectory (using the solvers::FunctionTraits style
* interface).
*/
class AcrobotFinalCost {
public:
static size_t numInputs() {
return AcrobotStateVectorIndices::kNumCoordinates + 1;
}
static size_t numOutputs() { return 1; }

template <typename ScalarType>
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
void eval(const VecIn<ScalarType>& x, VecOut<ScalarType>& y) const {
DRAKE_ASSERT(static_cast<size_t>(x.rows()) == numInputs());
DRAKE_ASSERT(static_cast<size_t>(y.rows()) == numOutputs());

y(0) = x(0);
}
};
} // anon namespace

void AddSwingUpTrajectoryParams(
int num_time_samples,
const Eigen::Vector4d& x0, const Eigen::Vector4d& xG,
systems::DircolTrajectoryOptimization* dircol_traj) {
systems::DircolTrajectoryOptimization* dircol) {

// Current limit for MIT's acrobot is 7-9 Amps, accroding to Michael Posa.
// Current limit for MIT's acrobot is 7-9 Amps, according to Michael Posa.
const double kTorqueLimit = 8;
const drake::Vector1d umin(-kTorqueLimit);
const drake::Vector1d umax(kTorqueLimit);
dircol_traj->AddInputBounds(umin, umax);

dircol_traj->AddStateConstraint(
std::make_shared<LinearEqualityConstraint>(
Eigen::Matrix4d::Identity(), x0), {0});
dircol_traj->AddStateConstraint(
std::make_shared<LinearEqualityConstraint>(
Eigen::Matrix4d::Identity(), xG), {num_time_samples - 1});
auto u = dircol->input();
dircol->AddConstraintToAllKnotPoints(-kTorqueLimit <= u(0));
dircol->AddConstraintToAllKnotPoints(u(0) <= kTorqueLimit);

dircol->AddLinearConstraint(dircol->initial_state() == x0);
dircol->AddLinearConstraint(dircol->final_state() == xG);

dircol_traj->AddRunningCostFunc(AcrobotRunningCost());
dircol_traj->AddFinalCostFunc(AcrobotFinalCost());
const double R = 10; // Cost on input "effort".
dircol->AddRunningCost((R * u) * u);
}

} // namespace acrobot
Expand Down
1 change: 0 additions & 1 deletion drake/examples/acrobot/acrobot_swing_up.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@ namespace acrobot {
* between @p x0 and @p xG).
*/
void AddSwingUpTrajectoryParams(
int num_time_samples,
const Eigen::Vector4d& x0, const Eigen::Vector4d& xG,
systems::DircolTrajectoryOptimization*);

Expand Down
10 changes: 5 additions & 5 deletions drake/examples/pendulum/pendulum_swing_up.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,16 +23,16 @@ namespace pendulum {
void AddSwingUpTrajectoryParams(const Eigen::Vector2d& x0,
const Eigen::Vector2d& xG,
systems::DircolTrajectoryOptimization* dircol) {
const int kTorqueLimit = 3; // Arbitrary, taken from PendulumPlant.m.
const drake::Vector1d umin(-kTorqueLimit);
const drake::Vector1d umax(kTorqueLimit);
dircol->AddInputBounds(umin, umax);
auto u = dircol->input();

const double kTorqueLimit = 3.0; // N*m.
dircol->AddConstraintToAllKnotPoints(-kTorqueLimit <= u(0));
dircol->AddConstraintToAllKnotPoints(u(0) <= kTorqueLimit);

dircol->AddLinearConstraint(dircol->initial_state() == x0);
dircol->AddLinearConstraint(dircol->final_state() == xG);

const double R = 10; // Cost on input "effort".
auto u = dircol->input();
dircol->AddRunningCost((R * u) * u);
}

Expand Down
26 changes: 14 additions & 12 deletions drake/examples/pendulum/test/pendulum_trajectory_optimization_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,7 @@ namespace examples {
namespace pendulum {
namespace {

GTEST_TEST(PendulumTrajectoryOptimization,
PendulumTrajectoryOptimizationTest) {
GTEST_TEST(PendulumTrajectoryOptimization, PendulumTrajectoryOptimizationTest) {
const int kNumTimeSamples = 21;
const int kTrajectoryTimeLowerBound = 2;
const int kTrajectoryTimeUpperBound = 6;
Expand All @@ -30,35 +29,38 @@ GTEST_TEST(PendulumTrajectoryOptimization,
auto context = pendulum.CreateDefaultContext();

systems::DircolTrajectoryOptimization dircol(
&pendulum, *context,
kNumTimeSamples, kTrajectoryTimeLowerBound,
&pendulum, *context, kNumTimeSamples, kTrajectoryTimeLowerBound,
kTrajectoryTimeUpperBound);
AddSwingUpTrajectoryParams(x0, xG, &dircol);

const double timespan_init = 4;
auto traj_init_x = PiecewisePolynomialType::FirstOrderHold(
{0, timespan_init}, {x0, xG});
auto traj_init_x =
PiecewisePolynomialType::FirstOrderHold({0, timespan_init}, {x0, xG});
const SolutionResult result =
dircol.SolveTraj(timespan_init, PiecewisePolynomialType(),
traj_init_x);
dircol.SolveTraj(timespan_init, PiecewisePolynomialType(), traj_init_x);
ASSERT_EQ(result, SolutionResult::kSolutionFound) << "Result is an Error";

Eigen::MatrixXd inputs;
Eigen::MatrixXd states;
std::vector<double> times_out;

dircol.GetResultSamples(&inputs, &states, &times_out);
EXPECT_TRUE(CompareMatrices(x0, states.col(0),
1e-10, MatrixCompareType::absolute));
EXPECT_TRUE(CompareMatrices(xG, states.col(kNumTimeSamples - 1),
1e-10, MatrixCompareType::absolute));
EXPECT_TRUE(CompareMatrices(x0, states.col(0), 1e-10,
MatrixCompareType::absolute));
EXPECT_TRUE(CompareMatrices(xG, states.col(kNumTimeSamples - 1), 1e-10,
MatrixCompareType::absolute));
const PiecewisePolynomialTrajectory state_traj =
dircol.ReconstructStateTrajectory();
for (int i = 0; i < kNumTimeSamples; ++i) {
EXPECT_TRUE(CompareMatrices(states.col(i), state_traj.value(times_out[i]),
1e-10, MatrixCompareType::absolute));
}

// Test input limits
EXPECT_TRUE((inputs.array() <= 3.0).all()); // These limits are hard-coded in
// AddSwingUpTrajectoryParams.
EXPECT_TRUE((inputs.array() >= -3.0).all());

// Test interpolation
for (int i = 0; i < kNumTimeSamples - 1; ++i) {
double t_l = times_out[i];
Expand Down
73 changes: 0 additions & 73 deletions drake/systems/trajectory_optimization/direct_collocation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,60 +51,6 @@ DircolTrajectoryOptimization::DircolTrajectoryOptimization(
}
}

namespace {
/// Since the running cost evaluation needs the timestep mangled, we
/// need to wrap it and convert the input.
class RunningCostEndWrapper : public solvers::Cost {
public:
explicit RunningCostEndWrapper(const std::shared_ptr<solvers::Cost>& cost)
: solvers::Cost(cost->num_vars()), cost_(cost) {}

protected:
void DoEval(const Eigen::Ref<const Eigen::VectorXd>&,
Eigen::VectorXd&) const override {
throw std::runtime_error("Non-Taylor constraint eval not implemented.");
}

void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
AutoDiffVecXd& y) const override {
AutoDiffVecXd wrapped_x = x;
wrapped_x(0) *= 0.5;
cost_->Eval(wrapped_x, y);
};

private:
const std::shared_ptr<Cost> cost_;
};

class RunningCostMidWrapper : public solvers::Cost {
public:
explicit RunningCostMidWrapper(const std::shared_ptr<solvers::Cost>& cost)
: Cost(cost->num_vars() + 1), // We wrap x(0) and x(1) into
// (x(0) + x(1)) * 0.5, so one
// less variable when calling
// Eval.
cost_(cost) {}

protected:
void DoEval(const Eigen::Ref<const Eigen::VectorXd>&,
Eigen::VectorXd&) const override {
throw std::runtime_error("Non-Taylor constraint eval not implemented.");
}

void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
AutoDiffVecXd& y) const override {
AutoDiffVecXd wrapped_x(x.rows() - 1);
wrapped_x.tail(x.rows() - 2) = x.tail(x.rows() - 2);
wrapped_x(0) = (x(0) + x(1)) * 0.5;
cost_->Eval(wrapped_x, y);
};

private:
const std::shared_ptr<solvers::Cost> cost_;
};

} // anon namespace

void DircolTrajectoryOptimization::DoAddRunningCost(
const symbolic::Expression& g) {
// Trapezoidal integration:
Expand All @@ -121,25 +67,6 @@ void DircolTrajectoryOptimization::DoAddRunningCost(
SubstitutePlaceholderVariables(g * h_vars()(N() - 2) / 2, N() - 1));
}

// We just use a generic constraint here since we need to mangle the
// input and output anyway.
void DircolTrajectoryOptimization::DoAddRunningCost(
std::shared_ptr<solvers::Cost> cost) {
AddCost(std::make_shared<RunningCostEndWrapper>(cost),
{h_vars().head(1), x_vars().head(num_states()),
u_vars().head(num_inputs())});

for (int i = 1; i < N() - 1; i++) {
AddCost(std::make_shared<RunningCostMidWrapper>(cost),
{h_vars().segment(i - 1, 2),
x_vars().segment(i * num_states(), num_states()),
u_vars().segment(i * num_inputs(), num_inputs())});
}

AddCost(std::make_shared<RunningCostEndWrapper>(cost),
{h_vars().tail(1), x_vars().tail(num_states()),
u_vars().tail(num_inputs())});
}

PiecewisePolynomialTrajectory
DircolTrajectoryOptimization::ReconstructStateTrajectory() const {
Expand Down
4 changes: 1 addition & 3 deletions drake/systems/trajectory_optimization/direct_collocation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ namespace drake {
namespace systems {

/**
* DircolTrajectoryOptimization implements a direct colocation
* DircolTrajectoryOptimization implements a direct collocation
* approach to trajectory optimization, as described in
* C. R. Hargraves and S. W. Paris. Direct trajectory optimization using
* nonlinear programming and collocation. J Guidance, 10(4):338-342,
Expand Down Expand Up @@ -57,8 +57,6 @@ class DircolTrajectoryOptimization : public DirectTrajectoryOptimization {
private:
void DoAddRunningCost(const symbolic::Expression& e) override;

void DoAddRunningCost(std::shared_ptr<solvers::Cost> cost) override;

const System<double>* system_{nullptr};
const std::unique_ptr<Context<double>> context_{nullptr};
const std::unique_ptr<ContinuousState<double>> continuous_state_{nullptr};
Expand Down
Loading

0 comments on commit 04fa63a

Please sign in to comment.