Enable Fixed Complexity Slip MPC #332

ShaneRozenLevy · 2022-12-16T18:13:55Z

To run the code:
bazel run examples/Cassie/kinematic_centroidal_planner:cassie_kcmpc_trajopt

to check the lifting and reduction function run:
bazel run examples/Cassie/kinematic_centroidal_planner/simple_models:cassie_slip_lifting_test

The main changes are:

the creation of the relevant slip constraints and functions in examples/Cassie/kinematic_centroidal_planner/simple_models/
Modify systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc to take slip specific functions in examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
Tweak trajectory parameters to include the complexity schedule and slip parameters

Main todo before merging in would be:

Create knot point class
Remove any mention of SLIP from kinematic_centroidal_solver
Make sure I didn't break anything related to the mpc

This change is

…momentum_decision_variable

yangwill

Making my way through the PR, just need to look at the slip lifting and reduction functions

Reviewed 32 of 40 files at r1, all commit messages.
Reviewable status: 32 of 40 files reviewed, 19 unresolved discussions (waiting on @ShaneRozenLevy)

examples/Cassie/kinematic_centroidal_planner/BUILD.bazel line 79 at r1 (raw file):

cc_library(
    name = "cassie_reference_utils",

let's rename this to be consistent - makes finding the files easier

examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc line 110 at r1 (raw file):

      plant, traj_params.n_knot_points,
      time_points.back() / (traj_params.n_knot_points - 1), 0.4,
      reference_state.head(plant.num_positions()), traj_params.spring_constant,

probably should split up params into kinematic centroidal and slip and just pass in those classes so it's cleaner

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h line 13 at r1 (raw file):

/*!
 * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop

Also adds a bunch of variables related to slip, not clear from description here why.

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h line 38 at r1 (raw file):

    AddLoopClosure();

    slip_contact_points_ = CreateSlipContactPoints(plant, mu);

Because this isn't trivial anymore, we should move this to the .cc file

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h line 53 at r1 (raw file):

    std::vector<bool> stance_mode(2);
    std::fill(stance_mode.begin(), stance_mode.end(), true);
    std::fill(slip_contact_sequence_.begin(), slip_contact_sequence_.end(),

why is slip_contact_sequence initialized to all trues? probably add a comment here explaining why.

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h line 173 at r1 (raw file):

      slip_contact_points_;

  std::map<std::vector<bool>, std::vector<bool>> complex_mode_to_slip_mode_{

should probably use std::unordered_map here

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 214 at r1 (raw file):

    const drake::trajectories::PiecewisePolynomial<double>& com_trajectory) {
  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
    prog_->SetInitialGuess(slip_com_vars_[knot_point],

i think i'm just confused about the overall structure, i understand why we might want to override SetComPositionGuess for the cassie specific implementation, but why does it have slip variables? Maybe this class should be renamed?

examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h line 36 at r1 (raw file):

    a->Visit(DRAKE_NVP(gait_sequence));
    a->Visit(DRAKE_NVP(com_vel_values));
    a->Visit(DRAKE_NVP(complexity_schedule));

my bad, should probably rename to Motion to reflect the yamls that use it, also probably at this point we should have a short comment detailing the complicated parameters like complexity schedule. Only way to figure out the format is to either look at the regex parser or through the examples.

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc line 25 at r1 (raw file):

          "SlipReductionConstraint[" + std::to_string(knot_index) + "]"),
      reducing_function_(reducing_function),
      slip_dim_(3 + 3 + 2 * 3 * n_slip_feet + n_slip_feet),

just for sanity checking, probably use the same math when declaring how many slip variables there are, so slip_dim_(3 + 3 + 3*n_slip_feet + 3*n_slip_feet + n_slip_feet). It took me a bit to make sure that was the same.

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc line 115 at r1 (raw file):

void SlipDynamicsConstraint<T>::EvaluateConstraint(
    const Eigen::Ref<const drake::VectorX<T>>& x, drake::VectorX<T>* y) const {
  const auto& com0 = x.head(3);

i wish there was a cleaner way to do this - i guess that's why tests exist.

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc line 144 at r1 (raw file):

    const drake::VectorX<T>& contact_loc, const drake::VectorX<T>& slip_force,
    const std::vector<bool>& contact_mask) const {
  drake::Vector3<T> ddcom = {0, 0, -9.81};

consider using drake's constant in case they change the convention later down the line. Either by constructing a drake::multibody::UniformGravityFieldElement and getting its gravity_vector(). Or gravity = Vector3d(0, 0, -multibody::UniformGravityFieldElement<double>::kDefaultStrength);.

fyi, this is because drake is considering switching from 9.81 to the NIST standard 9.80665.

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h line 4 at r1 (raw file):

template <typename T>
T SlipGrf(double k, double r0, double b, T r, T dr, T force);

Just to distinguish between classes and functions, can we rename this to CalcSlipGrf?

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc line 9 at r1 (raw file):

template <typename T>
T SlipGrf(double k, double r0, double b, T r, T dr, T force) {
  auto F = k * (r0 - r) + force - b * dr;

personal preference, but i prefer F = force + k * (r0 - r) + b*(0 - dr) just for consistency.

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc line 11 at r1 (raw file):

  auto F = k * (r0 - r) + force - b * dr;
  if (F < 0) {
    F = 0;

does this work with AutoDiff?

systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h line 431 at r1 (raw file):

   */
  virtual drake::VectorX<double> LiftSlipSolution(int knot_point) {
    DRAKE_DEMAND(false);

maybe we should have a macro for not implemented yet?

systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc line 238 at r1 (raw file):

    const drake::solvers::SolverOptions& solver_options) {
  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
    switch (complexity_schedule_[knot_point]) {

this looks great! I assume we can move the adding of constraint to the knot point classes in the future?

systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc line 310 at r1 (raw file):

          switch (complexity_schedule_[knot_point + 1]) {
            case KINEMATIC_CENTROIDAL:
              AddKinematicCentroidalCosts(knot_point, t, 0.5);

might need to revisit this in the future. im not convinced that this logic is correct

examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc line 164 at r1 (raw file):

  std::regex number_regex("(^|\\s)([0-9]+)($|\\s)");
  std::smatch match;
  for (const auto& complexity_string : complexity_string_list) {

if we dont't already, we should write a test for this.

examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc line 178 at r1 (raw file):

      }
    } else {
      DRAKE_DEMAND(false);

While we can trace this back to this line, this isn't a helpful assertion. maybe earlier have DRAKE_DEMAND(complexity_string.at(0) == 'c' || complexity_string.at(0) == 's'

yangwill

Reviewable status: 32 of 40 files reviewed, 34 unresolved discussions (waiting on @ShaneRozenLevy)

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 12 at r1 (raw file):

CassieKinematicCentroidalSolver::CreateSlipContactPoints(
    const drake::multibody::MultibodyPlant<double>& plant, double mu) {
  auto left_toe_pair = dairlib::LeftToeFront(plant);

should check to see if the center of the foot is a better contact point.

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 68 at r1 (raw file):

      // flight before restricting the foot to be in the air to limit over
      // constraining the optimization problem
      if (!is_first_knot(knot_point) and !is_last_knot(knot_point) and

we might want to consider making a cleaner check

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 89 at r1 (raw file):

  }

  // Project linear momentum

Projection explanation can be clearer, maybe a comment is all that's necessary

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 91 at r1 (raw file):

  // Project linear momentum
  if (mom_ref_traj_) {
    const Eigen::MatrixXd Q_vel = gains_.lin_momentum.asDiagonal() * m_;

need a comment here detailing the costs. also nitpick but i like putting scalars before matrices so m_ * gains.lin_momentum.asDiagonal()

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 156 at r1 (raw file):

    int knot_point) {
  prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point));
  prog_->AddConstraint(slip_vel_vars_[knot_point] * m_ ==

same comment here `m_ * slip_vel_vars_[knot_point]

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 190 at r1 (raw file):

         slip_contact_pos_vars_[knot_point + 1],
         slip_force_vars_[knot_point + 1]}));
    prog_->AddConstraint(slip_contact_pos_vars_[knot_point + 1] ==

need comment explaining this constraint, what are we assuming? x(t+1) = x(t) + dt * 0.5 * (v(t) + v(t+1))

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 278 at r1 (raw file):

    const drake::solvers::SolverOptions& solver_options) {
  for (int knot = 0; knot < n_knot_points_; knot++) {
    lifters_.emplace_back(new SlipLifter(

SLIP parameters should be moved in to a class/struct for organization. it'll be much cleaner to pass.

examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc line 307 at r1 (raw file):

  // Identity orientation
  // TODO figure out why this identity quaternion constraint is hard for the mpc
  prog_->AddBoundingBoxConstraint(

i think there are other formulations for the quaternion constraint we can look into

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h line 71 at r1 (raw file):

  const T m_;
  const int n_feet_;
  const std::vector<bool> contact_mask0_;

isn't clear from the name what contact_mask is for.

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h line 149 at r1 (raw file):

  drake::solvers::VectorXDecisionVariable com_vars_;

  const int kSLIP_DIM = 3;

can be static, also move to beginning of file

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc line 71 at r1 (raw file):

  DRAKE_DEMAND(slip_feet_positions.size() == 3 * slip_contact_points_.size());
  // Add com position constraint
  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(

is there a reason we use add/remove instead of update?

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc line 149 at r1 (raw file):

  // Finite difference to calculate momentum jacobian
  // TODO replace this with analytical gradient

pinocchio functionality for this works now, will merge that feature after this PR

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc line 172 at r1 (raw file):

  rv.head(3) = drake::VectorX<double>::Zero(3);
  // Solve the linear least squares for other velocities
  rv.tail(n_v_ - 3) = A.rightCols(n_v_ - 3)

assuming you're using the best least squares method provided by eigen: https://eigen.tuxfamily.org/dox/group__LeastSquares.html

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc line 215 at r1 (raw file):

       simple_index++) {
    // Calculate the slip grf
    double r = (com_pos - slip_feet_pos.segment(simple_index * 3, 3)).norm();

looks good!

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc line 290 at r1 (raw file):

  const auto& complex_contact_pos = LiftContactPos(generalized_pos);

  (*complex_state) << slip_com,

would a struct help? it might help reduce errors from indexing and ordering. drake has something called BasicVector and multiple classes that we've written that inherit from it that do the indexing for passing between systems. We might want to do something similar for slip/complex states.

yangwill

Reviewable status: 32 of 40 files reviewed, 35 unresolved discussions (waiting on @ShaneRozenLevy)

examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc line 17 at r1 (raw file):

    int n_complex_feet, int knot_index)
    : dairlib::solvers::NonlinearConstraint<double>(
          3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet,

on second read through, i think it makes sense to make static class functions called num_slip_vars(n_slip_feet) and num_complex_vars(n_slip_feet). That'll probably make this function much more readable.

ShaneRozenLevy

Reviewable status: 26 of 40 files reviewed, 36 unresolved discussions (waiting on @yangwill)

a discussion (no related file):
Just pushed the first set of fixes to your comments.

examples/Cassie/kinematic_centroidal_planner/BUILD.bazel line 79 at r1 (raw file):