Skip to content

Commit

Permalink
Add constructors of MultipleShooting which accept placeholders
Browse files Browse the repository at this point in the history
  • Loading branch information
@soonho-tri
soonho-tri committed Sep 14, 2019
1 parent 354b4ef commit fa46106
Show file tree
Hide file tree
Showing 4 changed files with 198 additions and 54 deletions.
96 changes: 65 additions & 31 deletions systems/trajectory_optimization/multiple_shooting.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,13 @@ namespace drake {
namespace systems {
namespace trajectory_optimization {

using trajectories::PiecewisePolynomial;
using symbolic::Expression;
using internal::SequentialExpressionManager;
using symbolic::Expression;
using symbolic::Formula;
using symbolic::MakeVectorContinuousVariable;
using symbolic::Substitution;
using symbolic::Variable;
using trajectories::PiecewisePolynomial;

// For readability of long lines, these single-letter variables names are
// sometimes used:
Expand All @@ -33,6 +37,13 @@ MultipleShooting::MultipleShooting(int num_inputs, int num_states,
false /* timesteps_are_decision_variables */,
fixed_timestep, fixed_timestep) {}

MultipleShooting::MultipleShooting(
const solvers::VectorXDecisionVariable& input,
const solvers::VectorXDecisionVariable& state, int num_time_samples,
double fixed_timestep)
: MultipleShooting(input, state, num_time_samples, nullopt, fixed_timestep,
fixed_timestep) {}

MultipleShooting::MultipleShooting(int num_inputs, int num_states,
int num_time_samples,
double minimum_timestep,
Expand All @@ -41,32 +52,56 @@ MultipleShooting::MultipleShooting(int num_inputs, int num_states,
true /* timesteps_are_decision_variables */,
minimum_timestep, maximum_timestep) {}

MultipleShooting::MultipleShooting(
const solvers::VectorXDecisionVariable& input,
const solvers::VectorXDecisionVariable& state,
const solvers::DecisionVariable& time, int num_time_samples,
double minimum_timestep, double maximum_timestep)
: MultipleShooting(input, state, num_time_samples, time, minimum_timestep,
maximum_timestep) {}

MultipleShooting::MultipleShooting(int num_inputs, int num_states,
int num_time_samples,
bool timesteps_are_decision_variables,
double minimum_timestep,
double maximum_timestep)
: MultipleShooting(
MakeVectorContinuousVariable(num_inputs, "u"),
MakeVectorContinuousVariable(num_states, "x"), num_time_samples,
timesteps_are_decision_variables
? optional<solvers::DecisionVariable>{solvers::DecisionVariable{
"t"}}
: nullopt,
minimum_timestep, maximum_timestep) {}

MultipleShooting::MultipleShooting(
const solvers::VectorXDecisionVariable& input,
const solvers::VectorXDecisionVariable& state, int num_time_samples,
const optional<solvers::DecisionVariable>& time_var,
double minimum_timestep, double maximum_timestep)
: MathematicalProgram(),
num_inputs_(num_inputs),
num_states_(num_states),
num_inputs_(input.size()),
num_states_(state.size()),
N_(num_time_samples),
timesteps_are_decision_variables_(timesteps_are_decision_variables),
timesteps_are_decision_variables_(time_var),
fixed_timestep_(minimum_timestep),
x_vars_(NewContinuousVariables(num_states_ * N_, "x")),
u_vars_(NewContinuousVariables(num_inputs_ * N_, "u")),
placeholder_x_vars_(state),
placeholder_u_vars_(input),
sequential_expression_manager_(N_) {
placeholder_x_vars_ =
sequential_expression_manager_.RegisterSequentialExpressions(
Eigen::Map<solvers::MatrixXDecisionVariable>(x_vars_.data(),
num_states_, N_)
.cast<symbolic::Expression>(),
"x");
placeholder_u_vars_ =
sequential_expression_manager_.RegisterSequentialExpressions(
Eigen::Map<solvers::MatrixXDecisionVariable>(u_vars_.data(),
num_inputs_, N_)
.cast<symbolic::Expression>(),
"u");
sequential_expression_manager_.RegisterSequentialExpressions(
state,
Eigen::Map<solvers::MatrixXDecisionVariable>(x_vars_.data(), num_states_,
N_)
.cast<Expression>(),
"x");
sequential_expression_manager_.RegisterSequentialExpressions(
input,
Eigen::Map<solvers::MatrixXDecisionVariable>(u_vars_.data(), num_inputs_,
N_)
.cast<Expression>(),
"u");
DRAKE_DEMAND(num_time_samples > 1);
DRAKE_DEMAND(num_states_ > 0);
DRAKE_DEMAND(num_inputs_ >= 0);
Expand All @@ -83,9 +118,9 @@ MultipleShooting::MultipleShooting(int num_inputs, int num_states,
for (int i = 1; i < N_; ++i) {
t_expressions(i) = t_expressions(i - 1) + h_vars_(i - 1);
}
placeholder_t_var_(0) =
sequential_expression_manager_.RegisterSequentialExpressions(
t_expressions, "t")(0);
placeholder_t_var_(0) = *time_var;
sequential_expression_manager_.RegisterSequentialExpressions(
placeholder_t_var_, t_expressions, "t");
} else {
h_vars_ = solvers::VectorXDecisionVariable(0);
DRAKE_DEMAND(fixed_timestep_ > 0);
Expand All @@ -100,8 +135,7 @@ MultipleShooting::MultipleShooting(int num_inputs, int num_states,
solvers::VectorXDecisionVariable MultipleShooting::NewSequentialVariable(
int rows, const std::string& name) {
return sequential_expression_manager_.RegisterSequentialExpressions(
NewContinuousVariables(rows, N_, name).cast<symbolic::Expression>(),
name);
NewContinuousVariables(rows, N_, name).cast<Expression>(), name);
}

solvers::VectorXDecisionVariable MultipleShooting::GetSequentialVariableAtIndex(
Expand Down Expand Up @@ -284,7 +318,8 @@ Eigen::MatrixXd MultipleShooting::GetStateSamples(
Eigen::MatrixXd MultipleShooting::GetSequentialVariableSamples(
const solvers::MathematicalProgramResult& result,
const std::string& name) const {
int num_sequential_variables = sequential_expression_manager_.num_rows(name);
const int num_sequential_variables =
sequential_expression_manager_.num_rows(name);
Eigen::MatrixXd sequential_variables(num_sequential_variables, N_);
for (int i = 0; i < N_; i++) {
sequential_variables.col(i) =
Expand All @@ -293,31 +328,30 @@ Eigen::MatrixXd MultipleShooting::GetSequentialVariableSamples(
return sequential_variables;
}

symbolic::Substitution
MultipleShooting::ConstructPlaceholderVariableSubstitution(
Substitution MultipleShooting::ConstructPlaceholderVariableSubstitution(
int interval_index) const {
return sequential_expression_manager_
.ConstructPlaceholderVariableSubstitution(interval_index);
}

symbolic::Expression MultipleShooting::SubstitutePlaceholderVariables(
const symbolic::Expression& e, int interval_index) const {
Expression MultipleShooting::SubstitutePlaceholderVariables(
const Expression& e, int interval_index) const {
return e.Substitute(ConstructPlaceholderVariableSubstitution(interval_index));
}

const solvers::VectorXDecisionVariable MultipleShooting::GetSequentialVariable(
const std::string& name) const {
int rows = sequential_expression_manager_.num_rows(name);
VectorX<symbolic::Expression> sequential_variable(rows * N_);
const int rows = sequential_expression_manager_.num_rows(name);
VectorX<Expression> sequential_variable(rows * N_);
for (int i = 0; i < N_; i++) {
sequential_variable.segment(i * rows, rows) =
sequential_expression_manager_.GetSequentialExpressionsByName(name, i);
}
return symbolic::GetVariableVector(sequential_variable);
}

symbolic::Formula MultipleShooting::SubstitutePlaceholderVariables(
const symbolic::Formula& f, int interval_index) const {
Formula MultipleShooting::SubstitutePlaceholderVariables(
const Formula& f, int interval_index) const {
return f.Substitute(ConstructPlaceholderVariableSubstitution(interval_index));
}

Expand Down
39 changes: 37 additions & 2 deletions systems/trajectory_optimization/multiple_shooting.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -342,7 +342,8 @@ class MultipleShooting : public solvers::MathematicalProgram {
}

protected:
/// Constructs a MultipleShooting instance with fixed sample times.
/// Constructs a MultipleShooting instance with fixed sample times. It creates
/// new placeholder variables for input and state.
///
/// @param num_inputs Number of inputs at each sample point.
/// @param num_states Number of states at each sample point.
Expand All @@ -351,8 +352,21 @@ class MultipleShooting : public solvers::MathematicalProgram {
MultipleShooting(int num_inputs, int num_states, int num_time_samples,
double fixed_timestep);

/// Constructs a MultipleShooting instance with fixed sample times. It uses
/// the provided `input` and `state` as placeholders instead of creating new
/// placeholder variables for them.
///
/// @param input Placeholder variables for input.
/// @param state Placeholder variables for state.
/// @param num_time_samples Number of time samples.
/// @param fixed_timestep The spacing between sample times.
MultipleShooting(const solvers::VectorXDecisionVariable& input,
const solvers::VectorXDecisionVariable& state,
int num_time_samples, double fixed_timestep);

/// Constructs a MultipleShooting instance with sample times as decision
/// variables.
/// variables. It creates new placeholder variables for input, state, and
/// time.
///
/// @param num_inputs Number of inputs at each sample point.
/// @param num_states Number of states at each sample point.
Expand All @@ -362,6 +376,21 @@ class MultipleShooting : public solvers::MathematicalProgram {
MultipleShooting(int num_inputs, int num_states, int num_time_samples,
double minimum_timestep, double maximum_timestep);

/// Constructs a MultipleShooting instance with sample times as decision
/// variables. It uses the provided `input`, `state`, and `time` as
/// placeholders instead of creating new placeholder variables for them.
///
/// @param input Placeholder variables for input.
/// @param state Placeholder variables for state.
/// @param time Placeholder variable for time.
/// @param num_time_samples Number of time samples.
/// @param minimum_timestep Minimum spacing between sample times.
/// @param maximum_timestep Maximum spacing between sample times.
MultipleShooting(const solvers::VectorXDecisionVariable& input,
const solvers::VectorXDecisionVariable& state,
const solvers::DecisionVariable& time, int num_time_samples,
double minimum_timestep, double maximum_timestep);

/// Replaces e.g. placeholder_x_var_ with x_vars_ at time interval
/// @p interval_index, for all placeholder variables.
symbolic::Expression SubstitutePlaceholderVariables(
Expand Down Expand Up @@ -394,6 +423,12 @@ class MultipleShooting : public solvers::MathematicalProgram {
const std::string& name) const;

private:
MultipleShooting(const solvers::VectorXDecisionVariable& input,
const solvers::VectorXDecisionVariable& state,
int num_time_samples,
const optional<solvers::DecisionVariable>& time_var,
double minimum_timestep, double maximum_timestep);

MultipleShooting(int num_inputs, int num_states, int num_time_samples,
bool timesteps_are_decision_variables,
double minimum_timestep, double maximum_timestep);
Expand Down
14 changes: 7 additions & 7 deletions systems/trajectory_optimization/sequential_expression_manager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ namespace systems {
namespace trajectory_optimization {
namespace internal {

using std::string;
using symbolic::Expression;
using symbolic::Substitution;
using symbolic::Variable;
Expand All @@ -19,7 +20,7 @@ SequentialExpressionManager::SequentialExpressionManager(int num_samples)

VectorX<Variable> SequentialExpressionManager::RegisterSequentialExpressions(
const Eigen::Ref<const MatrixX<Expression>>& sequential_expressions,
const std::string& name) {
const string& name) {
const int rows = sequential_expressions.rows();
DRAKE_THROW_UNLESS(sequential_expressions.cols() == num_samples_);
const VectorX<Variable> placeholders{
Expand All @@ -29,10 +30,9 @@ VectorX<Variable> SequentialExpressionManager::RegisterSequentialExpressions(
}

void SequentialExpressionManager::RegisterSequentialExpressions(
const VectorX<symbolic::Variable>& placeholders,
const Eigen::Ref<const MatrixX<symbolic::Expression>>&
sequential_expressions,
const std::string& name) {
const VectorX<Variable>& placeholders,
const Eigen::Ref<const MatrixX<Expression>>& sequential_expressions,
const string& name) {
DRAKE_THROW_UNLESS(sequential_expressions.rows() == placeholders.size());
DRAKE_THROW_UNLESS(sequential_expressions.cols() == num_samples_);
const auto pair = name_to_placeholders_and_sequential_expressions_.insert(
Expand All @@ -59,7 +59,7 @@ SequentialExpressionManager::ConstructPlaceholderVariableSubstitution(
}

VectorX<Expression> SequentialExpressionManager::GetSequentialExpressionsByName(
const std::string& name, int index) const {
const string& name, int index) const {
DRAKE_THROW_UNLESS(0 <= index && index < num_samples_);
const auto it = name_to_placeholders_and_sequential_expressions_.find(name);
DRAKE_THROW_UNLESS(it !=
Expand All @@ -68,7 +68,7 @@ VectorX<Expression> SequentialExpressionManager::GetSequentialExpressionsByName(
return sequential_expressions.col(index);
}

int SequentialExpressionManager::num_rows(const std::string& name) const {
int SequentialExpressionManager::num_rows(const string& name) const {
const auto it = name_to_placeholders_and_sequential_expressions_.find(name);
DRAKE_THROW_UNLESS(it !=
name_to_placeholders_and_sequential_expressions_.end());
Expand Down
Loading

0 comments on commit fa46106

Please sign in to comment.