From 82bcb3ac3a2ff85375ed2003ec0a1841b8d212c9 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 14 Oct 2022 17:48:51 -0400
Subject: [PATCH 01/76] Code sort of works, but bounces

---
 .../cassie_kcmpc_trajopt.cc                   |  58 +++----
 .../kinematic_centroidal_constraints.cc       | 147 +++++++++++-------
 .../kinematic_centroidal_constraints.h        |  62 ++++++--
 .../kinematic_centroidal_mpc.cc               |  87 ++++-------
 .../kinematic_centroidal_mpc.h                |  42 +----
 5 files changed, 212 insertions(+), 184 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 465457fc29..367b838845 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -247,14 +247,14 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   double cost_force = 0.01;
 
-  double cost_joint_pos = 0.0005;
-  double cost_joint_vel = 0.0001;
+  double cost_joint_pos = 0.5;
+  double cost_joint_vel = 0.01;
 
   double cost_contact_pos = 1;
   double cost_contact_vel = 2;
 
   double cost_com_pos = 10;
-  double cost_com_vel = 0.001;
+  double cost_com_vel = 2;
   double cost_com_orientation = 8;
   double cost_angular_vel = 0.01;
 
@@ -274,34 +274,38 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
 
   Eigen::VectorXd Q_state = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities());
+
+  Q_state.head(4) = cost_com_orientation * Eigen::VectorXd::Ones(4);
+  Q_state.segment(4,3) = cost_com_pos * Eigen::VectorXd::Ones(3);
   Q_state.segment(7, plant.num_positions()-7) = cost_joint_pos * Eigen::VectorXd::Ones(plant.num_positions()-7);
+  Q_state.segment(plant.num_positions(), 3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
+  Q_state.segment(plant.num_positions() + 3, 3) = cost_com_vel * Eigen::VectorXd::Ones(3);
   Q_state.tail(plant.num_velocities() - 6) = cost_joint_vel * Eigen::VectorXd::Ones(plant.num_velocities() - 6);
   mpc.AddConstantStateReferenceCost(reference_state, Q_state.asDiagonal());
 
-  Eigen::VectorXd reference_cent_state = Eigen::VectorXd::Zero(13);
-  reference_cent_state[0] = 1;
-  reference_cent_state[6] = com_height;
-
-  Eigen::VectorXd reference_cent_state_bottom = Eigen::VectorXd::Zero(13);
-  reference_cent_state_bottom[0] = 1;
-  reference_cent_state_bottom[6] = com_height-squat_distance;
-  std::vector<double> time_points = {0, duration};
-  auto centroidal_reference = drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points,
-                                                                                               {reference_cent_state,
-                                                                                                reference_cent_state_bottom});
-
-  Eigen::VectorXd Q_cent(13);
-  Q_cent.head(4) = cost_com_orientation * Eigen::VectorXd::Ones(4);
-  Q_cent.segment(4,3) = cost_com_pos * Eigen::VectorXd::Ones(3);
-  Q_cent.segment(7,3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
-  Q_cent.segment(10,3) = cost_com_vel * Eigen::VectorXd::Ones(3);
-  mpc.AddCentroidalReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(centroidal_reference),
-                                 Q_cent.asDiagonal());
-
-  Eigen::VectorXd Q_contact = cost_contact_pos * Eigen::VectorXd::Ones(4 * 6);
-  Q_contact.tail(4 * 3) = cost_contact_vel * Eigen::VectorXd::Ones(4 * 3);
-  mpc.AddContactTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
-      4 * 6)), Q_contact.asDiagonal());
+//  Eigen::VectorXd reference_cent_state = Eigen::VectorXd::Zero(13);
+//  reference_cent_state[0] = 1;
+//  reference_cent_state[6] = com_height;
+//  Eigen::VectorXd reference_cent_state_bottom = Eigen::VectorXd::Zero(13);
+//  reference_cent_state_bottom[0] = 1;
+//  reference_cent_state_bottom[6] = com_height-squat_distance;
+//  std::vector<double> time_points = {0, duration};
+//  auto centroidal_reference = drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points,
+//                                                                                               {reference_cent_state,
+//                                                                                                reference_cent_state_bottom});
+//
+//  Eigen::VectorXd Q_cent(13);
+//  Q_cent.head(4) = cost_com_orientation * Eigen::VectorXd::Ones(4);
+//  Q_cent.segment(4,3) = cost_com_pos * Eigen::VectorXd::Ones(3);
+//  Q_cent.segment(7,3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
+//  Q_cent.segment(10,3) = cost_com_vel * Eigen::VectorXd::Ones(3);
+//  mpc.AddCentroidalReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(centroidal_reference),
+//                                 Q_cent.asDiagonal());
+
+//  Eigen::VectorXd Q_contact = cost_contact_pos * Eigen::VectorXd::Ones(4 * 6);
+//  Q_contact.tail(4 * 3) = cost_contact_vel * Eigen::VectorXd::Ones(4 * 3);
+//  mpc.AddContactTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
+//      4 * 6)), Q_contact.asDiagonal());
 
   std::cout<<"Adding solver options"<<std::endl;
   {
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
index 28ca54980c..67b2195ada 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
@@ -9,87 +9,97 @@ CentroidalDynamicsConstraint<T>::CentroidalDynamicsConstraint(const drake::multi
                                                               int n_contact,
                                                               double dt,
                                                               int knot_index): dairlib::solvers::NonlinearConstraint<T>(
-    13,  2 * 13 + plant.num_positions() + plant.num_velocities() + 2 * 3 * n_contact,
-    Eigen::VectorXd::Zero(13),
-    Eigen::VectorXd::Zero(13),
-    "centroidal_collocation[" +
+    6,  2 * 6 + 3 + 2 * 3 * n_contact,
+    Eigen::VectorXd::Zero(6),
+    Eigen::VectorXd::Zero(6),
+    "momentum_collocation[" +
         std::to_string(knot_index) + "]"),
                                                                                plant_(plant),
                                                                                context_(context),
-                                                                               n_x_(plant.num_positions()
-                                                                                        + plant.num_velocities()),
-                                                                               n_u_(plant.num_actuators()),
                                                                                n_contact_(n_contact),
                                                                                dt_(dt),
-                                                                               zero_control_(Eigen::VectorXd::Zero(n_u_)) {}
+                                                                               m_(plant_.CalcTotalMass(*context)){}
 
 
 /// The format of the input to the eval() function is in the order
-///   - xCent0, centroidal state at time k
-///   - xCent1, centroidal state at time k+1
-///   - x0, state at time k
+///   - xMom0, momentum state at time k
+///   - xMom1, momentum state at time k+1
+///   - com0, location of com at time k
 ///   - cj0, contact locations time k
 ///   - Fj0, contact forces at time k
 template <typename T>
 void CentroidalDynamicsConstraint<T>::EvaluateConstraint(
     const Eigen::Ref<const drake::VectorX<T>>& x, drake::VectorX<T>* y) const {
   // Extract decision variables
-  const auto& xCent0 = x.segment(0, n_cent_);
-  const auto& xCent1 = x.segment(n_cent_, n_cent_);
-  const auto& x0 = x.segment(2 * n_cent_, n_x_);
-  const auto& cj0 = x.segment(2 * n_cent_ + n_x_, 3 * n_contact_);
-  const auto& Fj0 = x.segment(2 * n_cent_ + n_x_ + 3 * n_contact_, 3 * n_contact_);
+  const auto& xMom0 = x.segment(0, n_mom_);
+  const auto& xMom1 = x.segment(n_mom_, n_mom_);
+  const auto& com0 = x.segment(2 * n_mom_, 3);
+  const auto& cj0 = x.segment(2 * n_mom_ + 3, 3 * n_contact_);
+  const auto& Fj0 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_, 3 * n_contact_);
 
-  // Evaluate dynamics at k
-  dairlib::multibody::SetContext<T>(plant_, x0, zero_control_, context_);
-  const auto& xdot0Cent = CalcTimeDerivativesWithForce(context_, xCent0,cj0, Fj0);
+  drake::Vector<T, 6> xdot0Mom = CalcTimeDerivativesWithForce(com0, cj0, Fj0);
 
   // Predict state and return error
-  const auto x1Predict = xCent0 + xdot0Cent * dt_;
-  *y = xCent1 - x1Predict;
+  const auto x1Predict = xMom0 + xdot0Mom * dt_;
+  *y = xMom1 - x1Predict;
 }
 
 template<typename T>
-drake::VectorX<T> CentroidalDynamicsConstraint<T>::CalcTimeDerivativesWithForce(drake::systems::Context<T> *context,
-                                                                                const drake::VectorX<T>& xCent,
+drake::VectorX<T> CentroidalDynamicsConstraint<T>::CalcTimeDerivativesWithForce(const drake::VectorX<T>& com_position,
                                                                                 const drake::VectorX<T>& contact_locations,
                                                                                 const drake::VectorX<T>& contact_forces) const {
-  // We use this constructor since passing in a vector has the order [x,y,z,w], while xCent is [w,x,y,z]
-  const Eigen::Quaternion<T> w_Q_b(xCent[0], xCent[1], xCent[2], xCent[3]);
-  const auto b_Q_w = w_Q_b.inverse();
-  const auto& r = xCent.segment(4, 3);
-  const drake::Vector3<T>& omega_ewrt_b = xCent.segment(7, 3);
-  const auto& d_r = xCent.segment(10, 3);
-
-  const auto& body_frame = plant_.get_body(*(plant_.GetFloatingBaseBodies().begin())).body_frame();
-  const drake::multibody::SpatialInertia< T >& spatial_inertia =
-      plant_.CalcSpatialInertia(*context, body_frame, plant_.GetBodyIndices(drake::multibody::ModelInstanceIndex(2)));
-  const auto& rotational_inertia = spatial_inertia.CalcRotationalInertia().CopyToFullMatrix3();
-  const auto& mass = spatial_inertia.get_mass();
-
-  drake::Vector3<T> sum_moments;
-  drake::Vector3<T> sum_forces;
+  drake::Vector3<T> sum_moments(0,0,0);
+  drake::Vector3<T> sum_forces = - m_ * drake::Vector3<T>(0, 0, 9.81);
   for(int contact = 0; contact < n_contact_; contact ++){
     const drake::Vector3<T>& location = contact_locations.segment(contact * 3, 3);
     const drake::Vector3<T>& force = contact_forces.segment(contact * 3, 3);
 
-    sum_moments = sum_forces + (location - r).cross(force);
+    sum_moments = sum_moments + (location - com_position).cross(force);
     sum_forces = sum_forces + force;
   }
 
   // Working in body frame angular velocity
-  const auto d_quat = drake::math::CalculateQuaternionDtFromAngularVelocityExpressedInB(w_Q_b, omega_ewrt_b);
-  const auto d_omega_ewrt_b = rotational_inertia.transpose()* (b_Q_w * sum_moments - omega_ewrt_b.cross(rotational_inertia * omega_ewrt_b));
-  const auto dd_r = sum_forces/mass - drake::Vector3<T>(0, 0, 9.81);
-
-  drake::Vector<T, 13> rv;
-  rv.head(4) = d_quat;
-  rv.segment(4,3) = d_r;
-  rv.segment(7,3) = d_omega_ewrt_b;
-  rv.segment(10,3) = dd_r;
+  const auto d_ang_mom =  sum_moments;
+  const auto d_lin_mom = sum_forces;
+  drake::Vector<T, 6> rv;
+  rv.head(3) = d_ang_mom;
+  rv.tail(3) = d_lin_mom;
   return rv;
 }
 
+template<typename T>
+KinematicIntegratorConstraint<T>::KinematicIntegratorConstraint(const drake::multibody::MultibodyPlant<T> &plant,
+                                                                  drake::systems::Context<T> *context,
+                                                                  double dt,
+                                                                  int knot_index): dairlib::solvers::NonlinearConstraint<T>(
+    plant.num_positions(),  2 * plant.num_positions()+ plant.num_velocities(),
+    Eigen::VectorXd::Zero(plant.num_positions()),
+    Eigen::VectorXd::Zero(plant.num_positions()),
+    "generalized_velocity_integrator[" +
+        std::to_string(knot_index) + "]"),
+                                                                                   plant_(plant),
+                                                                                   context_(context),
+                                                                                   n_q_(plant_.num_positions()),
+                                                                                   n_v_(plant_.num_velocities()),
+                                                                                   dt_(dt) {}
+
+/// The format of the input to the eval() function is in the order
+///   - q0, generalized position at time k
+///   - q1, generalized position at time k + 1
+///   - v0, generalized velocity at time k
+template<typename T>
+void KinematicIntegratorConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
+                                                           drake::VectorX<T> *y) const {
+  const auto& q0 = x.head(n_q_);
+  const auto& q1 = x.segment(n_q_, n_q_);
+  const auto& v0 = x.tail(n_v_);
+
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, q0, context_);
+  drake::VectorX<T> qdot0(n_q_);
+  plant_.MapVelocityToQDot(*context_, v0, &qdot0);
+  *y = q0 + dt_ * qdot0 - q1;
+}
+
 template<typename T>
 CenterofMassPositionConstraint<T>::CenterofMassPositionConstraint(const drake::multibody::MultibodyPlant<T> &plant,
                                                                   drake::systems::Context<T> *context,
@@ -114,11 +124,42 @@ void CenterofMassPositionConstraint<T>::EvaluateConstraint(const Eigen::Ref<cons
                                                            drake::VectorX<T> *y) const {
   const auto& rCom = x.segment(0, 3);
   const auto& x0 = x.segment(3, n_x_);
-
   dairlib::multibody::SetContext<T>(plant_, x0, zero_control_, context_);
   *y = rCom - plant_.CalcCenterOfMassPositionInWorld(*context_);
 }
 
+template<typename T>
+CentroidalMomentumConstraint<T>::CentroidalMomentumConstraint(const drake::multibody::MultibodyPlant<T> &plant,
+                                                                  drake::systems::Context<T> *context,
+                                                                  int knot_index): dairlib::solvers::NonlinearConstraint<T>(
+    6,  plant.num_positions()+ plant.num_velocities()+6 + 3,
+    Eigen::VectorXd::Zero(6),
+    Eigen::VectorXd::Zero(6),
+    "centroidal_momentum[" +
+        std::to_string(knot_index) + "]"),
+                                                                                   plant_(plant),
+                                                                                   context_(context),
+                                                                                   n_x_(plant.num_positions()
+                                                                                            + plant.num_velocities()),
+                                                                                   n_u_(plant.num_actuators()),
+                                                                                   zero_control_(Eigen::VectorXd::Zero(n_u_)) {}
+
+/// The format of the input to the eval() function is in the order
+///   - q, generalized positions
+///   - v, generalized velocities
+///   - r, location of the com
+///   - h_WC, angular momentum, linear momentum in the wf about the com
+template<typename T>
+void CentroidalMomentumConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
+                                                           drake::VectorX<T> *y) const {
+  const auto& x0 = x.head(n_x_);
+  const auto& r = x.segment(n_x_, 3);
+  const auto& h_WC = x.segment(n_x_ + 3, 6);
+  dairlib::multibody::SetContext<T>(plant_, x0, zero_control_, context_);
+  const auto& spatial_momentum = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, r);
+  *y = spatial_momentum.get_coeffs() - h_WC;
+}
+
 template<typename T>
 CenterofMassVelocityConstraint<T>::CenterofMassVelocityConstraint(const drake::multibody::MultibodyPlant<T> &plant,
                                                                   drake::systems::Context<T> *context,
@@ -131,9 +172,7 @@ CenterofMassVelocityConstraint<T>::CenterofMassVelocityConstraint(const drake::m
                                                                                    plant_(plant),
                                                                                    context_(context),
                                                                                    n_x_(plant.num_positions()
-                                                                                            + plant.num_velocities()),
-                                                                                   n_u_(plant.num_actuators()),
-                                                                                   zero_control_(Eigen::VectorXd::Zero(n_u_)) {}
+                                                                                            + plant.num_velocities()){}
 
 /// The format of the input to the eval() function is in the order
 ///   - drCOM, location of the center of mass
@@ -144,7 +183,7 @@ void CenterofMassVelocityConstraint<T>::EvaluateConstraint(const Eigen::Ref<cons
   const auto& drCom = x.segment(0, 3);
   const auto& x0 = x.segment(3, n_x_);
 
-  dairlib::multibody::SetContext<T>(plant_, x0, zero_control_, context_);
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, x0, context_);
   *y = drCom - plant_.CalcCenterOfMassTranslationalVelocityInWorld(*context_);
 }
 
@@ -171,6 +210,8 @@ void AngularVelocityConstraint<T>::EvaluateConstraint(const Eigen::Ref<const dra
 }
 
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class CentroidalDynamicsConstraint);
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class KinematicIntegratorConstraint);
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class CenterofMassPositionConstraint);
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class CentroidalMomentumConstraint);
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class CenterofMassVelocityConstraint);
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class AngularVelocityConstraint);
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
index d85ce6da3b..6a94c6a7ae 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
@@ -33,28 +33,48 @@ class CentroidalDynamicsConstraint : public dairlib::solvers::NonlinearConstrain
  private:
   /*!
    * @brief Calculates the time derivative of the centroidal state
-   * @param context context used to calcualte inertia tensor and mass
-   * @param xCent centroidal state = [quat, r, omega, dr]
+   * @param com_position center of mass position in world
    * @param contact_locations vector of the contact locations in world frame = [contact1x, contact1y, contact1z, ...]
    * @param contact_forces vector of the contact forces in world frame = [force1x, force1y, force1z, ...]
-   * @return time derivate of centroidal state
+   * @return time derivative of momentum
    */
   drake::VectorX<T> CalcTimeDerivativesWithForce(
-      drake::systems::Context<T>* context,
-      const drake::VectorX<T>& xCent,
+      const drake::VectorX<T>& com_position,
       const drake::VectorX<T>& contact_locations,
       const drake::VectorX<T>& contact_forces) const;
 
   const drake::multibody::MultibodyPlant<T>& plant_;
   drake::systems::Context<T>* context_;
-  int n_x_;
-  int n_u_;
   int n_contact_;
-  int n_cent_ = 13;
+  const int n_mom_ = 6;
+  double dt_;
+  T m_;
+};
+
+/*!
+ * @brief Nonlinear constraint on euler integrating generalized velocity
+ */
+template <typename T>
+class KinematicIntegratorConstraint : public dairlib::solvers::NonlinearConstraint<T> {
+
+ public:
+  KinematicIntegratorConstraint(const drake::multibody::MultibodyPlant<T>& plant,
+                                 drake::systems::Context<T>* context,
+                                 double dt,
+                                 int knot_index);
+
+ public:
+  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
+                          drake::VectorX<T>* y) const override;
+
+  const drake::multibody::MultibodyPlant<T>& plant_;
+  drake::systems::Context<T>* context_;
+  int n_q_;
+  int n_v_;
   double dt_;
-  const drake::VectorX<T> zero_control_;
 };
 
+
 /*!
  * @brief Nonlinear constraint on center of mass position matching centroidal state
  */
@@ -77,6 +97,28 @@ class CenterofMassPositionConstraint : public dairlib::solvers::NonlinearConstra
   const drake::VectorX<T> zero_control_;
 };
 
+/*!
+ * @brief Nonlinear constraint on center of mass position matching centroidal state
+ */
+template <typename T>
+class CentroidalMomentumConstraint : public dairlib::solvers::NonlinearConstraint<T> {
+
+ public:
+  CentroidalMomentumConstraint(const drake::multibody::MultibodyPlant<T>& plant,
+                                 drake::systems::Context<T>* context,
+                                 int knot_index);
+
+ public:
+  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
+                          drake::VectorX<T>* y) const override;
+
+  const drake::multibody::MultibodyPlant<T>& plant_;
+  drake::systems::Context<T>* context_;
+  int n_x_;
+  int n_u_;
+  const drake::VectorX<T> zero_control_;
+};
+
 /*!
  * @brief Nonlinear constraint on center of mass velocity matching centroidal velocity
  */
@@ -95,8 +137,6 @@ class CenterofMassVelocityConstraint : public dairlib::solvers::NonlinearConstra
   const drake::multibody::MultibodyPlant<T>& plant_;
   drake::systems::Context<T>* context_;
   int n_x_;
-  int n_u_;
-  const drake::VectorX<T> zero_control_;
 };
 
 /*!
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index c845b07465..862b08d9b2 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -31,7 +31,8 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
   for(int knot = 0; knot < n_knot_points; knot ++){
     contexts_[knot] = plant_.CreateDefaultContext();
     x_vars_.push_back(prog_->NewContinuousVariables(n_q_ + n_v_, "x_vars_" + std::to_string(knot)));
-    x_cent_vars_.push_back(prog_->NewContinuousVariables(kCentroidalPosDim + kCentroidalVelDim, "x_cent_vars_" + std::to_string(knot)));
+    mom_vars_.push_back(prog_->NewContinuousVariables(6, "mom_vars_" + std::to_string(knot)));
+    com_vars_.push_back(prog_->NewContinuousVariables(3, "com_vars_" + std::to_string(knot)));
     contact_pos_.push_back(prog_->NewContinuousVariables(3 * n_contact_points_, "contact_pos_" + std::to_string(knot)));
     contact_vel_.push_back(prog_->NewContinuousVariables(3 * n_contact_points_, "contact_vel_" + std::to_string(knot)));
     contact_force_.push_back(prog_->NewContinuousVariables(3 * n_contact_points_, "contact_force_" + std::to_string(knot)));
@@ -80,11 +81,9 @@ void KinematicCentroidalMPC::AddCentroidalDynamics() {
     auto constraint = std::make_shared<CentroidalDynamicsConstraint<double>>(
         plant_, contexts_[knot_point].get(), n_contact_points_, dt_,knot_point);
     centroidal_dynamics_binding_.push_back(prog_->AddConstraint(constraint,
-                                                                {centroidal_pos_vars(knot_point),
-                                                                centroidal_vel_vars(knot_point),
-                                                                centroidal_pos_vars(knot_point + 1),
-                                                                centroidal_vel_vars(knot_point + 1),
-                                                                state_vars(knot_point),
+                                                                {momentum_vars(knot_point),
+                                                                 momentum_vars(knot_point + 1),
+                                                                 com_pos_vars(knot_point),
                                                                 contact_pos_[knot_point],
                                                                 contact_force_[knot_point]}));
   }
@@ -92,9 +91,13 @@ void KinematicCentroidalMPC::AddCentroidalDynamics() {
 
 void KinematicCentroidalMPC::AddKinematicsIntegrator() {
   for (int knot_point = 0; knot_point < n_knot_points_ - 1; knot_point++) {
-    // Integrate joint states
-    prog_->AddConstraint(
-        joint_pos_vars(knot_point + 1) == joint_pos_vars(knot_point) + dt_ * joint_vel_vars(knot_point));
+    // Integrate generalized velocities to get generalized positions
+    auto constraint = std::make_shared<KinematicIntegratorConstraint<double>>(
+        plant_, contexts_[knot_point].get(), dt_,knot_point);
+    centroidal_dynamics_binding_.push_back(prog_->AddConstraint(constraint,
+                                                                {state_vars(knot_point).head(n_q_),
+                                                                 state_vars(knot_point+1).head(n_q_),
+                                                                 state_vars(knot_point).tail(n_v_)}));
 
     // Integrate foot states
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
@@ -123,6 +126,12 @@ void KinematicCentroidalMPC::AddContactConstraints() {
 
 void KinematicCentroidalMPC::AddCentroidalKinematicConsistency() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    // Ensure linear and angular momentum line up
+    auto centroidal_momentum =
+        std::make_shared<CentroidalMomentumConstraint<double>>(plant_, contexts_[knot_point].get(), knot_point);
+    prog_->AddConstraint(centroidal_momentum, {state_vars(knot_point),
+                                               com_pos_vars(knot_point),
+                                               momentum_vars(knot_point)});
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
       // Ensure foot position line up with kinematics
       auto foot_position_constraint =
@@ -140,22 +149,6 @@ void KinematicCentroidalMPC::AddCentroidalKinematicConsistency() {
         std::make_shared<CenterofMassPositionConstraint<double>>(
             plant_, contexts_[knot_point].get(), knot_point);
     prog_->AddConstraint(com_position, {com_pos_vars(knot_point), state_vars(knot_point)});
-
-    // Constrain com velocity
-    auto com_velocity =
-        std::make_shared<CenterofMassVelocityConstraint<double>>(
-            plant_, contexts_[knot_point].get(), knot_point);
-    prog_->AddConstraint(com_velocity, {com_vel_vars(knot_point), state_vars(knot_point)});
-
-    // Make sure centroidal and state quaternion match
-    prog_->AddConstraint( cent_quat_vars(knot_point) == state_vars(knot_point).head(4));
-
-    //Angular velocity constraint. Centroidal angular velocity is in body frame, state is in world frame
-    auto omega_constraint =
-        std::make_shared<AngularVelocityConstraint<double>>(knot_point);
-    prog_->AddConstraint(omega_constraint, {cent_quat_vars(knot_point),
-                                            cent_omega_vars(knot_point),
-                                            state_vars(knot_point).segment(n_q_,3)});
   }
 }
 
@@ -170,13 +163,6 @@ void KinematicCentroidalMPC::AddFrictionConeConstraints() {
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::state_vars(int knotpoint_index) const {
   return x_vars_[knotpoint_index];
 }
-
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::centroidal_pos_vars(int knotpoint_index) const {
-  return x_cent_vars_[knotpoint_index].segment(0, kCentroidalPosDim);
-}
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::centroidal_vel_vars(int knotpoint_index) const {
-  return x_cent_vars_[knotpoint_index].segment(kCentroidalPosDim, kCentroidalVelDim);
-}
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::joint_pos_vars(int knotpoint_index) const {
   return x_vars_[knotpoint_index].segment(kCentroidalPosDim, n_joint_q_);
 }
@@ -214,27 +200,27 @@ void KinematicCentroidalMPC::AddCosts() {
     double t = dt_ * knot_point;
     if(ref_traj_){
       const auto& ref = ref_traj_->value(t);
-      prog_->AddQuadraticCost(2 * Q_,  -Q_ * ref , state_vars(knot_point));
-    }
-    if(centroidal_ref_traj_){
-      const auto& ref = centroidal_ref_traj_->value(t);
-      prog_->AddQuadraticCost(2 * Q_cent_,  -Q_cent_ * ref , {centroidal_pos_vars(knot_point), centroidal_vel_vars(knot_point)});
+      prog_->AddQuadraticCost(Q_,  -Q_ * ref , state_vars(knot_point));
     }
+//    if(centroidal_ref_traj_){
+//      const auto& ref = centroidal_ref_traj_->value(t);
+//      prog_->AddQuadraticCost(Q_cent_,  -Q_cent_ * ref , {centroidal_pos_vars(knot_point), centroidal_vel_vars(knot_point)});
+//    }
     if(contact_ref_traj_){
       const auto& ref = contact_ref_traj_->value(t);
       drake::solvers::VariableRefList contact_vars;
-      prog_->AddQuadraticCost(2 * Q_contact_,  -Q_contact_ * ref , {contact_pos_[knot_point],contact_vel_[knot_point]});
+      prog_->AddQuadraticCost(Q_contact_,  -Q_contact_ * ref , {contact_pos_[knot_point],contact_vel_[knot_point]});
     }
     if(force_ref_traj_){
       const auto& ref = force_ref_traj_->value(t);
       drake::solvers::VariableRefList force_vars;
-      prog_->AddQuadraticCost(2 * Q_force_, -Q_force_ * ref, contact_force_[knot_point]);
+      prog_->AddQuadraticCost(Q_force_, -Q_force_ * ref, contact_force_[knot_point]);
     }
   }
 }
 
 void KinematicCentroidalMPC::SetZeroInitialGuess() {
-  Eigen::VectorXd initialGuess= Eigen::VectorXd::Zero(n_q_+n_v_+n_contact_points_*9 + kCentroidalPosDim + kCentroidalVelDim);
+  Eigen::VectorXd initialGuess= Eigen::VectorXd::Zero(n_q_+n_v_+n_contact_points_*9 + 6 + 3);
   prog_->SetInitialGuessForAllVariables(initialGuess.replicate(n_knot_points_, 1));
   // Make sure unit quaternions
   for (int i = 0; i < n_knot_points_; i++) {
@@ -243,13 +229,6 @@ void KinematicCentroidalMPC::SetZeroInitialGuess() {
     prog_->SetInitialGuess(xi(1), 0);
     prog_->SetInitialGuess(xi(2), 0);
     prog_->SetInitialGuess(xi(3), 0);
-
-    auto qi = centroidal_pos_vars(i);
-    prog_->SetInitialGuess(qi(0), 1);
-    prog_->SetInitialGuess(qi(1), 0);
-    prog_->SetInitialGuess(qi(2), 0);
-    prog_->SetInitialGuess(qi(3), 0);
-
   }
 }
 
@@ -323,16 +302,7 @@ void KinematicCentroidalMPC::AddPlantJointLimits(const std::vector<std::string>&
 
 }
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::com_pos_vars(int knotpoint_index) const {
-  return centroidal_pos_vars(knotpoint_index).tail(3);
-}
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::com_vel_vars(int knotpoint_index) const {
-  return centroidal_vel_vars(knotpoint_index).tail(3);
-}
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::cent_quat_vars(int knotpoint_index) const {
-  return centroidal_pos_vars(knotpoint_index).head(4);
-}
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::cent_omega_vars(int knotpoint_index) const {
-  return centroidal_vel_vars(knotpoint_index).head(3);
+  return com_vars_[knotpoint_index];
 }
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::contact_pos_vars(int knotpoint_index,
                                                                                  int contact_index) const {
@@ -357,3 +327,6 @@ void KinematicCentroidalMPC::SetRobotStateGuess(const drake::VectorX<double> &st
    prog_->SetInitialGuess(state_vars, state);
   }
 }
+drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::momentum_vars(int knotpoint_index) const {
+  return mom_vars_[knotpoint_index];
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 053c3c90a9..8ad089a7a5 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -93,22 +93,6 @@ class KinematicCentroidalMPC {
   drake::solvers::VectorXDecisionVariable state_vars(
       int knotpoint_index) const;
 
-  /*!
-   * @brief accessor for centroidal position decision vars
-   * @param knotpoint_index
-   * @return
-   */
-   drake::solvers::VectorXDecisionVariable centroidal_pos_vars(
-      int knotpoint_index) const;
-
-   /*!
-    * @brief accessor for centroidal velocity decision vars
-    * @param knotpoint_index
-    * @return
-    */
-   drake::solvers::VectorXDecisionVariable centroidal_vel_vars(
-      int knotpoint_index) const;
-
    /*!
     * @brief accessor for joint position decision vars
     * @param knotpoint_index
@@ -134,27 +118,11 @@ class KinematicCentroidalMPC {
       int knotpoint_index) const;
 
   /*!
-   * @brief accessor for center of mass velocity decision vars
-   * @param knotpoint_index
-   * @return
-   */
-  drake::solvers::VectorXDecisionVariable com_vel_vars(
-      int knotpoint_index) const;
-
-  /*!
-   * @brief accessor for centroidal quaternion decision vars
-   * @param knotpoint_index
-   * @return
-   */
-  drake::solvers::VectorXDecisionVariable cent_quat_vars(
-      int knotpoint_index) const;
-
-  /*!
-   * @brief accessor for com angular velocity decision variables (body frame)
+   * @brief accessor for center of momentum decision variables
    * @param knotpoint_index
    * @return
    */
-  drake::solvers::VectorXDecisionVariable cent_omega_vars(
+  drake::solvers::VectorXDecisionVariable momentum_vars(
       int knotpoint_index) const;
 
   /*!
@@ -300,8 +268,10 @@ class KinematicCentroidalMPC {
   //DecisionVariables
   // Full robot state
   std::vector<drake::solvers::VectorXDecisionVariable>  x_vars_;
-  // Centroidal position and orientation and velocity
-  std::vector<drake::solvers::VectorXDecisionVariable>  x_cent_vars_;
+  // angular and linear momentum variables (in that order)
+  std::vector<drake::solvers::VectorXDecisionVariable>  mom_vars_;
+  // center of mass position
+  std::vector<drake::solvers::VectorXDecisionVariable>  com_vars_;
 
   // Contact position, velocity, and force [n_knot_points, 3 * n_contact_points]
   std::vector<drake::solvers::VectorXDecisionVariable>  contact_pos_;

From e4151e76c661cb9725f36fcb63ec903cba29ef80 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 18 Oct 2022 18:14:05 -0400
Subject: [PATCH 02/76] Changed how costs work, but getting some strange
 solutions

---
 .../cassie_kcmpc_trajopt.cc                   | 74 +++++++++++--------
 .../kinematic_centroidal_mpc.cc               | 53 +++++++------
 .../kinematic_centroidal_mpc.h                | 17 +++--
 3 files changed, 86 insertions(+), 58 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 367b838845..68c3a54c1c 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -245,19 +245,23 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   Eigen::VectorXd reference_state = GenerateNominalStand(plant, com_height, stance_width);
   mpc.SetRobotStateGuess(reference_state);
 
-  double cost_force = 0.01;
+  double cost_force = 0.001;
 
-  double cost_joint_pos = 0.5;
-  double cost_joint_vel = 0.01;
+  double cost_joint_pos = 0.01;
+  double cost_joint_vel = 0.02;
 
   double cost_contact_pos = 1;
   double cost_contact_vel = 2;
 
   double cost_com_pos = 10;
-  double cost_com_vel = 2;
-  double cost_com_orientation = 8;
+
+  double cost_base_vel = 1;
+  double cost_orientation = 8;
   double cost_angular_vel = 0.01;
 
+  double cost_lin_mom = 0.1;
+  double cost_ang_mom = 0.1;
+
   double stance_wiggle = 0.01;
 
   Eigen::Vector3d left_lb(std::numeric_limits<double>::lowest(), -stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
@@ -270,42 +274,48 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   mpc.AddContactPointPositionConstraint(2, right_lb, right_ub);
   mpc.AddContactPointPositionConstraint(3, right_lb, right_ub);
 
-  mpc.AddConstantForceTrackingReferenceCost(Eigen::VectorXd::Zero(12), cost_force * Eigen::MatrixXd::Identity(12, 12));
+
+  Eigen::VectorXd Q_force = cost_force * Eigen::VectorXd::Ones(12);
+  Eigen::VectorXd ref_force = Eigen::VectorXd::Zero(12);
+  ref_force[2] = 33*9.81/4;
+  ref_force[5] = 33*9.81/4;
+  ref_force[8] = 33*9.81/4;
+  ref_force[11] = 33*9.81/4;
+
+  mpc.AddConstantForceTrackingReferenceCost(ref_force, Q_force.asDiagonal());
 
 
   Eigen::VectorXd Q_state = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities());
 
-  Q_state.head(4) = cost_com_orientation * Eigen::VectorXd::Ones(4);
-  Q_state.segment(4,3) = cost_com_pos * Eigen::VectorXd::Ones(3);
+  Q_state.head(4) = cost_orientation * Eigen::VectorXd::Ones(4);
   Q_state.segment(7, plant.num_positions()-7) = cost_joint_pos * Eigen::VectorXd::Ones(plant.num_positions()-7);
   Q_state.segment(plant.num_positions(), 3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
-  Q_state.segment(plant.num_positions() + 3, 3) = cost_com_vel * Eigen::VectorXd::Ones(3);
+  Q_state.segment(plant.num_positions() + 3, 3) = cost_base_vel * Eigen::VectorXd::Ones(3);
   Q_state.tail(plant.num_velocities() - 6) = cost_joint_vel * Eigen::VectorXd::Ones(plant.num_velocities() - 6);
   mpc.AddConstantStateReferenceCost(reference_state, Q_state.asDiagonal());
 
-//  Eigen::VectorXd reference_cent_state = Eigen::VectorXd::Zero(13);
-//  reference_cent_state[0] = 1;
-//  reference_cent_state[6] = com_height;
-//  Eigen::VectorXd reference_cent_state_bottom = Eigen::VectorXd::Zero(13);
-//  reference_cent_state_bottom[0] = 1;
-//  reference_cent_state_bottom[6] = com_height-squat_distance;
-//  std::vector<double> time_points = {0, duration};
-//  auto centroidal_reference = drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points,
-//                                                                                               {reference_cent_state,
-//                                                                                                reference_cent_state_bottom});
-//
-//  Eigen::VectorXd Q_cent(13);
-//  Q_cent.head(4) = cost_com_orientation * Eigen::VectorXd::Ones(4);
-//  Q_cent.segment(4,3) = cost_com_pos * Eigen::VectorXd::Ones(3);
-//  Q_cent.segment(7,3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
-//  Q_cent.segment(10,3) = cost_com_vel * Eigen::VectorXd::Ones(3);
-//  mpc.AddCentroidalReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(centroidal_reference),
-//                                 Q_cent.asDiagonal());
-
-//  Eigen::VectorXd Q_contact = cost_contact_pos * Eigen::VectorXd::Ones(4 * 6);
-//  Q_contact.tail(4 * 3) = cost_contact_vel * Eigen::VectorXd::Ones(4 * 3);
-//  mpc.AddContactTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
-//      4 * 6)), Q_contact.asDiagonal());
+  Eigen::VectorXd reference_com = Eigen::VectorXd::Zero(3);
+  reference_com[2] = com_height;
+  Eigen::VectorXd reference_com_bottom = Eigen::VectorXd::Zero(3);
+  reference_com_bottom[2] = com_height-squat_distance;
+  std::vector<double> time_points = {0, duration};
+  auto com_reference = drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points,
+                                                                                               {reference_com,
+                                                                                                reference_com_bottom});
+  Eigen::VectorXd Q_com = cost_com_pos * Eigen::VectorXd::Ones(3);
+  mpc.AddComReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_reference),
+                          Q_com.asDiagonal());
+
+  Eigen::VectorXd Q_contact = cost_contact_pos * Eigen::VectorXd::Ones(4 * 6);
+  Q_contact.tail(4 * 3) = cost_contact_vel * Eigen::VectorXd::Ones(4 * 3);
+  mpc.AddContactTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
+      4 * 6)), Q_contact.asDiagonal());
+
+
+  Eigen::VectorXd Q_momentum(6);
+  Q_momentum.head(3) = cost_ang_mom * Eigen::Vector3d::Ones();
+  Q_momentum.tail(3) = cost_lin_mom * Eigen::Vector3d::Ones();
+  mpc.AddConstantMomentumReferenceCost(Eigen::VectorXd::Zero(6), Q_momentum.asDiagonal());
 
   std::cout<<"Adding solver options"<<std::endl;
   {
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 862b08d9b2..3e82b7fe52 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -67,13 +67,21 @@ void KinematicCentroidalMPC::AddForceTrackingReferenceCost(std::unique_ptr<drake
   Q_force_ = Q_force;
 }
 
-void KinematicCentroidalMPC::AddCentroidalReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                                                        const Eigen::MatrixXd &Q) {
-  DRAKE_DEMAND(Q.rows() == kCentroidalPosDim + kCentroidalVelDim);
-  DRAKE_DEMAND(Q.cols() == kCentroidalPosDim + kCentroidalVelDim);
+void KinematicCentroidalMPC::AddComReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
+                                                 const Eigen::MatrixXd &Q) {
+  DRAKE_DEMAND(Q.rows() == 3);
+  DRAKE_DEMAND(Q.cols() == 3);
 
-  centroidal_ref_traj_ = std::move(ref_traj);
-  Q_cent_ = Q;
+  com_ref_traj_ = std::move(ref_traj);
+  Q_com_ = Q;
+}
+
+void KinematicCentroidalMPC::AddMomentumReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
+                                                      const Eigen::MatrixXd &Q) {
+  DRAKE_DEMAND(Q.rows() == 6);
+  DRAKE_DEMAND(Q.cols() == 6);
+  mom_ref_traj_ = std::move(ref_traj);
+  Q_mom_ = Q;
 }
 
 void KinematicCentroidalMPC::AddCentroidalDynamics() {
@@ -190,31 +198,34 @@ void KinematicCentroidalMPC::AddConstantForceTrackingReferenceCost(const drake::
   AddForceTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q_force);
 }
 
-void KinematicCentroidalMPC::AddConstantCentroidalReferenceCost(const drake::VectorX<double> &value,
-                                                                const Eigen::MatrixXd &Q) {
-  AddCentroidalReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q);
+void KinematicCentroidalMPC::AddConstantComReferenceCost(const drake::VectorX<double> &value,
+                                                         const Eigen::MatrixXd &Q) {
+  AddComReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q);
+}
+
+void KinematicCentroidalMPC::AddConstantMomentumReferenceCost(const drake::VectorX<double> &value,
+                                                              const Eigen::MatrixXd &Q) {
+  AddMomentumReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q);
 }
 
 void KinematicCentroidalMPC::AddCosts() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     double t = dt_ * knot_point;
     if(ref_traj_){
-      const auto& ref = ref_traj_->value(t);
-      prog_->AddQuadraticCost(Q_,  -Q_ * ref , state_vars(knot_point));
+      prog_->AddQuadraticErrorCost(Q_, ref_traj_->value(t), state_vars(knot_point));
     }
-//    if(centroidal_ref_traj_){
-//      const auto& ref = centroidal_ref_traj_->value(t);
-//      prog_->AddQuadraticCost(Q_cent_,  -Q_cent_ * ref , {centroidal_pos_vars(knot_point), centroidal_vel_vars(knot_point)});
-//    }
+    if(com_ref_traj_){
+      prog_->AddQuadraticErrorCost(Q_com_,  com_ref_traj_->value(t) , com_pos_vars(knot_point));
+    }
+    if(mom_ref_traj_){
+      prog_->AddQuadraticErrorCost(Q_mom_, mom_ref_traj_->value(t), momentum_vars(knot_point));
+    }
+
     if(contact_ref_traj_){
-      const auto& ref = contact_ref_traj_->value(t);
-      drake::solvers::VariableRefList contact_vars;
-      prog_->AddQuadraticCost(Q_contact_,  -Q_contact_ * ref , {contact_pos_[knot_point],contact_vel_[knot_point]});
+      prog_->AddQuadraticErrorCost(Q_contact_,  contact_ref_traj_->value(t) , {contact_pos_[knot_point],contact_vel_[knot_point]});
     }
     if(force_ref_traj_){
-      const auto& ref = force_ref_traj_->value(t);
-      drake::solvers::VariableRefList force_vars;
-      prog_->AddQuadraticCost(Q_force_, -Q_force_ * ref, contact_force_[knot_point]);
+      prog_->AddQuadraticErrorCost(Q_force_, force_ref_traj_->value(t), contact_force_[knot_point]);
     }
   }
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 8ad089a7a5..458c7f6963 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -60,8 +60,8 @@ class KinematicCentroidalMPC {
    * @param ref_traj trajectory in time
    * @param Q cost on error from reference
    */
-  void AddCentroidalReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                                  const Eigen::MatrixXd& Q);
+  void AddComReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
+                           const Eigen::MatrixXd& Q);
 
   /*!
    * @brief Adds a cost and reference for the contact position and velocity (x - x_ref)^T Q (x - x_ref)
@@ -71,6 +71,9 @@ class KinematicCentroidalMPC {
   void AddContactTrackingReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj,
                                        const Eigen::MatrixXd& Q_contact);
 
+  void AddMomentumReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
+                                       const Eigen::MatrixXd& Q);
+
   /*!
    * @brief Add a cost and reference for the contact forces (x - x_ref)^T Q (x - x_ref)
    * @param force_ref_traj trajectory in time
@@ -83,7 +86,9 @@ class KinematicCentroidalMPC {
 
   void AddConstantForceTrackingReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q_force);
 
-  void AddConstantCentroidalReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q);
+  void AddConstantComReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q);
+
+  void AddConstantMomentumReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q);
 
   /*!
    * @brief accessor for robot state decision vars
@@ -250,8 +255,10 @@ class KinematicCentroidalMPC {
   /// References and cost matrixes
   std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj_;
   Eigen::MatrixXd Q_;
-  std::unique_ptr<drake::trajectories::Trajectory<double>> centroidal_ref_traj_;
-  Eigen::MatrixXd Q_cent_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> com_ref_traj_;
+  Eigen::MatrixXd Q_com_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> mom_ref_traj_;
+  Eigen::MatrixXd Q_mom_;
   std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj_;
   Eigen::MatrixXd Q_contact_;
   std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj_;

From f07ffe0778ceecb396b58d8ee9c9ed44cc5a9331 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 19 Oct 2022 14:58:08 -0400
Subject: [PATCH 03/76] Cleaned up code

---
 .../cassie_kcmpc_trajopt.cc                   | 20 ++++++++++---------
 .../kinematic_centroidal_mpc.cc               | 11 +++++++++-
 .../kinematic_centroidal_mpc.h                |  5 +++++
 3 files changed, 26 insertions(+), 10 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 68c3a54c1c..f8e66e31dd 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -242,25 +242,25 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   std::cout<<"Setting initial guess"<<std::endl;
   mpc.SetZeroInitialGuess();
-  Eigen::VectorXd reference_state = GenerateNominalStand(plant, com_height, stance_width);
+  Eigen::VectorXd reference_state = GenerateNominalStand(plant, 1.9, stance_width);
   mpc.SetRobotStateGuess(reference_state);
 
   double cost_force = 0.001;
 
-  double cost_joint_pos = 0.01;
-  double cost_joint_vel = 0.02;
+  double cost_joint_pos = 0.001;
+  double cost_joint_vel = 0.002;
 
   double cost_contact_pos = 1;
   double cost_contact_vel = 2;
 
-  double cost_com_pos = 10;
+  double cost_com_pos = 40;
 
-  double cost_base_vel = 1;
+  double cost_base_vel = 0.001;
   double cost_orientation = 8;
   double cost_angular_vel = 0.01;
 
-  double cost_lin_mom = 0.1;
-  double cost_ang_mom = 0.1;
+  double cost_lin_mom = 0.0001;
+  double cost_ang_mom = 0.0001;
 
   double stance_wiggle = 0.01;
 
@@ -317,6 +317,8 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   Q_momentum.tail(3) = cost_lin_mom * Eigen::Vector3d::Ones();
   mpc.AddConstantMomentumReferenceCost(Eigen::VectorXd::Zero(6), Q_momentum.asDiagonal());
 
+//  mpc.AddComHeightBoundingConstraint(0.1,2);
+//  mpc.SetComPositionGuess({0, 0, com_height});
   std::cout<<"Adding solver options"<<std::endl;
   {
     drake::solvers::SolverOptions options;
@@ -325,7 +327,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
     options.SetOption(id, "dual_inf_tol", tol);
     options.SetOption(id, "constr_viol_tol", tol);
     options.SetOption(id, "compl_inf_tol", tol);
-    options.SetOption(id, "max_iter", 500);
+    options.SetOption(id, "max_iter", 2000);
     options.SetOption(id, "nlp_lower_bound_inf", -1e6);
     options.SetOption(id, "nlp_upper_bound_inf", 1e6);
     options.SetOption(id, "print_timing_statistics", "yes");
@@ -375,5 +377,5 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 }
 
 int main(int argc, char* argv[]) {
-  DoMain(10, 0.5, 1.9, 0.2, 0.5, 1e-3);
+  DoMain(10, 0.5, 0.9, 0.2, 0.5, 1e-3);
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 3e82b7fe52..e127ef8b31 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -220,7 +220,6 @@ void KinematicCentroidalMPC::AddCosts() {
     if(mom_ref_traj_){
       prog_->AddQuadraticErrorCost(Q_mom_, mom_ref_traj_->value(t), momentum_vars(knot_point));
     }
-
     if(contact_ref_traj_){
       prog_->AddQuadraticErrorCost(Q_contact_,  contact_ref_traj_->value(t) , {contact_pos_[knot_point],contact_vel_[knot_point]});
     }
@@ -341,3 +340,13 @@ void KinematicCentroidalMPC::SetRobotStateGuess(const drake::VectorX<double> &st
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::momentum_vars(int knotpoint_index) const {
   return mom_vars_[knotpoint_index];
 }
+void KinematicCentroidalMPC::AddComHeightBoundingConstraint(double lb, double ub) {
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++) {
+    prog_->AddBoundingBoxConstraint(lb, ub, com_pos_vars(knot_point)[2]);
+  }
+}
+void KinematicCentroidalMPC::SetComPositionGuess(const drake::Vector3<double> &state) {
+  for(const auto& com_pos : com_vars_){
+    prog_->SetInitialGuess(com_pos, state);
+  }
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 458c7f6963..4065919d93 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -176,6 +176,8 @@ class KinematicCentroidalMPC {
 
   void SetRobotStateGuess(const drake::VectorX<double>& state);
 
+  void SetComPositionGuess(const drake::Vector3<double>& state);
+
   void CreateVisualizationCallback(std::string model_file,
                                    double alpha,
                                    std::string weld_frame_to_world = "");
@@ -196,6 +198,9 @@ class KinematicCentroidalMPC {
 
   void AddKinematicConstraint(std::shared_ptr<dairlib::multibody::KinematicPositionConstraint<double>> con,
                               const Eigen::Ref<const drake::solvers::VectorXDecisionVariable>& vars);
+
+  void AddComHeightBoundingConstraint(double lb, double ub);
+
  private:
   /*!
    * @brief Adds dynamics for centroidal state

From d8e7101da6fd66b7090a557075e58acdf5cfda2b Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 20 Oct 2022 14:30:05 -0400
Subject: [PATCH 04/76] Updated saving of trajectories

---
 .../kinematic_centroidal_mpc.cc               | 27 +------------------
 .../kinematic_centroidal_mpc.h                |  3 ---
 2 files changed, 1 insertion(+), 29 deletions(-)

diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 761ad16899..e04a23ae40 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -271,32 +271,20 @@ bool KinematicCentroidalMPC::SaveSolutionToFile(const std::string& filepath){
   // check if there is a solution
   DRAKE_DEMAND(result_ != nullptr);
   Eigen::MatrixXd state_points;
-  Eigen::MatrixXd centroidal_state_points;
-  Eigen::MatrixXd contact_pos_points;
-  Eigen::MatrixXd contact_vel_points;
   Eigen::MatrixXd contact_force_points;
   std::vector<double> time_samples;
   this->SetFromSolution(*result_,
                        &state_points,
-                       &centroidal_state_points,
-                       &contact_pos_points,
-                       &contact_vel_points,
                        &contact_force_points,
                        &time_samples);
 
   dairlib::LcmTrajectory::Trajectory state_traj;
-  dairlib::LcmTrajectory::Trajectory centroidal_state_traj;
   dairlib::LcmTrajectory::Trajectory contact_force_traj;
   state_traj.traj_name = "state_traj";
   state_traj.datapoints = state_points;
   state_traj.time_vector = Eigen::Map<VectorXd>(time_samples.data(), time_samples.size());
   state_traj.datatypes = dairlib::multibody::CreateStateNameVectorFromMap(plant_);
 
-  centroidal_state_traj.traj_name = "centroidal_state_traj";
-  centroidal_state_traj.datapoints = centroidal_state_points;
-  centroidal_state_traj.time_vector = Eigen::Map<VectorXd>(time_samples.data(), time_samples.size());
-  centroidal_state_traj.datatypes = std::vector<std::string>(centroidal_state_traj.datapoints.rows());
-
   contact_force_traj.traj_name = "contact_force_traj";
   contact_force_traj.datapoints = contact_force_points;
   contact_force_traj.time_vector = Eigen::Map<VectorXd>(time_samples.data(), time_samples.size());
@@ -304,10 +292,8 @@ bool KinematicCentroidalMPC::SaveSolutionToFile(const std::string& filepath){
 
   std::vector<dairlib::LcmTrajectory::Trajectory> trajectories;
   trajectories.push_back(state_traj);
-  trajectories.push_back(centroidal_state_traj);
   trajectories.push_back(contact_force_traj);
   std::vector<std::string> trajectory_names = {state_traj.traj_name,
-                                               centroidal_state_traj.traj_name,
                                                contact_force_traj.traj_name};
   LcmTrajectory lcm_trajectory = LcmTrajectory(trajectories,
                 trajectory_names,
@@ -322,20 +308,13 @@ bool KinematicCentroidalMPC::SaveSolutionToFile(const std::string& filepath){
 void KinematicCentroidalMPC::SetFromSolution(
     const drake::solvers::MathematicalProgramResult& result,
     Eigen::MatrixXd* state_samples,
-    Eigen::MatrixXd* centroidal_samples,
-    Eigen::MatrixXd* contact_pos_samples,
-    Eigen::MatrixXd* contact_vel_samples,
     Eigen::MatrixXd* contact_force_samples,
     std::vector<double>* time_samples) const {
   DRAKE_ASSERT(state_samples != nullptr);
-  DRAKE_ASSERT(centroidal_samples != nullptr);
-  DRAKE_ASSERT(contact_pos_samples != nullptr);
-  DRAKE_ASSERT(contact_vel_samples != nullptr);
-  DRAKE_ASSERT(contact_vel_samples != nullptr);
+  DRAKE_ASSERT(contact_force_samples != nullptr);
   DRAKE_ASSERT(time_samples->empty());
 
   *state_samples = MatrixXd(n_q_ + n_v_, n_knot_points_);
-  *centroidal_samples = MatrixXd(kCentroidalPosDim + kCentroidalVelDim, n_knot_points_);
   *contact_force_samples = MatrixXd(3 * n_contact_points_, n_knot_points_);
   time_samples->resize(n_knot_points_);
 
@@ -343,12 +322,8 @@ void KinematicCentroidalMPC::SetFromSolution(
     time_samples->at(knot_point) = knot_point * dt_;
 
     VectorXd x = result.GetSolution(state_vars(knot_point));
-    VectorXd x_cent = result.GetSolution(x_cent_vars_[knot_point]);
-//    VectorXd contact_pos = result.GetSolution(state_vars(knot_point));
-//    VectorXd contact_vel = result.GetSolution(input_vars(knot_point));
     VectorXd contact_force = result.GetSolution(contact_force_.at(knot_point));
     state_samples->col(knot_point) = x;
-    centroidal_samples->col(knot_point) = x_cent;
     contact_force_samples->col(knot_point) = contact_force;
   }
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index d8e1fa1efd..43d52ba5fc 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -183,9 +183,6 @@ class KinematicCentroidalMPC {
   void SetFromSolution(
       const drake::solvers::MathematicalProgramResult& result,
       Eigen::MatrixXd* state_samples,
-      Eigen::MatrixXd* centroidal_samples,
-      Eigen::MatrixXd* contact_pos_samples,
-      Eigen::MatrixXd* contact_vel_samples,
       Eigen::MatrixXd* contact_force_samples,
       std::vector<double>* time_samples) const;
 

From 094e0b6a61c2251bc7833539a2f8a21d79ba78d6 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 19 Oct 2022 18:20:49 -0400
Subject: [PATCH 05/76] Switched to trapazoidal collocation

---
 .../cassie_kcmpc_trajopt.cc                   |  6 ++--
 .../kinematic_centroidal_constraints.cc       | 34 ++++++++++++++-----
 .../kinematic_centroidal_constraints.h        |  8 +++--
 .../kinematic_centroidal_mpc.cc               | 21 +++++++-----
 4 files changed, 46 insertions(+), 23 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 8bf45363d5..000b858dc3 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -317,8 +317,8 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   Q_momentum.tail(3) = cost_lin_mom * Eigen::Vector3d::Ones();
   mpc.AddConstantMomentumReferenceCost(Eigen::VectorXd::Zero(6), Q_momentum.asDiagonal());
 
-//  mpc.AddComHeightBoundingConstraint(0.1,2);
-//  mpc.SetComPositionGuess({0, 0, com_height});
+  mpc.AddComHeightBoundingConstraint(0.1,2);
+  mpc.SetComPositionGuess({0, 0, com_height});
   std::cout<<"Adding solver options"<<std::endl;
   {
     drake::solvers::SolverOptions options;
@@ -378,5 +378,5 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 }
 
 int main(int argc, char* argv[]) {
-  DoMain(10, 0.5, 0.9, 0.2, 0.5, 1e-3);
+  DoMain(10, 0.5, 0.95, 0.2, 0, 1e-3);
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
index 67b2195ada..99f9458185 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
@@ -9,7 +9,7 @@ CentroidalDynamicsConstraint<T>::CentroidalDynamicsConstraint(const drake::multi
                                                               int n_contact,
                                                               double dt,
                                                               int knot_index): dairlib::solvers::NonlinearConstraint<T>(
-    6,  2 * 6 + 3 + 2 * 3 * n_contact,
+    6,  2 * 6 + 2 * (3 + 2 * 3 * n_contact),
     Eigen::VectorXd::Zero(6),
     Eigen::VectorXd::Zero(6),
     "momentum_collocation[" +
@@ -27,6 +27,10 @@ CentroidalDynamicsConstraint<T>::CentroidalDynamicsConstraint(const drake::multi
 ///   - com0, location of com at time k
 ///   - cj0, contact locations time k
 ///   - Fj0, contact forces at time k
+///   - com1, location of com at time k+1
+///   - cj1, contact locations time k+1
+///   - Fj1, contact forces at time k+1
+
 template <typename T>
 void CentroidalDynamicsConstraint<T>::EvaluateConstraint(
     const Eigen::Ref<const drake::VectorX<T>>& x, drake::VectorX<T>* y) const {
@@ -36,11 +40,15 @@ void CentroidalDynamicsConstraint<T>::EvaluateConstraint(
   const auto& com0 = x.segment(2 * n_mom_, 3);
   const auto& cj0 = x.segment(2 * n_mom_ + 3, 3 * n_contact_);
   const auto& Fj0 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_, 3 * n_contact_);
+  const auto& com1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_, 3);
+  const auto& cj1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_+ 3, 3 * n_contact_);
+  const auto& Fj1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_+ 3 + 3 * n_contact_, 3 * n_contact_);
 
   drake::Vector<T, 6> xdot0Mom = CalcTimeDerivativesWithForce(com0, cj0, Fj0);
+  drake::Vector<T, 6> xdot1Mom = CalcTimeDerivativesWithForce(com1, cj1, Fj1);
 
   // Predict state and return error
-  const auto x1Predict = xMom0 + xdot0Mom * dt_;
+  const auto x1Predict = xMom0 + 0.5 * dt_ * (xdot0Mom + xdot1Mom);
   *y = xMom1 - x1Predict;
 }
 
@@ -69,16 +77,18 @@ drake::VectorX<T> CentroidalDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
 
 template<typename T>
 KinematicIntegratorConstraint<T>::KinematicIntegratorConstraint(const drake::multibody::MultibodyPlant<T> &plant,
-                                                                  drake::systems::Context<T> *context,
+                                                                  drake::systems::Context<T> *context0,
+                                                                  drake::systems::Context<T>* context1,
                                                                   double dt,
                                                                   int knot_index): dairlib::solvers::NonlinearConstraint<T>(
-    plant.num_positions(),  2 * plant.num_positions()+ plant.num_velocities(),
+    plant.num_positions(),  2 * plant.num_positions() + 2 * plant.num_velocities(),
     Eigen::VectorXd::Zero(plant.num_positions()),
     Eigen::VectorXd::Zero(plant.num_positions()),
     "generalized_velocity_integrator[" +
         std::to_string(knot_index) + "]"),
                                                                                    plant_(plant),
-                                                                                   context_(context),
+                                                                                   context0_(context0),
+                                                                                   context1_(context1),
                                                                                    n_q_(plant_.num_positions()),
                                                                                    n_v_(plant_.num_velocities()),
                                                                                    dt_(dt) {}
@@ -87,17 +97,23 @@ KinematicIntegratorConstraint<T>::KinematicIntegratorConstraint(const drake::mul
 ///   - q0, generalized position at time k
 ///   - q1, generalized position at time k + 1
 ///   - v0, generalized velocity at time k
+///   - v1, generalized velocity at time k + 1
 template<typename T>
 void KinematicIntegratorConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
                                                            drake::VectorX<T> *y) const {
   const auto& q0 = x.head(n_q_);
   const auto& q1 = x.segment(n_q_, n_q_);
-  const auto& v0 = x.tail(n_v_);
+  const auto& v0 = x.segment(n_q_ + n_q_, n_v_);
+  const auto& v1 = x.segment(n_q_ + n_q_ + n_v_, n_v_);
+
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, q0, context0_);
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, q0, context0_);
 
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, q0, context_);
   drake::VectorX<T> qdot0(n_q_);
-  plant_.MapVelocityToQDot(*context_, v0, &qdot0);
-  *y = q0 + dt_ * qdot0 - q1;
+  drake::VectorX<T> qdot1(n_q_);
+  plant_.MapVelocityToQDot(*context0_, v0, &qdot0);
+  plant_.MapVelocityToQDot(*context1_, v1, &qdot1);
+  *y = 0.5 * dt_ * (qdot0 + qdot1) + q0 - q1;
 }
 
 template<typename T>
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
index 6a94c6a7ae..8b2270f21d 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
@@ -59,8 +59,9 @@ class KinematicIntegratorConstraint : public dairlib::solvers::NonlinearConstrai
 
  public:
   KinematicIntegratorConstraint(const drake::multibody::MultibodyPlant<T>& plant,
-                                 drake::systems::Context<T>* context,
-                                 double dt,
+                                 drake::systems::Context<T>* context0,
+                                drake::systems::Context<T>* context1,
+                                double dt,
                                  int knot_index);
 
  public:
@@ -68,7 +69,8 @@ class KinematicIntegratorConstraint : public dairlib::solvers::NonlinearConstrai
                           drake::VectorX<T>* y) const override;
 
   const drake::multibody::MultibodyPlant<T>& plant_;
-  drake::systems::Context<T>* context_;
+  drake::systems::Context<T>* context0_;
+  drake::systems::Context<T>* context1_;
   int n_q_;
   int n_v_;
   double dt_;
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index e04a23ae40..9d9bd886ac 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -98,8 +98,11 @@ void KinematicCentroidalMPC::AddCentroidalDynamics() {
                                                                 {momentum_vars(knot_point),
                                                                  momentum_vars(knot_point + 1),
                                                                  com_pos_vars(knot_point),
-                                                                contact_pos_[knot_point],
-                                                                contact_force_[knot_point]}));
+                                                                 contact_pos_[knot_point],
+                                                                 contact_force_[knot_point],
+                                                                 com_pos_vars(knot_point+1),
+                                                                 contact_pos_[knot_point+1],
+                                                                 contact_force_[knot_point+1]}));
   }
 }
 
@@ -107,17 +110,19 @@ void KinematicCentroidalMPC::AddKinematicsIntegrator() {
   for (int knot_point = 0; knot_point < n_knot_points_ - 1; knot_point++) {
     // Integrate generalized velocities to get generalized positions
     auto constraint = std::make_shared<KinematicIntegratorConstraint<double>>(
-        plant_, contexts_[knot_point].get(), dt_,knot_point);
-    centroidal_dynamics_binding_.push_back(prog_->AddConstraint(constraint,
-                                                                {state_vars(knot_point).head(n_q_),
-                                                                 state_vars(knot_point+1).head(n_q_),
-                                                                 state_vars(knot_point).tail(n_v_)}));
+        plant_, contexts_[knot_point].get(), contexts_[knot_point + 1].get(), dt_, knot_point);
+    prog_->AddConstraint(constraint,
+                         {state_vars(knot_point).head(n_q_),
+                          state_vars(knot_point + 1).head(n_q_),
+                          state_vars(knot_point).tail(n_v_),
+                          state_vars(knot_point + 1).tail(n_v_)});
 
     // Integrate foot states
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
       prog_->AddConstraint(contact_pos_vars(knot_point + 1, contact_index)
                                == contact_pos_vars(knot_point, contact_index)
-                                   + dt_ * contact_vel_vars(knot_point, contact_index));
+                                   + 0.5 * dt_ * (contact_vel_vars(knot_point, contact_index) +
+                                       contact_vel_vars(knot_point + 1, contact_index)));
     }
   }
 }

From c0da82c78c6794d3fbce29b66338042ad047b471 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 19 Oct 2022 18:24:19 -0400
Subject: [PATCH 06/76] Setup timing

---
 .../Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc    | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 000b858dc3..92acba737a 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -378,5 +378,6 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 }
 
 int main(int argc, char* argv[]) {
-  DoMain(10, 0.5, 0.95, 0.2, 0, 1e-3);
+  // Assuming 2 cycles per second
+  DoMain(20, 1, 0.95, 0.2, 0, 1e-3);
 }

From 0b8130da993f03cb63c5e40d74f8adbf3e31087e Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 20 Oct 2022 11:13:30 -0400
Subject: [PATCH 07/76] Doing an alright job stepping in place

---
 .../cassie_kcmpc_trajopt.cc                   | 49 +++++++++------
 multibody/kinematic/kinematic_constraints.cc  |  2 +-
 .../kinematic_centroidal_mpc.cc               | 60 ++++++++++++++-----
 .../kinematic_centroidal_mpc.h                |  9 ++-
 4 files changed, 83 insertions(+), 37 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 92acba737a..b6d0ef82ca 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -178,19 +178,19 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   std::vector<int> toe_active_inds{0, 1, 2};
   std::vector<int> heel_active_inds{0, 1, 2};
 
-  auto left_toe_eval = std::make_shared<dairlib::multibody::WorldPointEvaluator<double>>(
+  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, left_toe_pair.first, left_toe_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), toe_active_inds);
 
-  auto left_heel_eval = std::make_shared<dairlib::multibody::WorldPointEvaluator<double>>(
+  auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, left_heel_pair.first, left_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), heel_active_inds);
 
-  auto right_toe_eval = std::make_shared<dairlib::multibody::WorldPointEvaluator<double>>(
+  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, right_toe_pair.first, right_toe_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), toe_active_inds);
 
-  auto right_heel_eval = std::make_shared<dairlib::multibody::WorldPointEvaluator<double>>(
+  auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, right_heel_pair.first, right_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), heel_active_inds);
 
@@ -245,17 +245,17 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   Eigen::VectorXd reference_state = GenerateNominalStand(plant, 1.9, stance_width);
   mpc.SetRobotStateGuess(reference_state);
 
-  double cost_force = 0.001;
+  double cost_force = 0.0001;
 
   double cost_joint_pos = 0.001;
   double cost_joint_vel = 0.002;
 
-  double cost_contact_pos = 1;
-  double cost_contact_vel = 2;
+  double cost_contact_pos = 0.1;
+  double cost_contact_vel = 0.2;
 
-  double cost_com_pos = 40;
+  double cost_com_pos = 160;
 
-  double cost_base_vel = 0.001;
+  double cost_base_vel = 0.1;
   double cost_orientation = 8;
   double cost_angular_vel = 0.01;
 
@@ -264,11 +264,11 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   double stance_wiggle = 0.01;
 
-  Eigen::Vector3d left_lb(std::numeric_limits<double>::lowest(), -stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
-  Eigen::Vector3d left_ub(std::numeric_limits<double>::max(), -stance_width/2+stance_wiggle, std::numeric_limits<double>::max());
+  Eigen::Vector3d left_lb(std::numeric_limits<double>::lowest(), stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
+  Eigen::Vector3d left_ub(std::numeric_limits<double>::max(), stance_width/2+stance_wiggle, std::numeric_limits<double>::max());
 
-  Eigen::Vector3d right_lb(std::numeric_limits<double>::lowest(), stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
-  Eigen::Vector3d right_ub(std::numeric_limits<double>::max(), stance_width/2+stance_wiggle, std::numeric_limits<double>::max());
+  Eigen::Vector3d right_lb(std::numeric_limits<double>::lowest(), -stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
+  Eigen::Vector3d right_ub(std::numeric_limits<double>::max(), -stance_width/2+stance_wiggle, std::numeric_limits<double>::max());
   mpc.AddContactPointPositionConstraint(0, left_lb, left_ub);
   mpc.AddContactPointPositionConstraint(1, left_lb, left_ub);
   mpc.AddContactPointPositionConstraint(2, right_lb, right_ub);
@@ -281,12 +281,10 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   ref_force[5] = 33*9.81/4;
   ref_force[8] = 33*9.81/4;
   ref_force[11] = 33*9.81/4;
-
   mpc.AddConstantForceTrackingReferenceCost(ref_force, Q_force.asDiagonal());
 
 
   Eigen::VectorXd Q_state = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities());
-
   Q_state.head(4) = cost_orientation * Eigen::VectorXd::Ones(4);
   Q_state.segment(7, plant.num_positions()-7) = cost_joint_pos * Eigen::VectorXd::Ones(plant.num_positions()-7);
   Q_state.segment(plant.num_positions(), 3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
@@ -319,6 +317,23 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   mpc.AddComHeightBoundingConstraint(0.1,2);
   mpc.SetComPositionGuess({0, 0, com_height});
+
+  std::vector<std::vector<bool>> mode_sequence(n_knot_points);
+  for(int i = 0; i < n_knot_points/4; i++){
+    mode_sequence[i] = {true, true, true, true};
+  }
+  for(int i = n_knot_points/4; i < 2 * n_knot_points/4; i++){
+    mode_sequence[i] = {true, true, false, false};
+  }
+  for(int i = 2 * n_knot_points/4; i < 3 * n_knot_points/4; i++){
+    mode_sequence[i] = {false, false, true, true};
+  }
+  for(int i = 3 * n_knot_points/4; i < n_knot_points; i++){
+    mode_sequence[i] = {true, true, true, true};
+  }
+  mpc.SetModeSequence(mode_sequence);
+  mpc.AddInitialStateConstraint(reference_state);
+
   std::cout<<"Adding solver options"<<std::endl;
   {
     drake::solvers::SolverOptions options;
@@ -371,7 +386,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(.4);
+    simulator.set_target_realtime_rate(.5);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
   }
@@ -379,5 +394,5 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
 int main(int argc, char* argv[]) {
   // Assuming 2 cycles per second
-  DoMain(20, 1, 0.95, 0.2, 0, 1e-3);
+  DoMain(40, 2, 0.95, 0.2, 0.0, 1e-3);
 }
diff --git a/multibody/kinematic/kinematic_constraints.cc b/multibody/kinematic/kinematic_constraints.cc
index 4b7b7ab01e..144a6ba3e2 100644
--- a/multibody/kinematic/kinematic_constraints.cc
+++ b/multibody/kinematic/kinematic_constraints.cc
@@ -58,7 +58,7 @@ void KinematicPositionConstraint<T>::EvaluateConstraint(
   // Add relative offsets, looping through the list of relative constraints
   auto it = full_constraint_relative_.begin();
   for (uint i = 0; i < full_constraint_relative_.size(); i++) {
-    (*y)(*it) += alpha(i);
+    (*y)(*it) -= alpha(i);
     it++;
   }
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 9d9bd886ac..d8b4fe46f2 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -14,7 +14,7 @@ using dairlib::LcmTrajectory;
 KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double> &plant,
                                                int n_knot_points,
                                                double dt,
-                                               const std::vector<std::shared_ptr<dairlib::multibody::WorldPointEvaluator<double>>>& contact_points):
+                                               const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& contact_points):
                                                plant_(plant),
                                                n_knot_points_(n_knot_points),
                                                dt_(dt),
@@ -22,7 +22,8 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
                                                n_q_(plant.num_positions()),
                                                n_v_(plant.num_velocities()),
                                                n_contact_points_(contact_points.size()),
-                                               contexts_(n_knot_points){
+                                               contexts_(n_knot_points),
+                                               contact_sequence_(n_knot_points){
 
   n_joint_q_ = n_q_ - kCentroidalPosDim;
   n_joint_v_ = n_v_ - kCentroidalVelDim;
@@ -31,7 +32,7 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
   result_ = std::make_unique<drake::solvers::MathematicalProgramResult>();
   for(int contact_index = 0; contact_index < n_contact_points_; contact_index ++){
     contact_sets_.emplace_back(plant_);
-    contact_sets_[contact_index].add_evaluator(contact_points_[contact_index].get());
+    contact_sets_[contact_index].add_evaluator(&contact_points_[contact_index]);
   }
 
   for(int knot = 0; knot < n_knot_points; knot ++){
@@ -43,6 +44,9 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
     contact_vel_.push_back(prog_->NewContinuousVariables(3 * n_contact_points_, "contact_vel_" + std::to_string(knot)));
     contact_force_.push_back(prog_->NewContinuousVariables(3 * n_contact_points_, "contact_force_" + std::to_string(knot)));
   }
+  std::vector<bool> stance_mode(n_contact_points_);
+  std::fill(stance_mode.begin(), stance_mode.end(), true);
+  std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
 }
 
 void KinematicCentroidalMPC::AddStateReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
@@ -131,13 +135,16 @@ void KinematicCentroidalMPC::AddContactConstraints() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
       //Make sure feet in stance are not moving and on the ground
-      //TODO replace with check to see if foot is in stance
-      if(true){
-        prog_->AddBoundingBoxConstraint(Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), contact_vel_vars(knot_point,contact_index));
-        prog_->AddBoundingBoxConstraint(0, 0, contact_pos_vars(knot_point,contact_index)[2]);
+      if (contact_sequence_[knot_point][contact_index]) {
+        prog_->AddBoundingBoxConstraint(Eigen::VectorXd::Zero(3),
+                                        Eigen::VectorXd::Zero(3),
+                                        contact_vel_vars(knot_point, contact_index));
+        if (knot_point != 0){
+          prog_->AddBoundingBoxConstraint(0, 0, contact_pos_vars(knot_point, contact_index)[2]);
+        }
       } else {
         // Feet are above the ground
-        prog_->AddBoundingBoxConstraint(0, 10, contact_pos_vars(knot_point,contact_index)[2]);
+        prog_->AddBoundingBoxConstraint(0.05, 10, contact_pos_vars(knot_point, contact_index)[2]);
       }
     }
   }
@@ -179,6 +186,19 @@ void KinematicCentroidalMPC::AddFrictionConeConstraints() {
     }
   }
 }
+
+void KinematicCentroidalMPC::AddFlightContactForceConstraints() {
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
+      // Feet in flight produce no force
+      if(!contact_sequence_[knot_point][contact_index]){
+        prog_->AddBoundingBoxConstraint(Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), contact_force_vars(knot_point,contact_index));
+      }
+    }
+  }
+
+}
+
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::state_vars(int knotpoint_index) const {
   return x_vars_[knotpoint_index];
 }
@@ -196,6 +216,7 @@ void KinematicCentroidalMPC::Build(const drake::solvers::SolverOptions &solver_o
   AddContactConstraints();
   AddCentroidalKinematicConsistency();
   AddFrictionConeConstraints();
+  AddFlightContactForceConstraints();
   AddCosts();
   prog_->SetSolverOptions(solver_options);
 }
@@ -340,26 +361,25 @@ void KinematicCentroidalMPC::CreateVisualizationCallback(std::string model_file,
   DRAKE_DEMAND(!callback_visualizer_);  // Cannot be set twice
 
   // Assemble variable list
-  drake::solvers::VectorXDecisionVariable vars(n_knot_points_ *
+  drake::solvers::VectorXDecisionVariable vars(n_knot_points_/2 *
   plant_.num_positions());
-  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
-    vars.segment(knot_point * plant_.num_positions(), plant_.num_positions()) = state_vars(knot_point).head(plant_.num_positions());
+  for(int knot_point = 0; knot_point < n_knot_points_/2; knot_point ++){
+    vars.segment(knot_point * plant_.num_positions(), plant_.num_positions()) = state_vars(knot_point*2).head(plant_.num_positions());
   }
-
-  Eigen::VectorXd alpha_vec = Eigen::VectorXd::Constant(n_knot_points_, alpha);
+  Eigen::VectorXd alpha_vec = Eigen::VectorXd::Constant(n_knot_points_/2, alpha);
   alpha_vec(0) = 1;
-  alpha_vec(n_knot_points_ - 1) = 1;
+  alpha_vec(n_knot_points_/2 - 1) = 1;
 
   // Create visualizer
   callback_visualizer_ = std::make_unique<dairlib::multibody::MultiposeVisualizer>(
-      model_file, n_knot_points_, alpha_vec, weld_frame_to_world);
+      model_file, n_knot_points_/2, alpha_vec, weld_frame_to_world);
 
 
   // Callback lambda function
   auto my_callback = [this](const Eigen::Ref<const Eigen::VectorXd>& vars) {
     Eigen::VectorXd vars_copy = vars;
     Eigen::Map<Eigen::MatrixXd> states(vars_copy.data(), this->plant_.num_positions(),
-                                       this->n_knot_points_);
+                                       this->n_knot_points_/2);
     this->callback_visualizer_->DrawPoses(states);
   };
 
@@ -423,3 +443,11 @@ void KinematicCentroidalMPC::SetComPositionGuess(const drake::Vector3<double> &s
     prog_->SetInitialGuess(com_pos, state);
   }
 }
+void KinematicCentroidalMPC::SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence) {
+  contact_sequence_ = contact_sequence;
+}
+void KinematicCentroidalMPC::AddInitialStateConstraint(const Eigen::VectorXd state) {
+  DRAKE_DEMAND(state.size() == state_vars(0).size());
+  prog_->AddBoundingBoxConstraint(state, state, state_vars((0)));
+
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 43d52ba5fc..397fbaa78d 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -43,7 +43,7 @@ class KinematicCentroidalMPC {
   KinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double>& plant,
                          int n_knot_points,
                          double dt,
-                         const std::vector<std::shared_ptr<dairlib::multibody::WorldPointEvaluator<double>>>& contact_points);
+                         const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& contact_points);
 
 
   /*!
@@ -221,6 +221,9 @@ class KinematicCentroidalMPC {
   int num_knot_points() const{
     return n_knot_points_;
   }
+  void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
+
+  void AddInitialStateConstraint(const Eigen::VectorXd state);
 
  private:
   /*!
@@ -267,7 +270,7 @@ class KinematicCentroidalMPC {
   int n_knot_points_;
   double dt_;
 
-  std::vector<std::shared_ptr<dairlib::multibody::WorldPointEvaluator<double>>> contact_points_;
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>> contact_points_;
   std::vector<dairlib::multibody::KinematicEvaluatorSet<double>> contact_sets_;
 
   static const int kCentroidalPosDim = 7;
@@ -291,7 +294,7 @@ class KinematicCentroidalMPC {
   Eigen::MatrixXd Q_force_;
 
 
-  std::vector<std::unordered_map<std::shared_ptr<dairlib::multibody::WorldPointEvaluator<double>>, bool>> contact_sequence_;
+  std::vector<std::vector<bool>> contact_sequence_;
   // MathematicalProgram
   std::unique_ptr<drake::solvers::MathematicalProgram> prog_;
   std::unique_ptr<drake::solvers::IpoptSolver> solver_;

From ac506c046f4154de6fe9ba136e336dea64988c26 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 20 Oct 2022 11:19:03 -0400
Subject: [PATCH 08/76] Dropped down gain and reduced knot points

---
 .../Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index b6d0ef82ca..e8df45284e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -253,7 +253,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   double cost_contact_pos = 0.1;
   double cost_contact_vel = 0.2;
 
-  double cost_com_pos = 160;
+  double cost_com_pos = 20;
 
   double cost_base_vel = 0.1;
   double cost_orientation = 8;
@@ -394,5 +394,5 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
 int main(int argc, char* argv[]) {
   // Assuming 2 cycles per second
-  DoMain(40, 2, 0.95, 0.2, 0.0, 1e-3);
+  DoMain(20, 2, 0.95, 0.2, 0.0, 1e-3);
 }

From 0315fe077727af37269d1c1b400aeb86541f4aff Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 20 Oct 2022 16:18:35 -0400
Subject: [PATCH 09/76] Fixed bazel run

---
 .../Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index e8df45284e..e32e24869b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -365,7 +365,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   std::cout << "Solving optimization\n\n";
   const auto pp_xtraj = mpc.Solve();
-  mpc.SaveSolutionToFile("examples/Cassie/saved_trajectories/kcmpc_solution");
+  mpc.SaveSolutionToFile(std::string(getenv("HOME")) + "/workspace/dairlib/examples/Cassie/saved_trajectories/kcmpc_solution");
 
   auto traj_source =
       builder.AddSystem<drake::systems::TrajectorySource>(pp_xtraj);

From c2c91e2d94d06614b4d54fffecc3d8b89885a5e7 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 20 Oct 2022 16:55:12 -0400
Subject: [PATCH 10/76] Added reference generator script

---
 .../kinematic_centroidal_mpc/BUILD.bazel      |  13 ++
 .../cassie_kcmpc_trajopt.cc                   | 131 +---------------
 .../reference_generator.cc                    | 144 ++++++++++++++++++
 .../reference_generator.h                     |  10 ++
 4 files changed, 168 insertions(+), 130 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/reference_generator.h

diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
index a620b5c153..e58de70087 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
@@ -22,6 +22,7 @@ cc_binary(
         "//solvers:optimization_utils",
         "//systems/primitives",
         "//systems/controllers/kinematic_centroidal_mpc",
+        "//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_reference_generator",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -54,3 +55,15 @@ cc_library(
         "@drake//:drake_shared_library",
     ],
 )
+
+cc_library(
+    name = "kinematic_centroidal_reference_generator",
+    srcs = ["reference_generator.cc"],
+    hdrs = ["reference_generator.h"],
+    deps = [
+        "//examples/Cassie:cassie_utils",
+        "//common",
+        "//multibody:visualization_utils",
+        "@drake//:drake_shared_library",
+    ],
+)
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index e32e24869b..05e0533fcc 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -8,148 +8,19 @@
 #include <drake/systems/rendering/multibody_position_to_geometry_pose.h>
 #include <drake/systems/analysis/simulator.h>
 #include <drake/geometry/drake_visualizer.h>
-#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 #include <drake/solvers/solve.h>
 #include "common/find_resource.h"
 #include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "systems/primitives/subvector_pass_through.h"
-#include "multibody/visualization_utils.h"
 #include "multibody/kinematic/kinematic_constraints.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/reference_generator.h"
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
 using drake::multibody::Parser;
 using drake::geometry::DrakeVisualizer;
 
-Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
-                                     double pelvis_height,
-                                     double stance_width,
-                                     bool visualize = false) {
-  using Eigen::VectorXd;
-  using Eigen::Vector3d;
-  int n_q = plant.num_positions();
-  int n_v = plant.num_velocities();
-  int n_x = n_q + n_v;
-  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
-
-  Eigen::VectorXd q_ik_guess = Eigen::VectorXd::Zero(n_q);
-
-  std::map<std::string, double> pos_value_map;
-  Eigen::Vector4d quat(2000.06, -0.339462, -0.609533, -0.760854);
-  quat.normalize();
-  pos_value_map["base_qw"] = quat(0);
-  pos_value_map["base_qx"] = quat(1);
-  pos_value_map["base_qy"] = quat(2);
-  pos_value_map["base_qz"] = quat(3);
-  pos_value_map["base_x"] = 0.000889849;
-  pos_value_map["base_y"] = 0.000626865;
-  pos_value_map["base_z"] = 1.0009;
-  pos_value_map["hip_roll_left"] = 0.00927845;
-  pos_value_map["hip_roll_right"] = 0.00927845;
-  pos_value_map["hip_yaw_left"] = -0.000895805;
-  pos_value_map["hip_yaw_right"] = 0.000895805;
-  pos_value_map["hip_pitch_left"] = 0.610808;
-  pos_value_map["hip_pitch_right"] = 0.610808;
-  pos_value_map["knee_left"] = -1.35926;
-  pos_value_map["knee_right"] = -1.35926;
-  pos_value_map["ankle_joint_left"] = 1.00716;
-  pos_value_map["ankle_joint_right"] = 1.00716;
-  pos_value_map["toe_left"] = -M_PI / 2;
-  pos_value_map["toe_right"] = -M_PI / 2;
-
-  for (auto pair : pos_value_map) {
-    q_ik_guess(positions_map.at(pair.first)) = pair.second;
-  }
-
-  Eigen::Vector3d heel_rt_toe = {.122, 0, 0};
-
-  Eigen::Vector3d pelvis_pos = {0,0, pelvis_height};
-  Eigen::Vector3d l_toe_pos = {0.06, stance_width/2, 0};
-  Eigen::Vector3d r_toe_pos = {0.06, -stance_width/2, 0};
-
-  Eigen::Vector3d l_heel_pos = l_toe_pos - heel_rt_toe;
-  Eigen::Vector3d r_heel_pos = r_toe_pos-heel_rt_toe;
-
-
-  const auto& world_frame = plant.world_frame();
-  const auto& pelvis_frame = plant.GetFrameByName("pelvis");
-  const auto& toe_left_frame = plant.GetFrameByName("toe_left");
-  const auto& toe_right_frame = plant.GetFrameByName("toe_right");
-
-  drake::multibody::InverseKinematics ik(plant);
-  double eps = 1e-3;
-  Vector3d eps_vec = eps * VectorXd::Ones(3);
-  ik.AddPositionConstraint(pelvis_frame, Vector3d(0, 0, 0), world_frame,
-                           pelvis_pos - eps * VectorXd::Ones(3),
-                           pelvis_pos + eps * VectorXd::Ones(3));
-  ik.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
-                              world_frame, drake::math::RotationMatrix<double>(), eps);
-  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, world_frame,
-                           l_toe_pos - eps_vec,
-                           l_toe_pos + eps_vec);
-  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first, world_frame,
-                           l_heel_pos - eps_vec,
-                           l_heel_pos + eps_vec);
-
-  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first, world_frame,
-                           r_toe_pos - eps_vec, r_toe_pos + eps_vec);
-  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first, world_frame,
-                           r_heel_pos - eps_vec, r_heel_pos + eps_vec);
-
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("hip_yaw_left")) == 0);
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("hip_yaw_right")) == 0);
-  // Four bar linkage constraint (without spring)
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("knee_left")) +
-          (ik.q())(positions_map.at("ankle_joint_left")) ==
-          M_PI * 13 / 180.0);
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("knee_right")) +
-          (ik.q())(positions_map.at("ankle_joint_right")) ==
-          M_PI * 13 / 180.0);
-
-  ik.get_mutable_prog()->SetInitialGuess(ik.q(), q_ik_guess);
-  const auto result = drake::solvers::Solve(ik.prog());
-  const auto q_sol = result.GetSolution(ik.q());
-  VectorXd q_sol_normd(n_q);
-  q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q - 4);
-  q_ik_guess = q_sol_normd;
-
-  if(visualize){
-    // Build temporary diagram for visualization
-    drake::systems::DiagramBuilder<double> builder_ik;
-    SceneGraph<double>& scene_graph_ik = *builder_ik.AddSystem<SceneGraph>();
-    scene_graph_ik.set_name("scene_graph_ik");
-    MultibodyPlant<double> plant_ik(0.0);
-    Parser parser(&plant_ik, &scene_graph_ik);
-    std::string full_name =
-        dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
-    parser.AddModelFromFile(full_name);
-    plant_ik.Finalize();
-
-    // Visualize
-    VectorXd x_const = VectorXd::Zero(n_x);
-    x_const.head(n_q) = q_sol;
-    drake::trajectories::PiecewisePolynomial<double> pp_xtraj(x_const);
-
-    dairlib::multibody::ConnectTrajectoryVisualizer(&plant_ik, &builder_ik,
-                                           &scene_graph_ik, pp_xtraj);
-    auto diagram = builder_ik.Build();
-    drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(.1);
-    simulator.Initialize();
-    simulator.AdvanceTo(1.0);
-  }
-
-  Eigen::VectorXd rv = Eigen::VectorXd::Zero(n_x);
-  rv.head(n_q) = q_ik_guess;
-  return rv;
-}
-
-
 void DoMain(int n_knot_points, double duration, double com_height, double stance_width, double squat_distance, double tol){
   // Create fix-spring Cassie MBP
   drake::systems::DiagramBuilder<double> builder;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
new file mode 100644
index 0000000000..76ec7dffe6
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
@@ -0,0 +1,144 @@
+
+
+#include "reference_generator.h"
+#include <drake/systems/framework/diagram_builder.h>
+#include <drake/geometry/scene_graph.h>
+#include <drake/multibody/parsing/parser.h>
+#include <drake/multibody/plant/multibody_plant.h>
+#include <iostream>
+#include <drake/systems/analysis/simulator.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
+#include <drake/solvers/solve.h>
+#include "examples/Cassie/cassie_utils.h"
+#include "multibody/visualization_utils.h"
+
+using drake::geometry::SceneGraph;
+using drake::multibody::MultibodyPlant;
+using drake::multibody::Parser;
+
+Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
+                                     double pelvis_height,
+                                     double stance_width,
+                                     bool visualize) {
+  using Eigen::VectorXd;
+  using Eigen::Vector3d;
+  int n_q = plant.num_positions();
+  int n_v = plant.num_velocities();
+  int n_x = n_q + n_v;
+  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
+
+  Eigen::VectorXd q_ik_guess = Eigen::VectorXd::Zero(n_q);
+
+  std::map<std::string, double> pos_value_map;
+  Eigen::Vector4d quat(2000.06, -0.339462, -0.609533, -0.760854);
+  quat.normalize();
+  pos_value_map["base_qw"] = quat(0);
+  pos_value_map["base_qx"] = quat(1);
+  pos_value_map["base_qy"] = quat(2);
+  pos_value_map["base_qz"] = quat(3);
+  pos_value_map["base_x"] = 0.000889849;
+  pos_value_map["base_y"] = 0.000626865;
+  pos_value_map["base_z"] = 1.0009;
+  pos_value_map["hip_roll_left"] = 0.00927845;
+  pos_value_map["hip_roll_right"] = 0.00927845;
+  pos_value_map["hip_yaw_left"] = -0.000895805;
+  pos_value_map["hip_yaw_right"] = 0.000895805;
+  pos_value_map["hip_pitch_left"] = 0.610808;
+  pos_value_map["hip_pitch_right"] = 0.610808;
+  pos_value_map["knee_left"] = -1.35926;
+  pos_value_map["knee_right"] = -1.35926;
+  pos_value_map["ankle_joint_left"] = 1.00716;
+  pos_value_map["ankle_joint_right"] = 1.00716;
+  pos_value_map["toe_left"] = -M_PI / 2;
+  pos_value_map["toe_right"] = -M_PI / 2;
+
+  for (auto pair : pos_value_map) {
+    q_ik_guess(positions_map.at(pair.first)) = pair.second;
+  }
+
+  Eigen::Vector3d heel_rt_toe = {.122, 0, 0};
+
+  Eigen::Vector3d pelvis_pos = {0,0, pelvis_height};
+  Eigen::Vector3d l_toe_pos = {0.06, stance_width/2, 0};
+  Eigen::Vector3d r_toe_pos = {0.06, -stance_width/2, 0};
+
+  Eigen::Vector3d l_heel_pos = l_toe_pos - heel_rt_toe;
+  Eigen::Vector3d r_heel_pos = r_toe_pos-heel_rt_toe;
+
+
+  const auto& world_frame = plant.world_frame();
+  const auto& pelvis_frame = plant.GetFrameByName("pelvis");
+  const auto& toe_left_frame = plant.GetFrameByName("toe_left");
+  const auto& toe_right_frame = plant.GetFrameByName("toe_right");
+
+  drake::multibody::InverseKinematics ik(plant);
+  double eps = 1e-3;
+  Vector3d eps_vec = eps * VectorXd::Ones(3);
+  ik.AddPositionConstraint(pelvis_frame, Vector3d(0, 0, 0), world_frame,
+                           pelvis_pos - eps * VectorXd::Ones(3),
+                           pelvis_pos + eps * VectorXd::Ones(3));
+  ik.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
+                              world_frame, drake::math::RotationMatrix<double>(), eps);
+  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, world_frame,
+                           l_toe_pos - eps_vec,
+                           l_toe_pos + eps_vec);
+  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first, world_frame,
+                           l_heel_pos - eps_vec,
+                           l_heel_pos + eps_vec);
+
+  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first, world_frame,
+                           r_toe_pos - eps_vec, r_toe_pos + eps_vec);
+  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first, world_frame,
+                           r_heel_pos - eps_vec, r_heel_pos + eps_vec);
+
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("hip_yaw_left")) == 0);
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("hip_yaw_right")) == 0);
+  // Four bar linkage constraint (without spring)
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("knee_left")) +
+          (ik.q())(positions_map.at("ankle_joint_left")) ==
+          M_PI * 13 / 180.0);
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("knee_right")) +
+          (ik.q())(positions_map.at("ankle_joint_right")) ==
+          M_PI * 13 / 180.0);
+
+  ik.get_mutable_prog()->SetInitialGuess(ik.q(), q_ik_guess);
+  const auto result = drake::solvers::Solve(ik.prog());
+  const auto q_sol = result.GetSolution(ik.q());
+  VectorXd q_sol_normd(n_q);
+  q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q - 4);
+  q_ik_guess = q_sol_normd;
+
+  if(visualize){
+    // Build temporary diagram for visualization
+    drake::systems::DiagramBuilder<double> builder_ik;
+    drake::geometry::SceneGraph<double>& scene_graph_ik = *builder_ik.AddSystem<drake::geometry::SceneGraph>();
+    scene_graph_ik.set_name("scene_graph_ik");
+    MultibodyPlant<double> plant_ik(0.0);
+    Parser parser(&plant_ik, &scene_graph_ik);
+    std::string full_name =
+        dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+    parser.AddModelFromFile(full_name);
+    plant_ik.Finalize();
+
+    // Visualize
+    VectorXd x_const = VectorXd::Zero(n_x);
+    x_const.head(n_q) = q_sol;
+    drake::trajectories::PiecewisePolynomial<double> pp_xtraj(x_const);
+
+    dairlib::multibody::ConnectTrajectoryVisualizer(&plant_ik, &builder_ik,
+                                                    &scene_graph_ik, pp_xtraj);
+    auto diagram = builder_ik.Build();
+    drake::systems::Simulator<double> simulator(*diagram);
+    simulator.set_target_realtime_rate(.1);
+    simulator.Initialize();
+    simulator.AdvanceTo(1.0);
+  }
+
+  Eigen::VectorXd rv = Eigen::VectorXd::Zero(n_x);
+  rv.head(n_q) = q_ik_guess;
+  return rv;
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
new file mode 100644
index 0000000000..9cb064db09
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
@@ -0,0 +1,10 @@
+
+#pragma once
+
+#include <drake/multibody/plant/multibody_plant.h>
+#include <Eigen/Core>
+
+Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
+                                     double pelvis_height,
+                                     double stance_width,
+                                     bool visualize = false);
\ No newline at end of file

From 91c16cf1366c3bcd3714f3acfc7cd28722f9513e Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 20 Oct 2022 23:42:20 -0400
Subject: [PATCH 11/76] Created cassie specific optimization class

---
 examples/Cassie/cassie_utils.cc               | 28 ++++++
 examples/Cassie/cassie_utils.h                |  1 +
 .../kinematic_centroidal_mpc/BUILD.bazel      | 15 +++-
 .../cassie_kcmpc_trajopt.cc                   | 88 ++-----------------
 .../cassie_kinematic_centroidal_mpc.cc        | 46 ++++++++++
 .../cassie_kinematic_centroidal_mpc.h         | 29 ++++++
 .../kinematic_centroidal_mpc.cc               |  8 +-
 .../kinematic_centroidal_mpc.h                |  1 +
 8 files changed, 131 insertions(+), 85 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h

diff --git a/examples/Cassie/cassie_utils.cc b/examples/Cassie/cassie_utils.cc
index 26e74cca4e..7fd6f66728 100644
--- a/examples/Cassie/cassie_utils.cc
+++ b/examples/Cassie/cassie_utils.cc
@@ -230,6 +230,34 @@ const systems::GearedMotor& AddMotorModel(
   return *cassie_motor;
 }
 
+std::vector<std::string> JointNames(){
+  // create joint/motor names
+  std::vector<std::pair<std::string, std::string>> l_r_pairs{
+      std::pair<std::string, std::string>("_left", "_right"),
+      std::pair<std::string, std::string>("_right", "_left"),
+  };
+  std::vector<std::string> asy_joint_names{
+      "hip_roll",
+      "hip_yaw",
+  };
+  std::vector<std::string> sym_joint_names{"hip_pitch", "knee", "ankle_joint", "toe"};
+  std::vector<std::string> joint_names{};
+  std::vector<std::string> motor_names{};
+  for (auto &l_r_pair : l_r_pairs) {
+    for (auto & asy_joint_name : asy_joint_names) {
+      joint_names.push_back(asy_joint_name + l_r_pair.first);
+      motor_names.push_back(asy_joint_name + l_r_pair.first + "_motor");
+    }
+    for (unsigned int i = 0; i < sym_joint_names.size(); i++) {
+      joint_names.push_back(sym_joint_names[i] + l_r_pair.first);
+      if (sym_joint_names[i].compare("ankle_joint") != 0) {
+        motor_names.push_back(sym_joint_names[i] + l_r_pair.first + "_motor");
+      }
+    }
+  }
+  return  joint_names;
+}
+
 template std::pair<const Vector3d, const Frame<double>&> LeftToeFront(
     const MultibodyPlant<double>& plant);  // NOLINT
 template std::pair<const Vector3d, const Frame<double>&> RightToeFront(
diff --git a/examples/Cassie/cassie_utils.h b/examples/Cassie/cassie_utils.h
index 10e4cd564b..8ff5b3e935 100644
--- a/examples/Cassie/cassie_utils.h
+++ b/examples/Cassie/cassie_utils.h
@@ -89,4 +89,5 @@ const systems::GearedMotor& AddMotorModel(
     drake::systems::DiagramBuilder<double>* builder,
     const drake::multibody::MultibodyPlant<double>& plant);
 
+std::vector<std::string> JointNames();
 }  // namespace dairlib
diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
index e58de70087..ca6728d715 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
@@ -21,7 +21,7 @@ cc_binary(
         "//multibody/kinematic",
         "//solvers:optimization_utils",
         "//systems/primitives",
-        "//systems/controllers/kinematic_centroidal_mpc",
+        "//examples/Cassie/kinematic_centroidal_mpc:cassie_kinematic_centroidal_mpc",
         "//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_reference_generator",
         "@drake//:drake_shared_library",
         "@gflags",
@@ -67,3 +67,16 @@ cc_library(
         "@drake//:drake_shared_library",
     ],
 )
+
+cc_library(
+    name = "cassie_kinematic_centroidal_mpc",
+    srcs = ["cassie_kinematic_centroidal_mpc.cc"],
+    hdrs = ["cassie_kinematic_centroidal_mpc.h"],
+    deps = [
+        "//examples/Cassie:cassie_utils",
+        "//common",
+        "//systems/controllers/kinematic_centroidal_mpc",
+        "//multibody:visualization_utils",
+        "@drake//:drake_shared_library",
+    ],
+)
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 05e0533fcc..cfc5a320f7 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -10,11 +10,9 @@
 #include <drake/geometry/drake_visualizer.h>
 #include <drake/solvers/solve.h>
 #include "common/find_resource.h"
-#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h"
-#include "examples/Cassie/cassie_utils.h"
 #include "systems/primitives/subvector_pass_through.h"
-#include "multibody/kinematic/kinematic_constraints.h"
 #include "examples/Cassie/kinematic_centroidal_mpc/reference_generator.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h"
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
@@ -39,81 +37,13 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   parser_vis.AddModelFromFile(full_name);
   plant.Finalize();
   plant_vis.Finalize();
-  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
-
-  auto left_toe_pair = dairlib::LeftToeFront(plant);
-  auto left_heel_pair = dairlib::LeftToeRear(plant);
-  auto right_toe_pair = dairlib::RightToeFront(plant);
-  auto right_heel_pair = dairlib::RightToeRear(plant);
-
-  std::vector<int> toe_active_inds{0, 1, 2};
-  std::vector<int> heel_active_inds{0, 1, 2};
-
-  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, left_toe_pair.first, left_toe_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), toe_active_inds);
-
-  auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, left_heel_pair.first, left_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), heel_active_inds);
-
-  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, right_toe_pair.first, right_toe_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), toe_active_inds);
-
-  auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, right_heel_pair.first, right_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), heel_active_inds);
 
   std::cout<<"Creating MPC"<<std::endl;
-  KinematicCentroidalMPC mpc (plant, n_knot_points, duration/(n_knot_points-1),
-                              {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval});
-
-  // create joint/motor names
-  std::vector<std::pair<std::string, std::string>> l_r_pairs{
-      std::pair<std::string, std::string>("_left", "_right"),
-      std::pair<std::string, std::string>("_right", "_left"),
-  };
-  std::vector<std::string> asy_joint_names{
-      "hip_roll",
-      "hip_yaw",
-  };
-  std::vector<std::string> sym_joint_names{"hip_pitch", "knee", "ankle_joint", "toe"};
-  std::vector<std::string> joint_names{};
-  std::vector<std::string> motor_names{};
-  for (auto &l_r_pair : l_r_pairs) {
-    for (unsigned int i = 0; i < asy_joint_names.size(); i++) {
-      joint_names.push_back(asy_joint_names[i] + l_r_pair.first);
-      motor_names.push_back(asy_joint_names[i] + l_r_pair.first + "_motor");
-    }
-    for (unsigned int i = 0; i < sym_joint_names.size(); i++) {
-      joint_names.push_back(sym_joint_names[i] + l_r_pair.first);
-      if (sym_joint_names[i].compare("ankle_joint") != 0) {
-        motor_names.push_back(sym_joint_names[i] + l_r_pair.first + "_motor");
-      }
-    }
-  }
-  mpc.AddPlantJointLimits(joint_names);
-
-  auto l_loop_evaluator = dairlib::LeftLoopClosureEvaluator(plant);
-  auto r_loop_evaluator = dairlib::RightLoopClosureEvaluator(plant);
-  dairlib::multibody::KinematicEvaluatorSet<double> evaluators(plant);
-  evaluators.add_evaluator(&l_loop_evaluator);
-  evaluators.add_evaluator(&r_loop_evaluator);
-
-  auto loop_closure =
-      std::make_shared<dairlib::multibody::KinematicPositionConstraint<double>>(
-          plant,
-          evaluators,
-          Eigen::VectorXd::Zero(2),
-          Eigen::VectorXd::Zero(2));
-  for(int knot_point = 0; knot_point < n_knot_points; knot_point ++){
-    mpc.AddKinematicConstraint(loop_closure, mpc.state_vars(knot_point).head(plant.num_positions()));
-  }
+  CassieKinematicCentroidalMPC mpc (plant, n_knot_points, duration/(n_knot_points-1));
 
   std::cout<<"Setting initial guess"<<std::endl;
   mpc.SetZeroInitialGuess();
-  Eigen::VectorXd reference_state = GenerateNominalStand(plant, 1.9, stance_width);
+  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), 1.9, stance_width);
   mpc.SetRobotStateGuess(reference_state);
 
   double cost_force = 0.0001;
@@ -155,12 +85,12 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   mpc.AddConstantForceTrackingReferenceCost(ref_force, Q_force.asDiagonal());
 
 
-  Eigen::VectorXd Q_state = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities());
+  Eigen::VectorXd Q_state = Eigen::VectorXd::Zero(mpc.Plant().num_positions() + mpc.Plant().num_velocities());
   Q_state.head(4) = cost_orientation * Eigen::VectorXd::Ones(4);
-  Q_state.segment(7, plant.num_positions()-7) = cost_joint_pos * Eigen::VectorXd::Ones(plant.num_positions()-7);
-  Q_state.segment(plant.num_positions(), 3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
-  Q_state.segment(plant.num_positions() + 3, 3) = cost_base_vel * Eigen::VectorXd::Ones(3);
-  Q_state.tail(plant.num_velocities() - 6) = cost_joint_vel * Eigen::VectorXd::Ones(plant.num_velocities() - 6);
+  Q_state.segment(7, mpc.Plant().num_positions()-7) = cost_joint_pos * Eigen::VectorXd::Ones(mpc.Plant().num_positions()-7);
+  Q_state.segment(mpc.Plant().num_positions(), 3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
+  Q_state.segment(mpc.Plant().num_positions() + 3, 3) = cost_base_vel * Eigen::VectorXd::Ones(3);
+  Q_state.tail(mpc.Plant().num_velocities() - 6) = cost_joint_vel * Eigen::VectorXd::Ones(mpc.Plant().num_velocities() - 6);
   mpc.AddConstantStateReferenceCost(reference_state, Q_state.asDiagonal());
 
   Eigen::VectorXd reference_com = Eigen::VectorXd::Zero(3);
@@ -241,7 +171,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   auto traj_source =
       builder.AddSystem<drake::systems::TrajectorySource>(pp_xtraj);
   auto passthrough = builder.AddSystem<dairlib::systems::SubvectorPassThrough>(
-      plant.num_positions() + plant.num_velocities(), 0, plant.num_positions());
+      mpc.Plant().num_positions() + mpc.Plant().num_velocities(), 0, mpc.Plant().num_positions());
   builder.Connect(traj_source->get_output_port(),
                   passthrough->get_input_port());
   auto to_pose =
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
new file mode 100644
index 0000000000..e9c803886f
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -0,0 +1,46 @@
+#include "cassie_kinematic_centroidal_mpc.h"
+#include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/multibody/parsing/parser.h"
+#include "common/find_resource.h"
+#include "examples/Cassie/cassie_utils.h"
+
+std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalMPC::CreateContactPoints(const drake::multibody::MultibodyPlant<
+    double> &plant) {
+  auto left_toe_pair = dairlib::LeftToeFront(plant);
+  auto left_heel_pair = dairlib::LeftToeRear(plant);
+  auto right_toe_pair = dairlib::RightToeFront(plant);
+  auto right_heel_pair = dairlib::RightToeRear(plant);
+
+  std::vector<int> active_inds{0, 1, 2};
+
+  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, left_toe_pair.first, left_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, left_heel_pair.first, left_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, right_toe_pair.first, right_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, right_heel_pair.first, right_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  return {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval};
+}
+void CassieKinematicCentroidalMPC::AddLoopClosure() {
+  loop_closure_evaluators.add_evaluator(&l_loop_evaluator_);
+  loop_closure_evaluators.add_evaluator(&r_loop_evaluator_);
+  auto loop_closure =
+      std::make_shared<dairlib::multibody::KinematicPositionConstraint<double>>(
+          Plant(),
+          loop_closure_evaluators,
+          Eigen::VectorXd::Zero(2),
+          Eigen::VectorXd::Zero(2));
+  for (int knot_point = 1; knot_point < num_knot_points(); knot_point++) {
+    AddKinematicConstraint(loop_closure, state_vars(knot_point).head(Plant().num_positions()));
+  }
+
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
new file mode 100644
index 0000000000..15c47e9bb2
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -0,0 +1,29 @@
+#pragma once
+#include <iostream>
+#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h"
+#include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/multibody/plant/multibody_plant.h"
+#include "examples/Cassie/cassie_utils.h"
+
+/*!
+ * @brief Cassie specific child class for kinematic centroidal mpc.
+ */
+class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
+ public:
+  CassieKinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points, double dt) :
+      KinematicCentroidalMPC(plant, n_knot_points, dt, CreateContactPoints(plant)),
+      l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
+      r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
+      loop_closure_evaluators(Plant()){
+    AddPlantJointLimits(dairlib::JointNames());
+    AddLoopClosure();
+  }
+ private:
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant);
+  void AddLoopClosure();
+
+  dairlib::multibody::DistanceEvaluator<double> l_loop_evaluator_;
+  dairlib::multibody::DistanceEvaluator<double> r_loop_evaluator_;
+  dairlib::multibody::KinematicEvaluatorSet<double> loop_closure_evaluators;
+
+};
+
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index d8b4fe46f2..57bd09fdc6 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -24,7 +24,6 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
                                                n_contact_points_(contact_points.size()),
                                                contexts_(n_knot_points),
                                                contact_sequence_(n_knot_points){
-
   n_joint_q_ = n_q_ - kCentroidalPosDim;
   n_joint_v_ = n_v_ - kCentroidalVelDim;
   prog_ = std::make_unique<drake::solvers::MathematicalProgram>();
@@ -361,10 +360,9 @@ void KinematicCentroidalMPC::CreateVisualizationCallback(std::string model_file,
   DRAKE_DEMAND(!callback_visualizer_);  // Cannot be set twice
 
   // Assemble variable list
-  drake::solvers::VectorXDecisionVariable vars(n_knot_points_/2 *
-  plant_.num_positions());
+  drake::solvers::VectorXDecisionVariable vars(n_knot_points_/2 * n_q_);
   for(int knot_point = 0; knot_point < n_knot_points_/2; knot_point ++){
-    vars.segment(knot_point * plant_.num_positions(), plant_.num_positions()) = state_vars(knot_point*2).head(plant_.num_positions());
+    vars.segment(knot_point * n_q_, n_q_) = state_vars(knot_point*2).head(n_q_);
   }
   Eigen::VectorXd alpha_vec = Eigen::VectorXd::Constant(n_knot_points_/2, alpha);
   alpha_vec(0) = 1;
@@ -378,7 +376,7 @@ void KinematicCentroidalMPC::CreateVisualizationCallback(std::string model_file,
   // Callback lambda function
   auto my_callback = [this](const Eigen::Ref<const Eigen::VectorXd>& vars) {
     Eigen::VectorXd vars_copy = vars;
-    Eigen::Map<Eigen::MatrixXd> states(vars_copy.data(), this->plant_.num_positions(),
+    Eigen::Map<Eigen::MatrixXd> states(vars_copy.data(), this->n_q_,
                                        this->n_knot_points_/2);
     this->callback_visualizer_->DrawPoses(states);
   };
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 397fbaa78d..fc2240db9a 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -225,6 +225,7 @@ class KinematicCentroidalMPC {
 
   void AddInitialStateConstraint(const Eigen::VectorXd state);
 
+  const drake::multibody::MultibodyPlant<double>& Plant(){return plant_;};
  private:
   /*!
    * @brief Adds dynamics for centroidal state

From 0763b3e758a18156cdfb4e08a5aaf35593c67cd2 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 21 Oct 2022 14:32:46 -0400
Subject: [PATCH 12/76] Added gains yaml

---
 .../kinematic_centroidal_mpc/BUILD.bazel      |  3 +-
 .../cassie_kcmpc_trajopt.cc                   | 38 +++--------
 .../kinematic_centroidal_mpc_gains.yaml       | 34 ++++++++++
 .../kinematic_centroidal_mpc/BUILD.bazel      |  9 +++
 .../kinematic_centroidal_gains.h              | 27 ++++++++
 .../kinematic_centroidal_mpc.cc               | 67 ++++++++++++++++++-
 .../kinematic_centroidal_mpc.h                |  3 +
 7 files changed, 149 insertions(+), 32 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h

diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
index ca6728d715..9b06fa9ca3 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
@@ -1,5 +1,6 @@
 package(default_visibility = ["//visibility:public"])
 
+exports_files(["kinematic_centroidal_mpc_gains.yaml"])
 
 cc_library(
     name = "centroidal_mpc",
@@ -13,7 +14,7 @@ cc_library(
 cc_binary(
     name = "cassie_kcmpc_trajopt",
     srcs = ["cassie_kcmpc_trajopt.cc"],
-    data = glob(["examples/Cassie/urdf/cassie_fixed_springs.urdf"]),
+    data = ["//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_mpc_gains.yaml"],
     deps = [
         "//examples/Cassie:cassie_utils",
         "//common",
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index cfc5a320f7..376dfbc2d4 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -9,10 +9,12 @@
 #include <drake/systems/analysis/simulator.h>
 #include <drake/geometry/drake_visualizer.h>
 #include <drake/solvers/solve.h>
+#include <drake/common/yaml/yaml_io.h>
 #include "common/find_resource.h"
 #include "systems/primitives/subvector_pass_through.h"
 #include "examples/Cassie/kinematic_centroidal_mpc/reference_generator.h"
 #include "examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h"
+#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h"
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
@@ -20,6 +22,7 @@ using drake::multibody::Parser;
 using drake::geometry::DrakeVisualizer;
 
 void DoMain(int n_knot_points, double duration, double com_height, double stance_width, double squat_distance, double tol){
+  auto gains = drake::yaml::LoadYamlFile<KinematicCentroidalGains>("examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml");
   // Create fix-spring Cassie MBP
   drake::systems::DiagramBuilder<double> builder;
   SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
@@ -46,23 +49,6 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), 1.9, stance_width);
   mpc.SetRobotStateGuess(reference_state);
 
-  double cost_force = 0.0001;
-
-  double cost_joint_pos = 0.001;
-  double cost_joint_vel = 0.002;
-
-  double cost_contact_pos = 0.1;
-  double cost_contact_vel = 0.2;
-
-  double cost_com_pos = 20;
-
-  double cost_base_vel = 0.1;
-  double cost_orientation = 8;
-  double cost_angular_vel = 0.01;
-
-  double cost_lin_mom = 0.0001;
-  double cost_ang_mom = 0.0001;
-
   double stance_wiggle = 0.01;
 
   Eigen::Vector3d left_lb(std::numeric_limits<double>::lowest(), stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
@@ -76,7 +62,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   mpc.AddContactPointPositionConstraint(3, right_lb, right_ub);
 
 
-  Eigen::VectorXd Q_force = cost_force * Eigen::VectorXd::Ones(12);
+  Eigen::VectorXd Q_force = Eigen::VectorXd::Zero(12);
   Eigen::VectorXd ref_force = Eigen::VectorXd::Zero(12);
   ref_force[2] = 33*9.81/4;
   ref_force[5] = 33*9.81/4;
@@ -86,11 +72,6 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
 
   Eigen::VectorXd Q_state = Eigen::VectorXd::Zero(mpc.Plant().num_positions() + mpc.Plant().num_velocities());
-  Q_state.head(4) = cost_orientation * Eigen::VectorXd::Ones(4);
-  Q_state.segment(7, mpc.Plant().num_positions()-7) = cost_joint_pos * Eigen::VectorXd::Ones(mpc.Plant().num_positions()-7);
-  Q_state.segment(mpc.Plant().num_positions(), 3) = cost_angular_vel * Eigen::VectorXd::Ones(3);
-  Q_state.segment(mpc.Plant().num_positions() + 3, 3) = cost_base_vel * Eigen::VectorXd::Ones(3);
-  Q_state.tail(mpc.Plant().num_velocities() - 6) = cost_joint_vel * Eigen::VectorXd::Ones(mpc.Plant().num_velocities() - 6);
   mpc.AddConstantStateReferenceCost(reference_state, Q_state.asDiagonal());
 
   Eigen::VectorXd reference_com = Eigen::VectorXd::Zero(3);
@@ -101,19 +82,18 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   auto com_reference = drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points,
                                                                                                {reference_com,
                                                                                                 reference_com_bottom});
-  Eigen::VectorXd Q_com = cost_com_pos * Eigen::VectorXd::Ones(3);
+  Eigen::VectorXd Q_com = Eigen::VectorXd::Zero(3);
   mpc.AddComReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_reference),
                           Q_com.asDiagonal());
 
-  Eigen::VectorXd Q_contact = cost_contact_pos * Eigen::VectorXd::Ones(4 * 6);
-  Q_contact.tail(4 * 3) = cost_contact_vel * Eigen::VectorXd::Ones(4 * 3);
+  Eigen::VectorXd Q_contact = Eigen::VectorXd::Zero(4 * 6);
   mpc.AddContactTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
       4 * 6)), Q_contact.asDiagonal());
 
 
   Eigen::VectorXd Q_momentum(6);
-  Q_momentum.head(3) = cost_ang_mom * Eigen::Vector3d::Ones();
-  Q_momentum.tail(3) = cost_lin_mom * Eigen::Vector3d::Ones();
+  Q_momentum.head(3) =  Eigen::Vector3d::Zero();
+  Q_momentum.tail(3) =  Eigen::Vector3d::Zero();
   mpc.AddConstantMomentumReferenceCost(Eigen::VectorXd::Zero(6), Q_momentum.asDiagonal());
 
   mpc.AddComHeightBoundingConstraint(0.1,2);
@@ -134,7 +114,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   }
   mpc.SetModeSequence(mode_sequence);
   mpc.AddInitialStateConstraint(reference_state);
-
+  mpc.SetGains(gains);
   std::cout<<"Adding solver options"<<std::endl;
   {
     drake::solvers::SolverOptions options;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
new file mode 100644
index 0000000000..9d7c510c71
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
@@ -0,0 +1,34 @@
+com_position: [2, 2, 40]
+generalized_positions:
+  base_qw: 8.0
+  base_qx: 8.0
+  base_qy: 8.0
+  base_qz: 8.0
+  base_x: 0
+  base_y: 0
+  base_z: 0
+  hip_roll_: 0.0001
+  hip_yaw_: 0.0001
+  hip_pitch_: 0.0001
+  knee_: 0.0001
+  ankle_joint_: 0.0001
+  toe_: 0.0001
+generalized_velocities:
+  base_wx: 0.01
+  base_wy: 0.01
+  base_wz: 0.01
+  base_vx: 0.1
+  base_vy: 0.1
+  base_vz: 0.1
+  hip_roll_: 0.002
+  hip_yaw_: 0.002
+  hip_pitch_: 0.002
+  knee_: 0.002
+  ankle_joint_: 0.002
+  toe_: 0.002
+
+contact_pos: [0.1, 0.1, 0.1]
+contact_vel: [0.2, 0.2, 0.2]
+contact_force: [0.0001, 0.0001, 0.0001]
+lin_momentum: [0.0001, 0.0001, 0.0001]
+ang_momentum: [0.0001, 0.0001, 0.0001]
diff --git a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
index d67eb893e4..222d6f5c4b 100644
--- a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
+++ b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
@@ -12,7 +12,16 @@ cc_library(
         "//multibody/kinematic:kinematic",
         "//multibody/kinematic:constraints",
         "//multibody:multipose_visualizer",
+        "//systems/controllers/kinematic_centroidal_mpc:kcmpc_gains",
         "//lcm:lcm_trajectory_saver",
         "@drake//:drake_shared_library",
     ],
 )
+
+cc_library(
+    name = "kcmpc_gains",
+    hdrs = ["kinematic_centroidal_gains.h"],
+    deps = [
+        "@drake//:drake_shared_library",
+    ],
+)
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
new file mode 100644
index 0000000000..46901e5fa4
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
@@ -0,0 +1,27 @@
+#pragma once
+#include "drake/common/yaml/yaml_read_archive.h"
+#include "yaml-cpp/yaml.h"
+#include "include/_usr_include_eigen3/Eigen/Core"
+
+struct KinematicCentroidalGains{
+  Eigen::Vector3d com_position;
+  std::unordered_map<std::string, double> generalized_positions;
+  std::unordered_map<std::string, double> generalized_velocities;
+  Eigen::Vector3d contact_pos;
+  Eigen::Vector3d contact_vel;
+  Eigen::Vector3d contact_force;
+  Eigen::Vector3d lin_momentum;
+  Eigen::Vector3d ang_momentum;
+
+  template <typename Archive>
+  void Serialize(Archive* a) {
+    a->Visit(DRAKE_NVP(com_position));
+    a->Visit(DRAKE_NVP(generalized_positions));
+    a->Visit(DRAKE_NVP(generalized_velocities));
+    a->Visit(DRAKE_NVP(contact_pos));
+    a->Visit(DRAKE_NVP(contact_vel));
+    a->Visit(DRAKE_NVP(contact_force));
+    a->Visit(DRAKE_NVP(lin_momentum));
+    a->Visit(DRAKE_NVP(ang_momentum));
+  }
+};
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 57bd09fdc6..23ebf0a9ad 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -22,8 +22,13 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
                                                n_q_(plant.num_positions()),
                                                n_v_(plant.num_velocities()),
                                                n_contact_points_(contact_points.size()),
-                                               contexts_(n_knot_points),
-                                               contact_sequence_(n_knot_points){
+                                               Q_(Eigen::MatrixXd::Zero(n_q_ + n_v_, n_q_ + n_v_)),
+                                               Q_com_(Eigen::MatrixXd::Zero(3, 3)),
+                                               Q_mom_(Eigen::MatrixXd::Zero(6, 6)),
+                                               Q_contact_(Eigen::MatrixXd::Zero(6 * n_contact_points_, 6 * n_contact_points_)),
+                                               Q_force_(Eigen::MatrixXd::Zero(3 * n_contact_points_, 3 * n_contact_points_)),
+                                               contact_sequence_(n_knot_points),
+                                               contexts_(n_knot_points){
   n_joint_q_ = n_q_ - kCentroidalPosDim;
   n_joint_v_ = n_v_ - kCentroidalVelDim;
   prog_ = std::make_unique<drake::solvers::MathematicalProgram>();
@@ -449,3 +454,61 @@ void KinematicCentroidalMPC::AddInitialStateConstraint(const Eigen::VectorXd sta
   prog_->AddBoundingBoxConstraint(state, state, state_vars((0)));
 
 }
+void KinematicCentroidalMPC::SetGains(const KinematicCentroidalGains &gains) {
+  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant_);
+  std::map<std::string, int> velocities_map = dairlib::multibody::MakeNameToVelocitiesMap(plant_);
+
+  // Generalize state
+  for(const auto&[name, index] : positions_map){
+    const auto it = gains.generalized_positions.find(name);
+    if(it != gains.generalized_positions.end()){
+      Q_(index, index) = it->second;
+    } else if(name.find("left") != std::string::npos) {
+      const auto it_left = gains.generalized_positions.find(name.substr(0, name.size()-4));
+      if(it_left != gains.generalized_positions.end()) {
+        Q_(index, index) = it_left->second;
+      }
+    } else if(name.find("right") != std::string::npos) {
+      const auto it_right = gains.generalized_positions.find(name.substr(0, name.size() - 5));
+      if (it_right != gains.generalized_positions.end()) {
+        Q_(index, index) = it_right->second;
+      }
+    }
+  }
+
+  for(const auto&[name, index] : velocities_map){
+    const auto it = gains.generalized_velocities.find(name);
+    if(it != gains.generalized_velocities.end()){
+      Q_(index+n_q_, index+n_q_) = it->second;
+    } else if(name.find("left") != std::string::npos) {
+      const auto it_left = gains.generalized_velocities.find(name.substr(0, name.size()-7));
+      if(it_left != gains.generalized_velocities.end()) {
+        Q_(index+n_q_, index+n_q_) = it_left->second;
+      }
+    }else if(name.find("right") != std::string::npos){
+      const auto it_right = gains.generalized_velocities.find(name.substr(0, name.size()-8));
+      if(it_right != gains.generalized_velocities.end()) {
+        Q_(index+n_q_, index+n_q_) = it_right->second;
+      }
+    }
+  }
+
+  //com
+  Q_com_ = gains.com_position.asDiagonal();
+
+  //momentum
+  Q_mom_.block<3,3>(0, 0, 3, 3) = gains.ang_momentum.asDiagonal();
+  Q_mom_.block<3,3>(3, 3, 3, 3) = gains.lin_momentum.asDiagonal();
+
+  //contact pos, contact vel
+  Q_contact_.block<Eigen::Dynamic, Eigen::Dynamic>(0, 0, 3 * n_contact_points_, 3 * n_contact_points_) =
+      gains.contact_pos.replicate(n_contact_points_, 1).asDiagonal();
+  Q_contact_.block<Eigen::Dynamic, Eigen::Dynamic>(3 * n_contact_points_,
+                                                   3 * n_contact_points_,
+                                                   3 * n_contact_points_,
+                                                   3 * n_contact_points_) =
+      gains.contact_vel.replicate(n_contact_points_, 1).asDiagonal();
+
+  //contact force
+  Q_force_ = gains.contact_force.replicate(n_contact_points_, 1).asDiagonal();
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index fc2240db9a..a91d684d26 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -9,6 +9,7 @@
 #include "multibody/kinematic/kinematic_evaluator_set.h"
 #include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h"
 #include "multibody/kinematic/kinematic_constraints.h"
+#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h"
 
 /*!
  * @brief Class for solving nonlinear kinematic centroidal mpc. Implementation is based on
@@ -46,6 +47,8 @@ class KinematicCentroidalMPC {
                          const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& contact_points);
 
 
+  void SetGains(const KinematicCentroidalGains& gains);
+
   /*!
    * @brief Adds a cost reference for the state of the robot of the form (x - x_ref)^T Q (x - x_ref)
    * @param ref_traj trajectory in time

From 563e1a1f5f1b4d0f36e20682e741b47693433156 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 21 Oct 2022 14:50:52 -0400
Subject: [PATCH 13/76] Removed extra function calls

---
 .../cassie_kcmpc_trajopt.cc                   | 26 +++------
 .../kinematic_centroidal_mpc.cc               | 54 +++++--------------
 .../kinematic_centroidal_mpc.h                | 27 ++++------
 3 files changed, 29 insertions(+), 78 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 376dfbc2d4..eb2db8c096 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -60,19 +60,14 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   mpc.AddContactPointPositionConstraint(1, left_lb, left_ub);
   mpc.AddContactPointPositionConstraint(2, right_lb, right_ub);
   mpc.AddContactPointPositionConstraint(3, right_lb, right_ub);
-
-
-  Eigen::VectorXd Q_force = Eigen::VectorXd::Zero(12);
+  
   Eigen::VectorXd ref_force = Eigen::VectorXd::Zero(12);
   ref_force[2] = 33*9.81/4;
   ref_force[5] = 33*9.81/4;
   ref_force[8] = 33*9.81/4;
   ref_force[11] = 33*9.81/4;
-  mpc.AddConstantForceTrackingReferenceCost(ref_force, Q_force.asDiagonal());
-
-
-  Eigen::VectorXd Q_state = Eigen::VectorXd::Zero(mpc.Plant().num_positions() + mpc.Plant().num_velocities());
-  mpc.AddConstantStateReferenceCost(reference_state, Q_state.asDiagonal());
+  mpc.AddConstantForceTrackingReference(ref_force);
+  mpc.AddConstantStateReference(reference_state);
 
   Eigen::VectorXd reference_com = Eigen::VectorXd::Zero(3);
   reference_com[2] = com_height;
@@ -82,19 +77,12 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   auto com_reference = drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points,
                                                                                                {reference_com,
                                                                                                 reference_com_bottom});
-  Eigen::VectorXd Q_com = Eigen::VectorXd::Zero(3);
-  mpc.AddComReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_reference),
-                          Q_com.asDiagonal());
-
-  Eigen::VectorXd Q_contact = Eigen::VectorXd::Zero(4 * 6);
-  mpc.AddContactTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
-      4 * 6)), Q_contact.asDiagonal());
+  mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_reference));
 
+  mpc.AddContactTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
+      4 * 6)));
+  mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
 
-  Eigen::VectorXd Q_momentum(6);
-  Q_momentum.head(3) =  Eigen::Vector3d::Zero();
-  Q_momentum.tail(3) =  Eigen::Vector3d::Zero();
-  mpc.AddConstantMomentumReferenceCost(Eigen::VectorXd::Zero(6), Q_momentum.asDiagonal());
 
   mpc.AddComHeightBoundingConstraint(0.1,2);
   mpc.SetComPositionGuess({0, 0, com_height});
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 23ebf0a9ad..baa670991e 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -53,49 +53,24 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
   std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
 }
 
-void KinematicCentroidalMPC::AddStateReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                                                   const Eigen::MatrixXd &Q) {
-  // Ensure matrix is square and has correct number of rows and columns
-  DRAKE_DEMAND(Q.rows() == n_q_ + n_v_);
-  DRAKE_DEMAND(Q.cols() == n_q_ + n_v_);
-
+void KinematicCentroidalMPC::AddStateReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
   ref_traj_ = std::move(ref_traj);
-  Q_ = Q;
 }
 
-void KinematicCentroidalMPC::AddContactTrackingReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj,
-                                                             const Eigen::MatrixXd &Q_contact) {
-  DRAKE_DEMAND(Q_contact.rows() == 2 * 3 * n_contact_points_);
-  DRAKE_DEMAND(Q_contact.cols() == 2 * 3 * n_contact_points_);
-
+void KinematicCentroidalMPC::AddContactTrackingReference(std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj) {
   contact_ref_traj_ = std::move(contact_ref_traj);
-  Q_contact_ = Q_contact;
 }
 
-void KinematicCentroidalMPC::AddForceTrackingReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj,
-                                                           const Eigen::MatrixXd &Q_force) {
-  DRAKE_DEMAND(Q_force.rows() == 3 * n_contact_points_);
-  DRAKE_DEMAND(Q_force.cols() == 3 * n_contact_points_);
-
+void KinematicCentroidalMPC::AddForceTrackingReference(std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj) {
   force_ref_traj_ = std::move(force_ref_traj);
-  Q_force_ = Q_force;
 }
 
-void KinematicCentroidalMPC::AddComReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                                                 const Eigen::MatrixXd &Q) {
-  DRAKE_DEMAND(Q.rows() == 3);
-  DRAKE_DEMAND(Q.cols() == 3);
-
+void KinematicCentroidalMPC::AddComReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
   com_ref_traj_ = std::move(ref_traj);
-  Q_com_ = Q;
 }
 
-void KinematicCentroidalMPC::AddMomentumReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                                                      const Eigen::MatrixXd &Q) {
-  DRAKE_DEMAND(Q.rows() == 6);
-  DRAKE_DEMAND(Q.cols() == 6);
+void KinematicCentroidalMPC::AddMomentumReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
   mom_ref_traj_ = std::move(ref_traj);
-  Q_mom_ = Q;
 }
 
 void KinematicCentroidalMPC::AddCentroidalDynamics() {
@@ -225,23 +200,20 @@ void KinematicCentroidalMPC::Build(const drake::solvers::SolverOptions &solver_o
   prog_->SetSolverOptions(solver_options);
 }
 
-void KinematicCentroidalMPC::AddConstantStateReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd &Q) {
-  AddStateReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q);
+void KinematicCentroidalMPC::AddConstantStateReference(const drake::VectorX<double>& value) {
+  AddStateReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
 }
 
-void KinematicCentroidalMPC::AddConstantForceTrackingReferenceCost(const drake::VectorX<double> &value,
-                                                                   const Eigen::MatrixXd &Q_force) {
-  AddForceTrackingReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q_force);
+void KinematicCentroidalMPC::AddConstantForceTrackingReference(const drake::VectorX<double> &value) {
+  AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
 }
 
-void KinematicCentroidalMPC::AddConstantComReferenceCost(const drake::VectorX<double> &value,
-                                                         const Eigen::MatrixXd &Q) {
-  AddComReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q);
+void KinematicCentroidalMPC::AddConstantComReference(const drake::VectorX<double> &value) {
+  AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
 }
 
-void KinematicCentroidalMPC::AddConstantMomentumReferenceCost(const drake::VectorX<double> &value,
-                                                              const Eigen::MatrixXd &Q) {
-  AddMomentumReferenceCost(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value), Q);
+void KinematicCentroidalMPC::AddConstantMomentumReference(const drake::VectorX<double> &value) {
+  AddMomentumReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
 }
 
 void KinematicCentroidalMPC::AddCosts() {
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index a91d684d26..26c940714b 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -52,45 +52,36 @@ class KinematicCentroidalMPC {
   /*!
    * @brief Adds a cost reference for the state of the robot of the form (x - x_ref)^T Q (x - x_ref)
    * @param ref_traj trajectory in time
-   * @param Q cost on error from the reference
    */
-  void AddStateReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                             const Eigen::MatrixXd& Q);
+  void AddStateReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
 
   /*!
    * @brief Adds a cost and reference for the centroidal state of the robot (x - x_ref)^T Q (x - x_ref)
    * @param ref_traj trajectory in time
-   * @param Q cost on error from reference
    */
-  void AddComReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                           const Eigen::MatrixXd& Q);
+  void AddComReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
 
   /*!
    * @brief Adds a cost and reference for the contact position and velocity (x - x_ref)^T Q (x - x_ref)
    * @param contact_ref_traj trajectory in time
-   * @param Q_contact cost on error from the reference
    */
-  void AddContactTrackingReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj,
-                                       const Eigen::MatrixXd& Q_contact);
+  void AddContactTrackingReference(std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj);
 
-  void AddMomentumReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj,
-                                       const Eigen::MatrixXd& Q);
+  void AddMomentumReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
 
   /*!
    * @brief Add a cost and reference for the contact forces (x - x_ref)^T Q (x - x_ref)
    * @param force_ref_traj trajectory in time
-   * @param Q_force cost on error from the reference
    */
-  void AddForceTrackingReferenceCost(std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj,
-                                     const Eigen::MatrixXd& Q_force);
+  void AddForceTrackingReference(std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj);
 
-  void AddConstantStateReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q);
+  void AddConstantStateReference(const drake::VectorX<double>& value);
 
-  void AddConstantForceTrackingReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q_force);
+  void AddConstantForceTrackingReference(const drake::VectorX<double>& value);
 
-  void AddConstantComReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q);
+  void AddConstantComReference(const drake::VectorX<double>& value);
 
-  void AddConstantMomentumReferenceCost(const drake::VectorX<double>& value, const Eigen::MatrixXd& Q);
+  void AddConstantMomentumReference(const drake::VectorX<double>& value);
 
   /*!
    * @brief accessor for robot state decision vars

From b1a9abfc2c85b215a491b6cb9b43d7b3374f2f3c Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Sun, 23 Oct 2022 17:51:27 -0400
Subject: [PATCH 14/76] Wrote the code necessary to generate references that
 may or may not work

---
 .../cassie_kcmpc_trajopt.cc                   |   3 +-
 .../reference_generator.cc                    | 130 ++++++++++++++++++
 .../reference_generator.h                     |  77 ++++++++++-
 .../kinematic_centroidal_constraints.cc       |   4 +-
 4 files changed, 210 insertions(+), 4 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index eb2db8c096..5367b099fa 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -60,7 +60,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   mpc.AddContactPointPositionConstraint(1, left_lb, left_ub);
   mpc.AddContactPointPositionConstraint(2, right_lb, right_ub);
   mpc.AddContactPointPositionConstraint(3, right_lb, right_ub);
-  
+
   Eigen::VectorXd ref_force = Eigen::VectorXd::Zero(12);
   ref_force[2] = 33*9.81/4;
   ref_force[5] = 33*9.81/4;
@@ -100,6 +100,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   for(int i = 3 * n_knot_points/4; i < n_knot_points; i++){
     mode_sequence[i] = {true, true, true, true};
   }
+
   mpc.SetModeSequence(mode_sequence);
   mpc.AddInitialStateConstraint(reference_state);
   mpc.SetGains(gains);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
index 76ec7dffe6..90c7811f0d 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
@@ -142,3 +142,133 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
   rv.head(n_q) = q_ik_guess;
   return rv;
 }
+
+drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
+                                                              const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+                                                              const std::vector<double>& time_points){
+  DRAKE_DEMAND(vel_ewrt_w.size() == time_points.size());
+  auto n_points = vel_ewrt_w.size();
+
+  std::vector<drake::MatrixX<double>> samples(n_points);
+  samples[0] = current_com;
+  for(int i = 1; i<n_points; i++){
+    samples[i] = samples[i-1] + (time_points[i] - time_points[i-1]) * vel_ewrt_w[i-1];
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points, samples);
+}
+
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedStateTrajectory(const Eigen::VectorXd& nominal_stand,
+                                                                                    const Eigen::Vector3d& base_rt_com_ewrt_w,
+                                                                                    const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                    const std::vector<double>& time_points,
+                                                                                    int base_pos_start,
+                                                                                    int base_vel_start){
+  auto n_points = time_points.size();
+  std::vector<drake::MatrixX<double>> samples(n_points);
+  for(int i = 0; i < n_points; i++){
+    samples[i] = nominal_stand;
+    samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(time_points[i]);
+    samples[i].block<3,1>(base_vel_start,0, 3, 1) = com_traj.derivative().value(time_points[i]);
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points, samples);
+}
+
+int FindCurrentMode(const Gait& active_gait, double time_now){
+  double phase_now = fmod(time_now/active_gait.period, 1);
+  for(int i = 0; i < active_gait.gait_pattern.size(); i++){
+    const auto& mode = active_gait.gait_pattern[i];
+    if(mode.start_phase <= phase_now){
+      return i;
+    }
+  }
+  DRAKE_ASSERT(false);
+  return 0;
+}
+
+drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
+                                                                      const std::vector<double>& time_points){
+  DRAKE_DEMAND(gait_sequence.size() == time_points.size());
+
+  auto traj = gait_sequence[0].ToTrajectory(time_points[0], time_points[1]);
+  for(int i = 1; i<gait_sequence.size() - 1 ; i++){
+    traj.ConcatenateInTime(gait_sequence[i].ToTrajectory(time_points[i], time_points[i + 1]));
+  }
+  return traj;
+}
+
+drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double current_time, double end_time) const {
+  std::vector<double> break_points;
+  std::vector<drake::MatrixX<double>> samples;
+
+  // Calculate initial mode index, and phase
+  int current_mode = FindCurrentMode(*this, current_time);
+  double current_phase = fmod(current_time/period, 1);
+
+  // Loop until time is greater than end time
+  while(current_time < end_time){
+    // Add the break point for the current time
+    break_points.push_back(current_time);
+    samples.emplace_back(gait_pattern[current_mode].contact_status.cast<double>());
+
+    // Update time based on how much longer the current mode will last
+    current_time += (gait_pattern[current_mode].end_phase - current_phase) * period;
+    // Update the mode and mod if necessary
+    current_mode = (current_mode + 1) % gait_pattern.size();
+    // The new phase is the start phase of the updated mode
+    current_phase = gait_pattern[current_mode].start_phase;
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(break_points, samples);
+}
+
+double TimeNextValue(const drake::trajectories::PiecewisePolynomial<double>& trajectory,
+                     double current_time,
+                     int index,
+                     double value){
+  if (trajectory.value(current_time).coeff(index) == value)
+    return current_time;
+
+  const auto& segment_times = trajectory.get_segment_times();
+
+  for(int i = trajectory.get_segment_index(current_time) + 1; i<segment_times.size(); i++){
+    if(trajectory.value(segment_times[i]).coeff(i) == value)
+      return segment_times[i];
+  }
+  return trajectory.end_time();
+}
+
+drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
+                                                                      double m){
+  std::vector<drake::MatrixX<double>> samples;
+  const int n_contact_points = mode_trajectory.rows();
+
+  for(const auto& time: mode_trajectory.get_segment_times()){
+    const auto& mode = mode_trajectory.value(time);
+    double num_in_contact = mode.sum();
+    auto& grf = samples.emplace_back(Eigen::VectorXd::Zero(3 * n_contact_points));
+    for(int i = 0; i<n_contact_points; i++){
+      grf.coeffRef(2 + 3 * i) = m * 9.81 /num_in_contact;
+    }
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(mode_trajectory.get_segment_times(), samples);
+}
+
+drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(const drake::multibody::MultibodyPlant<double> &plant,
+                                                                               const std::vector<dairlib::multibody::WorldPointEvaluator<
+                                                                                   double>> &contacts,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &state_trajectory){
+  auto context =  plant.CreateDefaultContext();
+  std::vector<double> break_points = state_trajectory.get_segment_times();
+  break_points.emplace_back(state_trajectory.end_time());
+  std::vector<drake::MatrixX<double>> samples;
+  int n_contact = contacts.size();
+  for(const auto& time : break_points){
+    dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, state_trajectory.value(time), context.get());
+    drake::VectorX<double> knot_point_value = samples.emplace_back(Eigen::VectorXd::Zero(6 * n_contact));
+    for(int i = 0; i <n_contact; i++){
+      knot_point_value.segment(i * 3, i * 3 + 3) = contacts[i].EvalFull(*context);
+      knot_point_value.segment(3 * n_contact + i * 3, i * 3 + 3) = contacts[i].EvalFullJacobianDotTimesV(*context);
+    }
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(break_points, samples);
+}
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
index 9cb064db09..dcca852e57 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
@@ -3,8 +3,83 @@
 
 #include <drake/multibody/plant/multibody_plant.h>
 #include <Eigen/Core>
+#include <drake/common/trajectories/piecewise_polynomial.h>
+#include "multibody/kinematic/world_point_evaluator.h"
+
+struct Mode{
+  double start_phase;
+  double end_phase;
+  drake::VectorX<bool> contact_status;
+};
+
+struct Gait{
+  double period;
+  std::vector<Mode> gait_pattern;
+
+  drake::trajectories::PiecewisePolynomial<double> ToTrajectory(double current_time, double end_time) const;
+};
 
 Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
                                      double pelvis_height,
                                      double stance_width,
-                                     bool visualize = false);
\ No newline at end of file
+                                     bool visualize = false);
+
+/*!
+ * @brief Constructs a com trajectory given world frame velocity
+ * @param current_com current xyz location of com
+ * @param vel_ewrt_w vector of velocity commands
+ * @param time_points vector of times for velocity commands, start time is first entry, final time of spline is last entry
+ * @return spline describing com position
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
+                                                              const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+                                                              const std::vector<double>& time_points);
+
+
+/*!
+ * @brief Creates a trajectory in generalized state given a nominal state and com trajectory
+ * @param nominal_stand nominal generalized state of the robot
+ * @param base_rt_com_ewrt_w offset from base to com
+ * @param com_traj trajectory of center of mass
+ * @param time_points points to use to create trajectory
+ * @param base_pos_start index in generalized state where base position starts
+ * @param base_vel_start index in generalized state where base velocity starts
+ * @return center of mass trajectory
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedStateTrajectory(const Eigen::VectorXd& nominal_stand,
+                                                                                    const Eigen::Vector3d& base_rt_com_ewrt_w,
+                                                                                    const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                    const std::vector<double>& time_points,
+                                                                                    int base_pos_start,
+                                                                                    int base_vel_start);
+
+int FindCurrentMode(const Gait& active_gait, double time_now);
+
+drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
+                                                                      const std::vector<double>& time_points,
+                                                                      double duration);
+
+/*!
+ * @brief Calculates the next time a index of the zoh trajectory goes to a specific value
+ * @param trajectory piecewise polynomial trajectory that is a zoh
+ * @param current_time the time to start the search
+ * @param index index of the trajectory output to track
+ * @param value value searching for
+ * @return current time if trajectory(current_time)[index] = value,
+ *         the first time greater than current time and the value matches,
+ *         If the trajectory never goes to value, returns trajectory.end_time()
+ */
+double TimeNextValue(const drake::trajectories::PiecewisePolynomial<double>& trajectory,
+                     double current_time,
+                     int index,
+                     double value);
+
+
+drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
+                                                                      double m);
+
+drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(const drake::multibody::MultibodyPlant<double> &plant,
+                                                                               const std::vector<dairlib::multibody::WorldPointEvaluator<
+                                                                                   double>> &contacts,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &state_trajectory);
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
index 99f9458185..97e73e1777 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
@@ -106,8 +106,8 @@ void KinematicIntegratorConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
   const auto& v0 = x.segment(n_q_ + n_q_, n_v_);
   const auto& v1 = x.segment(n_q_ + n_q_ + n_v_, n_v_);
 
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, q0, context0_);
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, q0, context0_);
+  dairlib::multibody::SetPositionsIfNew<T>(plant_, q0, context0_);
+  dairlib::multibody::SetPositionsIfNew<T>(plant_, q0, context0_);
 
   drake::VectorX<T> qdot0(n_q_);
   drake::VectorX<T> qdot1(n_q_);

From d9aee3217d3cbaa5530d5258dda8f85973d39164 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Sun, 23 Oct 2022 21:18:43 -0400
Subject: [PATCH 15/76] Looks like a walk

---
 .../cassie_kcmpc_trajopt.cc                   | 97 +++++++++----------
 .../cassie_kinematic_centroidal_mpc.h         |  3 +-
 .../kinematic_centroidal_mpc_gains.yaml       |  6 +-
 .../reference_generator.cc                    | 22 +++--
 .../reference_generator.h                     |  5 +-
 .../kinematic_centroidal_mpc.cc               |  6 +-
 6 files changed, 68 insertions(+), 71 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 5367b099fa..5d3c82c727 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -41,69 +41,62 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   plant.Finalize();
   plant_vis.Finalize();
 
-  std::cout<<"Creating MPC"<<std::endl;
   CassieKinematicCentroidalMPC mpc (plant, n_knot_points, duration/(n_knot_points-1));
 
-  std::cout<<"Setting initial guess"<<std::endl;
   mpc.SetZeroInitialGuess();
   Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), 1.9, stance_width);
-  mpc.SetRobotStateGuess(reference_state);
-
-  double stance_wiggle = 0.01;
-
-  Eigen::Vector3d left_lb(std::numeric_limits<double>::lowest(), stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
-  Eigen::Vector3d left_ub(std::numeric_limits<double>::max(), stance_width/2+stance_wiggle, std::numeric_limits<double>::max());
-
-  Eigen::Vector3d right_lb(std::numeric_limits<double>::lowest(), -stance_width/2-stance_wiggle, std::numeric_limits<double>::lowest());
-  Eigen::Vector3d right_ub(std::numeric_limits<double>::max(), -stance_width/2+stance_wiggle, std::numeric_limits<double>::max());
-  mpc.AddContactPointPositionConstraint(0, left_lb, left_ub);
-  mpc.AddContactPointPositionConstraint(1, left_lb, left_ub);
-  mpc.AddContactPointPositionConstraint(2, right_lb, right_ub);
-  mpc.AddContactPointPositionConstraint(3, right_lb, right_ub);
-
-  Eigen::VectorXd ref_force = Eigen::VectorXd::Zero(12);
-  ref_force[2] = 33*9.81/4;
-  ref_force[5] = 33*9.81/4;
-  ref_force[8] = 33*9.81/4;
-  ref_force[11] = 33*9.81/4;
-  mpc.AddConstantForceTrackingReference(ref_force);
-  mpc.AddConstantStateReference(reference_state);
-
-  Eigen::VectorXd reference_com = Eigen::VectorXd::Zero(3);
-  reference_com[2] = com_height;
-  Eigen::VectorXd reference_com_bottom = Eigen::VectorXd::Zero(3);
-  reference_com_bottom[2] = com_height-squat_distance;
-  std::vector<double> time_points = {0, duration};
-  auto com_reference = drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points,
-                                                                                               {reference_com,
-                                                                                                reference_com_bottom});
-  mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_reference));
-
-  mpc.AddContactTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(Eigen::VectorXd::Zero(
-      4 * 6)));
-  mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
-
+  auto context = plant.CreateDefaultContext();
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
+  const auto& com = plant.CalcCenterOfMassPositionInWorld(*context);
+  const auto& mass = plant.CalcTotalMass(*context);
 
+  mpc.SetRobotStateGuess(reference_state);
+  mpc.AddInitialStateConstraint(reference_state);
   mpc.AddComHeightBoundingConstraint(0.1,2);
   mpc.SetComPositionGuess({0, 0, com_height});
+  mpc.SetGains(gains);
+
+  Gait stand;
+  stand.period = 1;
+  stand.gait_pattern = {{0, 1, drake::Vector<bool, 4>(true, true, true, true)}};
+
+  Gait walk;
+  walk.period = 0.8;
+  walk.gait_pattern = {{0, 0.4, drake::Vector<bool, 4>(true, true, false, false)},
+                        {0.4, 0.5, drake::Vector<bool, 4>(true, true, true, true)},
+                        {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
+                        {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}};
+
+  auto com_trajectory = GenerateComTrajectory({0, 0, com_height},
+                                              {{0, 0, 0},
+                                               {0.5, 0, 0},
+                                               {0, 0, 0},
+                                               {0, 0, 0}},
+                                              {0, duration/4, 3 * duration/4, duration});
+  auto state_trajectory = GenerateGeneralizedStateTrajectory(reference_state,
+                                                             reference_state.segment(4, 3) - com,
+                                                             com_trajectory,
+                                                             4,
+                                                             4 + plant.num_positions());
+  auto contact_sequence = GenerateModeSequence({stand, walk, stand, stand}, {0, duration/4, 3 * duration/4, duration});
+  auto grf_traj = GenerateGrfReference(contact_sequence, mass);
+  auto contact_traj = GenerateContactPointReference(plant,mpc.CreateContactPoints(plant), state_trajectory);
+  mpc.AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(grf_traj));
+  mpc.AddStateReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(state_trajectory));
+  mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_trajectory));
+  mpc.AddContactTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(contact_traj));
+  mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
 
   std::vector<std::vector<bool>> mode_sequence(n_knot_points);
-  for(int i = 0; i < n_knot_points/4; i++){
-    mode_sequence[i] = {true, true, true, true};
-  }
-  for(int i = n_knot_points/4; i < 2 * n_knot_points/4; i++){
-    mode_sequence[i] = {true, true, false, false};
-  }
-  for(int i = 2 * n_knot_points/4; i < 3 * n_knot_points/4; i++){
-    mode_sequence[i] = {false, false, true, true};
-  }
-  for(int i = 3 * n_knot_points/4; i < n_knot_points; i++){
-    mode_sequence[i] = {true, true, true, true};
+  const double dt = duration/(n_knot_points-1);
+
+  for(int knot_point = 0; knot_point < n_knot_points; knot_point ++){
+    for(int contact_index = 0; contact_index < 4; contact_index ++){
+      mode_sequence[knot_point].emplace_back(contact_sequence.value(dt * knot_point).coeff(contact_index));
+    }
   }
 
   mpc.SetModeSequence(mode_sequence);
-  mpc.AddInitialStateConstraint(reference_state);
-  mpc.SetGains(gains);
   std::cout<<"Adding solver options"<<std::endl;
   {
     drake::solvers::SolverOptions options;
@@ -164,5 +157,5 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
 int main(int argc, char* argv[]) {
   // Assuming 2 cycles per second
-  DoMain(20, 2, 0.95, 0.2, 0.0, 1e-3);
+  DoMain(40, 4, 0.95, 0.2, 0.0, 1e-3);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 15c47e9bb2..6a4a019c8e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -17,8 +17,9 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
     AddPlantJointLimits(dairlib::JointNames());
     AddLoopClosure();
   }
- private:
+
   std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant);
+ private:
   void AddLoopClosure();
 
   dairlib::multibody::DistanceEvaluator<double> l_loop_evaluator_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
index 9d7c510c71..17c9abf1ac 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
@@ -1,4 +1,4 @@
-com_position: [2, 2, 40]
+com_position: [2, 2, 400]
 generalized_positions:
   base_qw: 8.0
   base_qx: 8.0
@@ -27,8 +27,8 @@ generalized_velocities:
   ankle_joint_: 0.002
   toe_: 0.002
 
-contact_pos: [0.1, 0.1, 0.1]
-contact_vel: [0.2, 0.2, 0.2]
+contact_pos: [2, 2, 0.1]
+contact_vel: [0.02, 0.02, 0.02]
 contact_force: [0.0001, 0.0001, 0.0001]
 lin_momentum: [0.0001, 0.0001, 0.0001]
 ang_momentum: [0.0001, 0.0001, 0.0001]
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
index 90c7811f0d..7efb24f59b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
@@ -160,24 +160,23 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eig
 drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedStateTrajectory(const Eigen::VectorXd& nominal_stand,
                                                                                     const Eigen::Vector3d& base_rt_com_ewrt_w,
                                                                                     const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                    const std::vector<double>& time_points,
                                                                                     int base_pos_start,
                                                                                     int base_vel_start){
-  auto n_points = time_points.size();
+  auto n_points = com_traj.get_segment_times().size();
   std::vector<drake::MatrixX<double>> samples(n_points);
   for(int i = 0; i < n_points; i++){
     samples[i] = nominal_stand;
-    samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(time_points[i]);
-    samples[i].block<3,1>(base_vel_start,0, 3, 1) = com_traj.derivative().value(time_points[i]);
+    samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(com_traj.get_segment_times()[i]);
+    samples[i].block<3,1>(base_vel_start,0, 3, 1) = com_traj.derivative().value(com_traj.get_segment_times()[i]);
   }
-  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points, samples);
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(com_traj.get_segment_times(), samples);
 }
 
 int FindCurrentMode(const Gait& active_gait, double time_now){
   double phase_now = fmod(time_now/active_gait.period, 1);
   for(int i = 0; i < active_gait.gait_pattern.size(); i++){
     const auto& mode = active_gait.gait_pattern[i];
-    if(mode.start_phase <= phase_now){
+    if(mode.start_phase <= phase_now && mode.end_phase >= phase_now){
       return i;
     }
   }
@@ -217,6 +216,8 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double curre
     // The new phase is the start phase of the updated mode
     current_phase = gait_pattern[current_mode].start_phase;
   }
+  break_points.push_back(end_time);
+  samples.push_back(samples[samples.size()-1]);
   return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(break_points, samples);
 }
 
@@ -259,15 +260,16 @@ drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(c
                                                                                    double> &state_trajectory){
   auto context =  plant.CreateDefaultContext();
   std::vector<double> break_points = state_trajectory.get_segment_times();
-  break_points.emplace_back(state_trajectory.end_time());
   std::vector<drake::MatrixX<double>> samples;
   int n_contact = contacts.size();
   for(const auto& time : break_points){
     dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, state_trajectory.value(time), context.get());
-    drake::VectorX<double> knot_point_value = samples.emplace_back(Eigen::VectorXd::Zero(6 * n_contact));
+    auto& knot_point_value = samples.emplace_back(Eigen::VectorXd::Zero(6 * n_contact));
     for(int i = 0; i <n_contact; i++){
-      knot_point_value.segment(i * 3, i * 3 + 3) = contacts[i].EvalFull(*context);
-      knot_point_value.segment(3 * n_contact + i * 3, i * 3 + 3) = contacts[i].EvalFullJacobianDotTimesV(*context);
+      knot_point_value.block(i * 3, 0,  3, 1) = contacts[i].EvalFull(*context);
+      knot_point_value.coeffRef(i*3 + 2) = 0;
+      //TODO fix velocity reference
+      knot_point_value.block(3 * n_contact + i * 3, 0, 3, 1) = contacts[i].EvalFullJacobianDotTimesV(*context);
     }
   }
   return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(break_points, samples);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
index dcca852e57..50b2ef313e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
@@ -41,7 +41,6 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eig
  * @param nominal_stand nominal generalized state of the robot
  * @param base_rt_com_ewrt_w offset from base to com
  * @param com_traj trajectory of center of mass
- * @param time_points points to use to create trajectory
  * @param base_pos_start index in generalized state where base position starts
  * @param base_vel_start index in generalized state where base velocity starts
  * @return center of mass trajectory
@@ -49,15 +48,13 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eig
 drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedStateTrajectory(const Eigen::VectorXd& nominal_stand,
                                                                                     const Eigen::Vector3d& base_rt_com_ewrt_w,
                                                                                     const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                    const std::vector<double>& time_points,
                                                                                     int base_pos_start,
                                                                                     int base_vel_start);
 
 int FindCurrentMode(const Gait& active_gait, double time_now);
 
 drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
-                                                                      const std::vector<double>& time_points,
-                                                                      double duration);
+                                                                      const std::vector<double>& time_points);
 
 /*!
  * @brief Calculates the next time a index of the zoh trajectory goes to a specific value
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index baa670991e..1f678b2ea5 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -123,7 +123,11 @@ void KinematicCentroidalMPC::AddContactConstraints() {
         }
       } else {
         // Feet are above the ground
-        prog_->AddBoundingBoxConstraint(0.05, 10, contact_pos_vars(knot_point, contact_index)[2]);
+        double lb = 0;
+        if(knot_point > 0 and knot_point + 1 < n_knot_points_ and !contact_sequence_[knot_point-1][contact_index] and !contact_sequence_[knot_point+1][contact_index]){
+          lb = 0.05;
+        }
+        prog_->AddBoundingBoxConstraint(lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
       }
     }
   }

From a54424066743d4e1cb2a365b567652a0fea71ae5 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Sun, 23 Oct 2022 23:26:15 -0400
Subject: [PATCH 16/76] Walk is looking even better

---
 .../cassie_kcmpc_trajopt.cc                   | 16 +++++++-------
 .../cassie_kinematic_centroidal_mpc.cc        | 11 +++++++++-
 .../cassie_kinematic_centroidal_mpc.h         |  6 +++---
 .../kinematic_centroidal_mpc_gains.yaml       |  8 +++----
 .../reference_generator.cc                    |  2 +-
 .../kinematic_centroidal_mpc.cc               | 21 ++++++++++++-------
 6 files changed, 39 insertions(+), 25 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 5d3c82c727..868ff6fcf1 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -41,10 +41,10 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   plant.Finalize();
   plant_vis.Finalize();
 
-  CassieKinematicCentroidalMPC mpc (plant, n_knot_points, duration/(n_knot_points-1));
+  CassieKinematicCentroidalMPC mpc (plant, n_knot_points, duration/(n_knot_points-1), 0.4);
 
   mpc.SetZeroInitialGuess();
-  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), 1.9, stance_width);
+  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), 1.85, stance_width);
   auto context = plant.CreateDefaultContext();
   dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
   const auto& com = plant.CalcCenterOfMassPositionInWorld(*context);
@@ -61,7 +61,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   stand.gait_pattern = {{0, 1, drake::Vector<bool, 4>(true, true, true, true)}};
 
   Gait walk;
-  walk.period = 0.8;
+  walk.period = 0.6;
   walk.gait_pattern = {{0, 0.4, drake::Vector<bool, 4>(true, true, false, false)},
                         {0.4, 0.5, drake::Vector<bool, 4>(true, true, true, true)},
                         {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
@@ -69,18 +69,18 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   auto com_trajectory = GenerateComTrajectory({0, 0, com_height},
                                               {{0, 0, 0},
-                                               {0.5, 0, 0},
+                                               {1, 0, 0},
                                                {0, 0, 0},
                                                {0, 0, 0}},
-                                              {0, duration/4, 3 * duration/4, duration});
+                                              {0, 0.5, duration - 0.5, duration});
   auto state_trajectory = GenerateGeneralizedStateTrajectory(reference_state,
                                                              reference_state.segment(4, 3) - com,
                                                              com_trajectory,
                                                              4,
                                                              4 + plant.num_positions());
-  auto contact_sequence = GenerateModeSequence({stand, walk, stand, stand}, {0, duration/4, 3 * duration/4, duration});
+  auto contact_sequence = GenerateModeSequence({stand, walk, stand, stand}, {0, 0.5, duration - 0.5, duration});
   auto grf_traj = GenerateGrfReference(contact_sequence, mass);
-  auto contact_traj = GenerateContactPointReference(plant,mpc.CreateContactPoints(plant), state_trajectory);
+  auto contact_traj = GenerateContactPointReference(plant,mpc.CreateContactPoints(plant, 0), state_trajectory);
   mpc.AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(grf_traj));
   mpc.AddStateReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(state_trajectory));
   mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_trajectory));
@@ -157,5 +157,5 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
 int main(int argc, char* argv[]) {
   // Assuming 2 cycles per second
-  DoMain(40, 4, 0.95, 0.2, 0.0, 1e-3);
+  DoMain(40, 3, 0.95, 0.2, 0.0, 1e-3);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index e9c803886f..6054cb05a0 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -4,7 +4,8 @@
 #include "examples/Cassie/cassie_utils.h"
 
 std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalMPC::CreateContactPoints(const drake::multibody::MultibodyPlant<
-    double> &plant) {
+    double> &plant,
+                                                                                                               double mu) {
   auto left_toe_pair = dairlib::LeftToeFront(plant);
   auto left_heel_pair = dairlib::LeftToeRear(plant);
   auto right_toe_pair = dairlib::RightToeFront(plant);
@@ -15,18 +16,26 @@ std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCent
   auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, left_toe_pair.first, left_toe_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  left_toe_eval.set_frictional();
+  left_toe_eval.set_mu(mu);
 
   auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, left_heel_pair.first, left_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  left_heel_eval.set_frictional();
+  left_heel_eval.set_mu(mu);
 
   auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, right_toe_pair.first, right_toe_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  right_toe_eval.set_frictional();
+  right_toe_eval.set_mu(mu);
 
   auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
       plant, right_heel_pair.first, right_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  right_heel_eval.set_frictional();
+  right_heel_eval.set_mu(mu);
 
   return {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval};
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 6a4a019c8e..4459b29ee9 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -9,8 +9,8 @@
  */
 class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
  public:
-  CassieKinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points, double dt) :
-      KinematicCentroidalMPC(plant, n_knot_points, dt, CreateContactPoints(plant)),
+  CassieKinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points, double dt, double mu) :
+      KinematicCentroidalMPC(plant, n_knot_points, dt, CreateContactPoints(plant, mu)),
       l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
       r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
       loop_closure_evaluators(Plant()){
@@ -18,7 +18,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
     AddLoopClosure();
   }
 
-  std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant);
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
  private:
   void AddLoopClosure();
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
index 17c9abf1ac..eceba88323 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
@@ -1,4 +1,4 @@
-com_position: [2, 2, 400]
+com_position: [2, 2, 2]
 generalized_positions:
   base_qw: 8.0
   base_qx: 8.0
@@ -27,8 +27,8 @@ generalized_velocities:
   ankle_joint_: 0.002
   toe_: 0.002
 
-contact_pos: [2, 2, 0.1]
+contact_pos: [0.1, 2, 0.001]
 contact_vel: [0.02, 0.02, 0.02]
 contact_force: [0.0001, 0.0001, 0.0001]
-lin_momentum: [0.0001, 0.0001, 0.0001]
-ang_momentum: [0.0001, 0.0001, 0.0001]
+lin_momentum: [0.00001, 0.00001, 0.00001]
+ang_momentum: [0.00001, 0.00001, 0.00001]
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
index 7efb24f59b..61af66cd3e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
@@ -176,7 +176,7 @@ int FindCurrentMode(const Gait& active_gait, double time_now){
   double phase_now = fmod(time_now/active_gait.period, 1);
   for(int i = 0; i < active_gait.gait_pattern.size(); i++){
     const auto& mode = active_gait.gait_pattern[i];
-    if(mode.start_phase <= phase_now && mode.end_phase >= phase_now){
+    if(mode.start_phase <= phase_now && mode.end_phase > phase_now){
       return i;
     }
   }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 1f678b2ea5..16e67a5d7d 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -124,7 +124,7 @@ void KinematicCentroidalMPC::AddContactConstraints() {
       } else {
         // Feet are above the ground
         double lb = 0;
-        if(knot_point > 0 and knot_point + 1 < n_knot_points_ and !contact_sequence_[knot_point-1][contact_index] and !contact_sequence_[knot_point+1][contact_index]){
+        if(knot_point > 0 and knot_point + 1 < n_knot_points_ and (!contact_sequence_[knot_point-1][contact_index] or !contact_sequence_[knot_point+1][contact_index])){
           lb = 0.05;
         }
         prog_->AddBoundingBoxConstraint(lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
@@ -162,10 +162,14 @@ void KinematicCentroidalMPC::AddCentroidalKinematicConsistency() {
 }
 
 void KinematicCentroidalMPC::AddFrictionConeConstraints() {
-  //{TODO} make this actual friction cone constraint
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
-      prog_->AddBoundingBoxConstraint(0, std::numeric_limits<double>::max(), contact_force_vars(knot_point,contact_index)[2]);
+      if(contact_sequence_[knot_point][contact_index]){
+        auto force_constraints_vec = contact_points_[contact_index].CreateLinearFrictionConstraints();
+        for(const auto& constraint : force_constraints_vec){
+          prog_->AddConstraint(constraint, contact_force_vars(knot_point, contact_index));
+        }
+      }
     }
   }
 }
@@ -222,21 +226,22 @@ void KinematicCentroidalMPC::AddConstantMomentumReference(const drake::VectorX<d
 
 void KinematicCentroidalMPC::AddCosts() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    double terminal_gain = knot_point == n_knot_points_-1 ? 10 : 1;
     double t = dt_ * knot_point;
     if(ref_traj_){
-      prog_->AddQuadraticErrorCost(Q_, ref_traj_->value(t), state_vars(knot_point));
+      prog_->AddQuadraticErrorCost(terminal_gain * Q_, ref_traj_->value(t), state_vars(knot_point));
     }
     if(com_ref_traj_){
-      prog_->AddQuadraticErrorCost(Q_com_,  com_ref_traj_->value(t) , com_pos_vars(knot_point));
+      prog_->AddQuadraticErrorCost(terminal_gain * Q_com_,  com_ref_traj_->value(t) , com_pos_vars(knot_point));
     }
     if(mom_ref_traj_){
-      prog_->AddQuadraticErrorCost(Q_mom_, mom_ref_traj_->value(t), momentum_vars(knot_point));
+      prog_->AddQuadraticErrorCost(terminal_gain * Q_mom_, mom_ref_traj_->value(t), momentum_vars(knot_point));
     }
     if(contact_ref_traj_){
-      prog_->AddQuadraticErrorCost(Q_contact_,  contact_ref_traj_->value(t) , {contact_pos_[knot_point],contact_vel_[knot_point]});
+      prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_,  contact_ref_traj_->value(t) , {contact_pos_[knot_point],contact_vel_[knot_point]});
     }
     if(force_ref_traj_){
-      prog_->AddQuadraticErrorCost(Q_force_, force_ref_traj_->value(t), contact_force_[knot_point]);
+      prog_->AddQuadraticErrorCost(terminal_gain * Q_force_, force_ref_traj_->value(t), contact_force_[knot_point]);
     }
   }
 }

From fdc7f96f5515326b489db319d0ebeaa388815363 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 24 Oct 2022 11:10:55 -0400
Subject: [PATCH 17/76] Still working well

---
 .../kinematic_centroidal_constraints.cc           | 15 +++++----------
 .../kinematic_centroidal_constraints.h            |  6 +-----
 .../kinematic_centroidal_mpc.cc                   |  9 +++------
 3 files changed, 9 insertions(+), 21 deletions(-)

diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
index 97e73e1777..aae59f37d5 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
@@ -127,10 +127,7 @@ CenterofMassPositionConstraint<T>::CenterofMassPositionConstraint(const drake::m
         std::to_string(knot_index) + "]"),
                                                                                    plant_(plant),
                                                                                    context_(context),
-                                                                                   n_x_(plant.num_positions()
-                                                                                            + plant.num_velocities()),
-                                                                                   n_u_(plant.num_actuators()),
-                                                                                   zero_control_(Eigen::VectorXd::Zero(n_u_)) {}
+                                                                                   n_q_(plant.num_positions()){}
 
 /// The format of the input to the eval() function is in the order
 ///   - rCOM, location of the center of mass
@@ -139,8 +136,8 @@ template<typename T>
 void CenterofMassPositionConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
                                                            drake::VectorX<T> *y) const {
   const auto& rCom = x.segment(0, 3);
-  const auto& x0 = x.segment(3, n_x_);
-  dairlib::multibody::SetContext<T>(plant_, x0, zero_control_, context_);
+  const auto& x0 = x.segment(3, n_q_);
+  dairlib::multibody::SetPositionsIfNew<T>(plant_, x0,  context_);
   *y = rCom - plant_.CalcCenterOfMassPositionInWorld(*context_);
 }
 
@@ -156,9 +153,7 @@ CentroidalMomentumConstraint<T>::CentroidalMomentumConstraint(const drake::multi
                                                                                    plant_(plant),
                                                                                    context_(context),
                                                                                    n_x_(plant.num_positions()
-                                                                                            + plant.num_velocities()),
-                                                                                   n_u_(plant.num_actuators()),
-                                                                                   zero_control_(Eigen::VectorXd::Zero(n_u_)) {}
+                                                                                            + plant.num_velocities()){}
 
 /// The format of the input to the eval() function is in the order
 ///   - q, generalized positions
@@ -171,7 +166,7 @@ void CentroidalMomentumConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
   const auto& x0 = x.head(n_x_);
   const auto& r = x.segment(n_x_, 3);
   const auto& h_WC = x.segment(n_x_ + 3, 6);
-  dairlib::multibody::SetContext<T>(plant_, x0, zero_control_, context_);
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, x0,  context_);
   const auto& spatial_momentum = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, r);
   *y = spatial_momentum.get_coeffs() - h_WC;
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
index 8b2270f21d..942d94afa1 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
@@ -94,9 +94,7 @@ class CenterofMassPositionConstraint : public dairlib::solvers::NonlinearConstra
 
   const drake::multibody::MultibodyPlant<T>& plant_;
   drake::systems::Context<T>* context_;
-  int n_x_;
-  int n_u_;
-  const drake::VectorX<T> zero_control_;
+  int n_q_;
 };
 
 /*!
@@ -117,8 +115,6 @@ class CentroidalMomentumConstraint : public dairlib::solvers::NonlinearConstrain
   const drake::multibody::MultibodyPlant<T>& plant_;
   drake::systems::Context<T>* context_;
   int n_x_;
-  int n_u_;
-  const drake::VectorX<T> zero_control_;
 };
 
 /*!
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 16e67a5d7d..b57cc3d618 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -164,11 +164,9 @@ void KinematicCentroidalMPC::AddCentroidalKinematicConsistency() {
 void KinematicCentroidalMPC::AddFrictionConeConstraints() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
-      if(contact_sequence_[knot_point][contact_index]){
-        auto force_constraints_vec = contact_points_[contact_index].CreateLinearFrictionConstraints();
-        for(const auto& constraint : force_constraints_vec){
-          prog_->AddConstraint(constraint, contact_force_vars(knot_point, contact_index));
-        }
+      auto force_constraints_vec = contact_points_[contact_index].CreateLinearFrictionConstraints();
+      for(const auto& constraint : force_constraints_vec){
+        prog_->AddConstraint(constraint, contact_force_vars(knot_point, contact_index));
       }
     }
   }
@@ -183,7 +181,6 @@ void KinematicCentroidalMPC::AddFlightContactForceConstraints() {
       }
     }
   }
-
 }
 
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::state_vars(int knotpoint_index) const {

From 2f747717845fcd6be3cccb22d97d32ab31247d62 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 24 Oct 2022 18:17:20 -0400
Subject: [PATCH 18/76] Working alright. Added guess from reference

---
 .../cassie_kcmpc_trajopt.cc                   | 48 +++++++------------
 .../kinematic_centroidal_constraints.cc       |  7 +--
 .../kinematic_centroidal_mpc.cc               | 40 ++++++++++++++++
 .../kinematic_centroidal_mpc.h                | 10 ++++
 4 files changed, 70 insertions(+), 35 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 868ff6fcf1..bd72eaef6d 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -21,7 +21,7 @@ using drake::multibody::MultibodyPlant;
 using drake::multibody::Parser;
 using drake::geometry::DrakeVisualizer;
 
-void DoMain(int n_knot_points, double duration, double com_height, double stance_width, double squat_distance, double tol){
+void DoMain(int n_knot_points, double duration, double base_height, double stance_width, double vel, double tol){
   auto gains = drake::yaml::LoadYamlFile<KinematicCentroidalGains>("examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml");
   // Create fix-spring Cassie MBP
   drake::systems::DiagramBuilder<double> builder;
@@ -42,43 +42,36 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   plant_vis.Finalize();
 
   CassieKinematicCentroidalMPC mpc (plant, n_knot_points, duration/(n_knot_points-1), 0.4);
+  mpc.SetGains(gains);
 
-  mpc.SetZeroInitialGuess();
-  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), 1.85, stance_width);
+  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), base_height, stance_width);
   auto context = plant.CreateDefaultContext();
   dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
   const auto& com = plant.CalcCenterOfMassPositionInWorld(*context);
   const auto& mass = plant.CalcTotalMass(*context);
 
-  mpc.SetRobotStateGuess(reference_state);
-  mpc.AddInitialStateConstraint(reference_state);
-  mpc.AddComHeightBoundingConstraint(0.1,2);
-  mpc.SetComPositionGuess({0, 0, com_height});
-  mpc.SetGains(gains);
-
   Gait stand;
   stand.period = 1;
   stand.gait_pattern = {{0, 1, drake::Vector<bool, 4>(true, true, true, true)}};
 
   Gait walk;
-  walk.period = 0.6;
+  walk.period = 1;
   walk.gait_pattern = {{0, 0.4, drake::Vector<bool, 4>(true, true, false, false)},
                         {0.4, 0.5, drake::Vector<bool, 4>(true, true, true, true)},
                         {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
                         {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}};
 
-  auto com_trajectory = GenerateComTrajectory({0, 0, com_height},
+  auto com_trajectory = GenerateComTrajectory(com,
                                               {{0, 0, 0},
-                                               {1, 0, 0},
-                                               {0, 0, 0},
-                                               {0, 0, 0}},
-                                              {0, 0.5, duration - 0.5, duration});
+                                               {vel, 0, 0},
+                                               {vel, 0, 0}},
+                                              {0, 0.5, duration});
   auto state_trajectory = GenerateGeneralizedStateTrajectory(reference_state,
                                                              reference_state.segment(4, 3) - com,
                                                              com_trajectory,
                                                              4,
                                                              4 + plant.num_positions());
-  auto contact_sequence = GenerateModeSequence({stand, walk, stand, stand}, {0, 0.5, duration - 0.5, duration});
+  auto contact_sequence = GenerateModeSequence({stand, walk, walk}, {0, 0.5, duration});
   auto grf_traj = GenerateGrfReference(contact_sequence, mass);
   auto contact_traj = GenerateContactPointReference(plant,mpc.CreateContactPoints(plant, 0), state_trajectory);
   mpc.AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(grf_traj));
@@ -86,18 +79,14 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_trajectory));
   mpc.AddContactTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(contact_traj));
   mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
+  mpc.SetModeSequence(contact_sequence);
 
-  std::vector<std::vector<bool>> mode_sequence(n_knot_points);
-  const double dt = duration/(n_knot_points-1);
-
-  for(int knot_point = 0; knot_point < n_knot_points; knot_point ++){
-    for(int contact_index = 0; contact_index < 4; contact_index ++){
-      mode_sequence[knot_point].emplace_back(contact_sequence.value(dt * knot_point).coeff(contact_index));
-    }
-  }
+  mpc.AddInitialStateConstraint(reference_state);
+  mpc.SetRobotStateGuess(state_trajectory);
+  mpc.SetComPositionGuess(com_trajectory);
+  mpc.SetContactGuess(contact_traj);
+  mpc.SetForceGuess(grf_traj);
 
-  mpc.SetModeSequence(mode_sequence);
-  std::cout<<"Adding solver options"<<std::endl;
   {
     drake::solvers::SolverOptions options;
     auto id = drake::solvers::IpoptSolver::id();
@@ -105,7 +94,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
     options.SetOption(id, "dual_inf_tol", tol);
     options.SetOption(id, "constr_viol_tol", tol);
     options.SetOption(id, "compl_inf_tol", tol);
-    options.SetOption(id, "max_iter", 2000);
+    options.SetOption(id, "max_iter", 500);
     options.SetOption(id, "nlp_lower_bound_inf", -1e6);
     options.SetOption(id, "nlp_upper_bound_inf", 1e6);
     options.SetOption(id, "print_timing_statistics", "yes");
@@ -149,13 +138,12 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(.5);
+    simulator.set_target_realtime_rate(1.0);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
   }
 }
 
 int main(int argc, char* argv[]) {
-  // Assuming 2 cycles per second
-  DoMain(40, 3, 0.95, 0.2, 0.0, 1e-3);
+  DoMain(40, 3, 1.1, 0.2, 0.0, 1e-3);
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
index aae59f37d5..303f647327 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
@@ -66,12 +66,9 @@ drake::VectorX<T> CentroidalDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
     sum_forces = sum_forces + force;
   }
 
-  // Working in body frame angular velocity
-  const auto d_ang_mom =  sum_moments;
-  const auto d_lin_mom = sum_forces;
   drake::Vector<T, 6> rv;
-  rv.head(3) = d_ang_mom;
-  rv.tail(3) = d_lin_mom;
+  rv.head(3) = sum_moments;
+  rv.tail(3) = sum_forces;
   return rv;
 }
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index b57cc3d618..11c06bcbf6 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -411,6 +411,14 @@ void KinematicCentroidalMPC::SetRobotStateGuess(const drake::VectorX<double> &st
    prog_->SetInitialGuess(state_vars, state);
   }
 }
+
+void KinematicCentroidalMPC::SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double>& state_trajectory) {
+  DRAKE_DEMAND(state_trajectory.rows() == n_q_ + n_v_);
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    prog_->SetInitialGuess(state_vars(knot_point), state_trajectory.value(dt_ * knot_point));
+  }
+}
+
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::momentum_vars(int knotpoint_index) const {
   return mom_vars_[knotpoint_index];
 }
@@ -424,9 +432,41 @@ void KinematicCentroidalMPC::SetComPositionGuess(const drake::Vector3<double> &s
     prog_->SetInitialGuess(com_pos, state);
   }
 }
+
+void KinematicCentroidalMPC::SetComPositionGuess(const drake::trajectories::PiecewisePolynomial<double>& com_trajectory) {
+  DRAKE_DEMAND(com_trajectory.rows() == 3);
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    prog_->SetInitialGuess(com_pos_vars(knot_point), com_trajectory.value(dt_ * knot_point));
+  }
+}
+
+void KinematicCentroidalMPC::SetContactGuess(const drake::trajectories::PiecewisePolynomial<double>& contact_trajectory){
+  DRAKE_DEMAND(contact_trajectory.rows() == 6 * n_contact_points_);
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    prog_->SetInitialGuess(contact_pos_[knot_point], drake::VectorX<double>(contact_trajectory.value(dt_ * knot_point)).head(3 * n_contact_points_));
+    prog_->SetInitialGuess(contact_vel_[knot_point], drake::VectorX<double>(contact_trajectory.value(dt_ * knot_point)).tail(3 * n_contact_points_));
+  }
+}
+void KinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double>& force_trajectory){
+  DRAKE_DEMAND(force_trajectory.rows() == 3 * n_contact_points_);
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    prog_->SetInitialGuess(contact_force_[knot_point], force_trajectory.value(dt_ * knot_point));
+  }
+
+}
+
+
 void KinematicCentroidalMPC::SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence) {
   contact_sequence_ = contact_sequence;
 }
+void KinematicCentroidalMPC::SetModeSequence(const drake::trajectories::PiecewisePolynomial<double>& contact_sequence){
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    for(int contact_index = 0; contact_index < 4; contact_index ++){
+      contact_sequence_[knot_point][contact_index] = contact_sequence.value(dt_ * knot_point).coeff(contact_index);
+    }
+  }
+}
+
 void KinematicCentroidalMPC::AddInitialStateConstraint(const Eigen::VectorXd state) {
   DRAKE_DEMAND(state.size() == state_vars(0).size());
   prog_->AddBoundingBoxConstraint(state, state, state_vars((0)));
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 26c940714b..af16103ce0 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -187,8 +187,16 @@ class KinematicCentroidalMPC {
 
   void SetRobotStateGuess(const drake::VectorX<double>& state);
 
+  void SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double>& state_trajectory);
+
   void SetComPositionGuess(const drake::Vector3<double>& state);
 
+  void SetComPositionGuess(const drake::trajectories::PiecewisePolynomial<double>& com_trajectory);
+
+  void SetContactGuess(const drake::trajectories::PiecewisePolynomial<double>& contact_trajectory);
+
+  void SetForceGuess(const drake::trajectories::PiecewisePolynomial<double>& force_trajectory);
+
   void CreateVisualizationCallback(std::string model_file,
                                    double alpha,
                                    std::string weld_frame_to_world = "");
@@ -217,6 +225,8 @@ class KinematicCentroidalMPC {
   }
   void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
 
+  void SetModeSequence(const drake::trajectories::PiecewisePolynomial<double>& contact_sequence);
+
   void AddInitialStateConstraint(const Eigen::VectorXd state);
 
   const drake::multibody::MultibodyPlant<double>& Plant(){return plant_;};

From b9e2cc4e3e0d9cf75a0374fcc52aa01910ad2163 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 24 Oct 2022 21:00:17 -0400
Subject: [PATCH 19/76] Cleaned up gains

---
 .../cassie_kcmpc_trajopt.cc                   | 21 ++++---
 .../kinematic_centroidal_mpc_gains.yaml       | 10 ++--
 .../reference_generator.cc                    | 31 +++++++---
 .../reference_generator.h                     | 20 ++++---
 .../kinematic_centroidal_mpc.cc               | 57 +++++++++++--------
 .../kinematic_centroidal_mpc.h                | 26 +++++----
 6 files changed, 98 insertions(+), 67 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index bd72eaef6d..8d23466a34 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -66,23 +66,26 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
                                                {vel, 0, 0},
                                                {vel, 0, 0}},
                                               {0, 0.5, duration});
-  auto state_trajectory = GenerateGeneralizedStateTrajectory(reference_state,
-                                                             reference_state.segment(4, 3) - com,
-                                                             com_trajectory,
-                                                             4,
-                                                             4 + plant.num_positions());
+  auto q_trajectory = GenerateGeneralizedPosTrajectory(reference_state.head(plant.num_positions()),
+                                                           reference_state.segment(4, 3) - com,
+                                                           com_trajectory,
+                                                           4);
+  auto v_trajectory = GenerateGeneralizedVelTrajectory(com_trajectory,
+                                                       plant.num_velocities(),
+                                                       3);
   auto contact_sequence = GenerateModeSequence({stand, walk, walk}, {0, 0.5, duration});
   auto grf_traj = GenerateGrfReference(contact_sequence, mass);
-  auto contact_traj = GenerateContactPointReference(plant,mpc.CreateContactPoints(plant, 0), state_trajectory);
+  auto contact_traj = GenerateContactPointReference(plant,mpc.CreateContactPoints(plant, 0), q_trajectory, v_trajectory);
   mpc.AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(grf_traj));
-  mpc.AddStateReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(state_trajectory));
+  mpc.AddGenPosReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(q_trajectory));
+  mpc.AddGenVelReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(v_trajectory));
   mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_trajectory));
   mpc.AddContactTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(contact_traj));
   mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
   mpc.SetModeSequence(contact_sequence);
 
   mpc.AddInitialStateConstraint(reference_state);
-  mpc.SetRobotStateGuess(state_trajectory);
+  mpc.SetRobotStateGuess(q_trajectory, v_trajectory);
   mpc.SetComPositionGuess(com_trajectory);
   mpc.SetContactGuess(contact_traj);
   mpc.SetForceGuess(grf_traj);
@@ -145,5 +148,5 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
 }
 
 int main(int argc, char* argv[]) {
-  DoMain(40, 3, 1.1, 0.2, 0.0, 1e-3);
+  DoMain(40, 3, 1.1, 0.2, 0.5, 1e-3);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
index eceba88323..0487437c36 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
@@ -1,9 +1,9 @@
-com_position: [2, 2, 2]
+com_position: [1, 1, 1]
 generalized_positions:
-  base_qw: 8.0
-  base_qx: 8.0
-  base_qy: 8.0
-  base_qz: 8.0
+  base_qw: 1.0
+  base_qx: 1.0
+  base_qy: 1.0
+  base_qz: 1.0
   base_x: 0
   base_y: 0
   base_z: 0
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
index 61af66cd3e..4020118222 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
@@ -157,21 +157,31 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eig
   return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points, samples);
 }
 
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedStateTrajectory(const Eigen::VectorXd& nominal_stand,
-                                                                                    const Eigen::Vector3d& base_rt_com_ewrt_w,
-                                                                                    const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                    int base_pos_start,
-                                                                                    int base_vel_start){
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
+                                                                                  const Eigen::Vector3d& base_rt_com_ewrt_w,
+                                                                                  const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int base_pos_start){
   auto n_points = com_traj.get_segment_times().size();
   std::vector<drake::MatrixX<double>> samples(n_points);
   for(int i = 0; i < n_points; i++){
     samples[i] = nominal_stand;
     samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(com_traj.get_segment_times()[i]);
-    samples[i].block<3,1>(base_vel_start,0, 3, 1) = com_traj.derivative().value(com_traj.get_segment_times()[i]);
   }
   return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(com_traj.get_segment_times(), samples);
 }
 
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int n_v,
+                                                                                  int base_vel_start){
+  auto n_points = com_traj.get_segment_times().size();
+  std::vector<drake::MatrixX<double>> samples(n_points);
+  for(int i = 0; i < n_points; i++){
+    samples[i] = drake::VectorX<double>::Zero(n_v);
+    samples[i].block<3,1>(base_vel_start,0, 3, 1) = com_traj.derivative().value(com_traj.get_segment_times()[i]);
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(com_traj.get_segment_times(), samples);
+}
+
 int FindCurrentMode(const Gait& active_gait, double time_now){
   double phase_now = fmod(time_now/active_gait.period, 1);
   for(int i = 0; i < active_gait.gait_pattern.size(); i++){
@@ -257,13 +267,16 @@ drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(c
                                                                                const std::vector<dairlib::multibody::WorldPointEvaluator<
                                                                                    double>> &contacts,
                                                                                const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &state_trajectory){
+                                                                                   double> &q_traj,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &v_traj){
   auto context =  plant.CreateDefaultContext();
-  std::vector<double> break_points = state_trajectory.get_segment_times();
+  std::vector<double> break_points = q_traj.get_segment_times();
   std::vector<drake::MatrixX<double>> samples;
   int n_contact = contacts.size();
   for(const auto& time : break_points){
-    dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, state_trajectory.value(time), context.get());
+    dairlib::multibody::SetPositionsIfNew<double>(plant, q_traj.value(time), context.get());
+    dairlib::multibody::SetVelocitiesIfNew<double>(plant, v_traj.value(time), context.get());
     auto& knot_point_value = samples.emplace_back(Eigen::VectorXd::Zero(6 * n_contact));
     for(int i = 0; i <n_contact; i++){
       knot_point_value.block(i * 3, 0,  3, 1) = contacts[i].EvalFull(*context);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
index 50b2ef313e..5b99f9b63d 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
@@ -37,19 +37,21 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eig
 
 
 /*!
- * @brief Creates a trajectory in generalized state given a nominal state and com trajectory
+ * @brief Creates a trajectory in generalized position given a nominal state and com trajectory
  * @param nominal_stand nominal generalized state of the robot
  * @param base_rt_com_ewrt_w offset from base to com
  * @param com_traj trajectory of center of mass
  * @param base_pos_start index in generalized state where base position starts
- * @param base_vel_start index in generalized state where base velocity starts
  * @return center of mass trajectory
  */
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedStateTrajectory(const Eigen::VectorXd& nominal_stand,
-                                                                                    const Eigen::Vector3d& base_rt_com_ewrt_w,
-                                                                                    const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                    int base_pos_start,
-                                                                                    int base_vel_start);
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
+                                                                                  const Eigen::Vector3d& base_rt_com_ewrt_w,
+                                                                                  const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int base_pos_start);
+
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int n_v,
+                                                                                  int base_vel_start);
 
 int FindCurrentMode(const Gait& active_gait, double time_now);
 
@@ -79,4 +81,6 @@ drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(c
                                                                                const std::vector<dairlib::multibody::WorldPointEvaluator<
                                                                                    double>> &contacts,
                                                                                const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &state_trajectory);
\ No newline at end of file
+                                                                                   double> &q_traj,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &v_traj);
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 11c06bcbf6..dd776488b8 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -22,7 +22,8 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
                                                n_q_(plant.num_positions()),
                                                n_v_(plant.num_velocities()),
                                                n_contact_points_(contact_points.size()),
-                                               Q_(Eigen::MatrixXd::Zero(n_q_ + n_v_, n_q_ + n_v_)),
+                                               Q_q_(Eigen::MatrixXd::Zero(n_q_, n_q_)),
+                                               Q_v_(Eigen::MatrixXd::Zero(n_v_, n_v_)),
                                                Q_com_(Eigen::MatrixXd::Zero(3, 3)),
                                                Q_mom_(Eigen::MatrixXd::Zero(6, 6)),
                                                Q_contact_(Eigen::MatrixXd::Zero(6 * n_contact_points_, 6 * n_contact_points_)),
@@ -53,8 +54,12 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(const drake::multibody::Multibody
   std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
 }
 
-void KinematicCentroidalMPC::AddStateReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
-  ref_traj_ = std::move(ref_traj);
+void KinematicCentroidalMPC::AddGenPosReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
+  q_ref_traj_ = std::move(ref_traj);
+}
+
+void KinematicCentroidalMPC::AddGenVelReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj){
+  v_ref_traj_ = std::move(ref_traj);
 }
 
 void KinematicCentroidalMPC::AddContactTrackingReference(std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj) {
@@ -205,28 +210,19 @@ void KinematicCentroidalMPC::Build(const drake::solvers::SolverOptions &solver_o
   prog_->SetSolverOptions(solver_options);
 }
 
-void KinematicCentroidalMPC::AddConstantStateReference(const drake::VectorX<double>& value) {
-  AddStateReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
-}
-
-void KinematicCentroidalMPC::AddConstantForceTrackingReference(const drake::VectorX<double> &value) {
-  AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
-}
-
-void KinematicCentroidalMPC::AddConstantComReference(const drake::VectorX<double> &value) {
-  AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
-}
-
 void KinematicCentroidalMPC::AddConstantMomentumReference(const drake::VectorX<double> &value) {
   AddMomentumReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(value));
 }
 
 void KinematicCentroidalMPC::AddCosts() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    double terminal_gain = knot_point == n_knot_points_-1 ? 10 : 1;
+    double terminal_gain = knot_point == n_knot_points_-1 ? 100 : 1;
     double t = dt_ * knot_point;
-    if(ref_traj_){
-      prog_->AddQuadraticErrorCost(terminal_gain * Q_, ref_traj_->value(t), state_vars(knot_point));
+    if(q_ref_traj_){
+      prog_->AddQuadraticErrorCost(terminal_gain * Q_q_, q_ref_traj_->value(t), state_vars(knot_point).head(n_q_));
+    }
+    if(v_ref_traj_){
+      prog_->AddQuadraticErrorCost(terminal_gain * Q_v_, v_ref_traj_->value(t), state_vars(knot_point).tail(n_v_));
     }
     if(com_ref_traj_){
       prog_->AddQuadraticErrorCost(terminal_gain * Q_com_,  com_ref_traj_->value(t) , com_pos_vars(knot_point));
@@ -419,6 +415,17 @@ void KinematicCentroidalMPC::SetRobotStateGuess(const drake::trajectories::Piece
   }
 }
 
+void KinematicCentroidalMPC::SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double>& q_traj,
+                                                const drake::trajectories::PiecewisePolynomial<double>& v_traj){
+  DRAKE_DEMAND(q_traj.rows() == n_q_);
+  DRAKE_DEMAND(v_traj.rows() == n_v_);
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    prog_->SetInitialGuess(state_vars(knot_point).head(n_q_), q_traj.value(dt_ * knot_point));
+    prog_->SetInitialGuess(state_vars(knot_point).tail(n_v_), v_traj.value(dt_ * knot_point));
+  }
+}
+
+
 drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::momentum_vars(int knotpoint_index) const {
   return mom_vars_[knotpoint_index];
 }
@@ -467,7 +474,7 @@ void KinematicCentroidalMPC::SetModeSequence(const drake::trajectories::Piecewis
   }
 }
 
-void KinematicCentroidalMPC::AddInitialStateConstraint(const Eigen::VectorXd state) {
+void KinematicCentroidalMPC::AddInitialStateConstraint(const Eigen::VectorXd& state) {
   DRAKE_DEMAND(state.size() == state_vars(0).size());
   prog_->AddBoundingBoxConstraint(state, state, state_vars((0)));
 
@@ -480,16 +487,16 @@ void KinematicCentroidalMPC::SetGains(const KinematicCentroidalGains &gains) {
   for(const auto&[name, index] : positions_map){
     const auto it = gains.generalized_positions.find(name);
     if(it != gains.generalized_positions.end()){
-      Q_(index, index) = it->second;
+      Q_q_(index, index) = it->second;
     } else if(name.find("left") != std::string::npos) {
       const auto it_left = gains.generalized_positions.find(name.substr(0, name.size()-4));
       if(it_left != gains.generalized_positions.end()) {
-        Q_(index, index) = it_left->second;
+        Q_q_(index, index) = it_left->second;
       }
     } else if(name.find("right") != std::string::npos) {
       const auto it_right = gains.generalized_positions.find(name.substr(0, name.size() - 5));
       if (it_right != gains.generalized_positions.end()) {
-        Q_(index, index) = it_right->second;
+        Q_q_(index, index) = it_right->second;
       }
     }
   }
@@ -497,16 +504,16 @@ void KinematicCentroidalMPC::SetGains(const KinematicCentroidalGains &gains) {
   for(const auto&[name, index] : velocities_map){
     const auto it = gains.generalized_velocities.find(name);
     if(it != gains.generalized_velocities.end()){
-      Q_(index+n_q_, index+n_q_) = it->second;
+      Q_v_(index, index) = it->second;
     } else if(name.find("left") != std::string::npos) {
       const auto it_left = gains.generalized_velocities.find(name.substr(0, name.size()-7));
       if(it_left != gains.generalized_velocities.end()) {
-        Q_(index+n_q_, index+n_q_) = it_left->second;
+        Q_v_(index, index) = it_left->second;
       }
     }else if(name.find("right") != std::string::npos){
       const auto it_right = gains.generalized_velocities.find(name.substr(0, name.size()-8));
       if(it_right != gains.generalized_velocities.end()) {
-        Q_(index+n_q_, index+n_q_) = it_right->second;
+        Q_v_(index, index) = it_right->second;
       }
     }
   }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index af16103ce0..bb1e358094 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -50,10 +50,16 @@ class KinematicCentroidalMPC {
   void SetGains(const KinematicCentroidalGains& gains);
 
   /*!
-   * @brief Adds a cost reference for the state of the robot of the form (x - x_ref)^T Q (x - x_ref)
+   * @brief Adds a cost reference for the generalized position of the robot of the form (x - x_ref)^T Q (x - x_ref)
    * @param ref_traj trajectory in time
    */
-  void AddStateReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
+  void AddGenPosReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
+
+  /*!
+   * @brief Adds a cost reference for the generalized velocity of the robot of the form (x - x_ref)^T Q (x - x_ref)
+   * @param ref_traj trajectory in time
+   */
+  void AddGenVelReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
 
   /*!
    * @brief Adds a cost and reference for the centroidal state of the robot (x - x_ref)^T Q (x - x_ref)
@@ -75,12 +81,6 @@ class KinematicCentroidalMPC {
    */
   void AddForceTrackingReference(std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj);
 
-  void AddConstantStateReference(const drake::VectorX<double>& value);
-
-  void AddConstantForceTrackingReference(const drake::VectorX<double>& value);
-
-  void AddConstantComReference(const drake::VectorX<double>& value);
-
   void AddConstantMomentumReference(const drake::VectorX<double>& value);
 
   /*!
@@ -189,6 +189,8 @@ class KinematicCentroidalMPC {
 
   void SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double>& state_trajectory);
 
+  void SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double>& q_traj, const drake::trajectories::PiecewisePolynomial<double>& v_traj);
+
   void SetComPositionGuess(const drake::Vector3<double>& state);
 
   void SetComPositionGuess(const drake::trajectories::PiecewisePolynomial<double>& com_trajectory);
@@ -227,7 +229,7 @@ class KinematicCentroidalMPC {
 
   void SetModeSequence(const drake::trajectories::PiecewisePolynomial<double>& contact_sequence);
 
-  void AddInitialStateConstraint(const Eigen::VectorXd state);
+  void AddInitialStateConstraint(const Eigen::VectorXd& state);
 
   const drake::multibody::MultibodyPlant<double>& Plant(){return plant_;};
  private:
@@ -287,8 +289,10 @@ class KinematicCentroidalMPC {
   int n_contact_points_;
 
   /// References and cost matrixes
-  std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj_;
-  Eigen::MatrixXd Q_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> q_ref_traj_;
+  Eigen::MatrixXd Q_q_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> v_ref_traj_;
+  Eigen::MatrixXd Q_v_;
   std::unique_ptr<drake::trajectories::Trajectory<double>> com_ref_traj_;
   Eigen::MatrixXd Q_com_;
   std::unique_ptr<drake::trajectories::Trajectory<double>> mom_ref_traj_;

From c2f5823b0825635fe142574b3a713d874667858b Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 24 Oct 2022 23:05:51 -0400
Subject: [PATCH 20/76] Added trapazoidal integration to cost

---
 .../cassie_kcmpc_trajopt.cc                       |  2 +-
 .../kinematic_centroidal_mpc.cc                   | 15 ++++++++-------
 2 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 8d23466a34..4b2a1ef8dc 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -148,5 +148,5 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
 }
 
 int main(int argc, char* argv[]) {
-  DoMain(40, 3, 1.1, 0.2, 0.5, 1e-3);
+  DoMain(36, 3, 1.2, 0.2, 0.5, 1e-2);
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index dd776488b8..edea319969 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -216,25 +216,26 @@ void KinematicCentroidalMPC::AddConstantMomentumReference(const drake::VectorX<d
 
 void KinematicCentroidalMPC::AddCosts() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    double terminal_gain = knot_point == n_knot_points_-1 ? 100 : 1;
+    const double terminal_gain = knot_point == n_knot_points_-1 ? 100 : 1;
+    const double collocation_gain = (knot_point == 0 or knot_point == n_knot_points_ -1) ? 0.5 : 1;
     double t = dt_ * knot_point;
     if(q_ref_traj_){
-      prog_->AddQuadraticErrorCost(terminal_gain * Q_q_, q_ref_traj_->value(t), state_vars(knot_point).head(n_q_));
+      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_q_, q_ref_traj_->value(t), state_vars(knot_point).head(n_q_));
     }
     if(v_ref_traj_){
-      prog_->AddQuadraticErrorCost(terminal_gain * Q_v_, v_ref_traj_->value(t), state_vars(knot_point).tail(n_v_));
+      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_v_, v_ref_traj_->value(t), state_vars(knot_point).tail(n_v_));
     }
     if(com_ref_traj_){
-      prog_->AddQuadraticErrorCost(terminal_gain * Q_com_,  com_ref_traj_->value(t) , com_pos_vars(knot_point));
+      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_com_,  com_ref_traj_->value(t) , com_pos_vars(knot_point));
     }
     if(mom_ref_traj_){
-      prog_->AddQuadraticErrorCost(terminal_gain * Q_mom_, mom_ref_traj_->value(t), momentum_vars(knot_point));
+      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_mom_, mom_ref_traj_->value(t), momentum_vars(knot_point));
     }
     if(contact_ref_traj_){
-      prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_,  contact_ref_traj_->value(t) , {contact_pos_[knot_point],contact_vel_[knot_point]});
+      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_contact_,  contact_ref_traj_->value(t) , {contact_pos_[knot_point],contact_vel_[knot_point]});
     }
     if(force_ref_traj_){
-      prog_->AddQuadraticErrorCost(terminal_gain * Q_force_, force_ref_traj_->value(t), contact_force_[knot_point]);
+      prog_->AddQuadraticErrorCost(collocation_gain *terminal_gain * Q_force_, force_ref_traj_->value(t), contact_force_[knot_point]);
     }
   }
 }

From 541995b6c987b9bfe9ebf0db1d0c6b6c5a0ccdb0 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 25 Oct 2022 12:39:30 -0400
Subject: [PATCH 21/76] Removed over constrained system

---
 .../kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index edea319969..d3ecbbd9ef 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -116,7 +116,7 @@ void KinematicCentroidalMPC::AddKinematicsIntegrator() {
 }
 
 void KinematicCentroidalMPC::AddContactConstraints() {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+  for (int knot_point = 1; knot_point < n_knot_points_; knot_point++) {
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
       //Make sure feet in stance are not moving and on the ground
       if (contact_sequence_[knot_point][contact_index]) {

From ddcade045ae7ebc39cd3a7b6bd27bf4e6f14327c Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 26 Oct 2022 14:00:16 -0400
Subject: [PATCH 22/76] Fixed reference bug

---
 examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
index 4020118222..3d491eeff6 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
@@ -165,7 +165,7 @@ drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajector
   std::vector<drake::MatrixX<double>> samples(n_points);
   for(int i = 0; i < n_points; i++){
     samples[i] = nominal_stand;
-    samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(com_traj.get_segment_times()[i]);
+    samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(com_traj.get_segment_times()[i]) + base_rt_com_ewrt_w;
   }
   return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(com_traj.get_segment_times(), samples);
 }

From 2cb002a86180ebec2d756feb78c87d875853cadc Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 26 Oct 2022 16:44:44 -0400
Subject: [PATCH 23/76] Fixed reference bug

---
 .../Cassie/kinematic_centroidal_mpc/reference_generator.cc    | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
index 3d491eeff6..f7cc10e8b8 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
@@ -257,7 +257,9 @@ drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drak
     double num_in_contact = mode.sum();
     auto& grf = samples.emplace_back(Eigen::VectorXd::Zero(3 * n_contact_points));
     for(int i = 0; i<n_contact_points; i++){
-      grf.coeffRef(2 + 3 * i) = m * 9.81 /num_in_contact;
+      if(mode.coeff(i)){
+        grf.coeffRef(2 + 3 * i) = m * 9.81 /num_in_contact;
+      }
     }
   }
   return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(mode_trajectory.get_segment_times(), samples);

From 2d5fb27f600bbb40ca6c0c3e375036a59b7583ae Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 26 Oct 2022 23:22:19 -0400
Subject: [PATCH 24/76] Experimenting with ipopt

---
 .../cassie_kcmpc_trajopt.cc                   |  6 ++++--
 .../kinematic_centroidal_mpc.cc               | 19 +++++++++++--------
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 4b2a1ef8dc..11169907e3 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -97,7 +97,7 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
     options.SetOption(id, "dual_inf_tol", tol);
     options.SetOption(id, "constr_viol_tol", tol);
     options.SetOption(id, "compl_inf_tol", tol);
-    options.SetOption(id, "max_iter", 500);
+    options.SetOption(id, "max_iter", 10);
     options.SetOption(id, "nlp_lower_bound_inf", -1e6);
     options.SetOption(id, "nlp_upper_bound_inf", 1e6);
     options.SetOption(id, "print_timing_statistics", "yes");
@@ -109,7 +109,9 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
     options.SetOption(id, "acceptable_constr_viol_tol", tol);
     options.SetOption(id, "acceptable_obj_change_tol", 1e-3);
     options.SetOption(id, "acceptable_tol", 1e2);
-    options.SetOption(id, "acceptable_iter", 5);
+    options.SetOption(id, "acceptable_iter", 1);
+
+    options.SetOption(id, "warm_start_init_point", "no");
     mpc.Build(options);
   }
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index d3ecbbd9ef..b46ec7a623 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -254,14 +254,17 @@ void KinematicCentroidalMPC::SetZeroInitialGuess() {
 }
 
 drake::trajectories::PiecewisePolynomial<double> KinematicCentroidalMPC::Solve() {
-  auto start = std::chrono::high_resolution_clock::now();
-  solver_->Solve(*prog_, prog_->initial_guess(),
-                prog_->solver_options(),
-                result_.get());
-  auto finish = std::chrono::high_resolution_clock::now();
-  std::chrono::duration<double> elapsed = finish - start;
-  std::cout << "Solve time:" << elapsed.count() << std::endl;
-  std::cout << "Cost:" << result_->get_optimal_cost() << std::endl;
+  for(int i = 0; i < 5; i ++){
+    auto start = std::chrono::high_resolution_clock::now();
+    solver_->Solve(*prog_, prog_->initial_guess(),
+                   prog_->solver_options(),
+                   result_.get());
+    auto finish = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> elapsed = finish - start;
+    std::cout << "Solve time:" << elapsed.count() << std::endl;
+    std::cout << "Cost:" << result_->get_optimal_cost() << std::endl;
+    prog_->SetInitialGuessForAllVariables(result_->GetSolution());
+  }
 
   std::vector<double> time_points;
   std::vector<drake::MatrixX<double>> states;

From acb489df952c08db9292e8c9ea59eda122ca2afa Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 27 Oct 2022 14:43:48 -0400
Subject: [PATCH 25/76] First pass of transitioning over to object reference
 creation

---
 .../kinematic_centroidal_mpc/BUILD.bazel      |   5 +-
 .../cassie_kcmpc_trajopt.cc                   |  57 +++--
 .../cassie_reference_utils.cc                 | 146 +++++++++++++
 .../cassie_reference_utils.h                  |  12 ++
 .../reference_generator.h                     |  86 --------
 .../kinematic_centroidal_mpc/BUILD.bazel      |  11 +
 .../kcmpc_reference_generator.cc              | 194 +++++-------------
 .../kcmpc_reference_generator.h               |  58 ++++++
 .../kinematic_centroidal_mpc.cc               |   2 +-
 9 files changed, 303 insertions(+), 268 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
 delete mode 100644 examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
 rename examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc => systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc (54%)
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h

diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
index 9b06fa9ca3..a789723f80 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
@@ -59,11 +59,12 @@ cc_library(
 
 cc_library(
     name = "kinematic_centroidal_reference_generator",
-    srcs = ["reference_generator.cc"],
-    hdrs = ["reference_generator.h"],
+    srcs = ["cassie_reference_utils.cc"],
+    hdrs = ["cassie_reference_utils.h"],
     deps = [
         "//examples/Cassie:cassie_utils",
         "//common",
+        "//systems/controllers/kinematic_centroidal_mpc:kinematic_centroidal_reference_generator",
         "//multibody:visualization_utils",
         "@drake//:drake_shared_library",
     ],
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 11169907e3..fe80021103 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -12,9 +12,10 @@
 #include <drake/common/yaml/yaml_io.h>
 #include "common/find_resource.h"
 #include "systems/primitives/subvector_pass_through.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/reference_generator.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h"
 #include "examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h"
 #include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h"
+#include "systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h"
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
@@ -45,11 +46,6 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
   mpc.SetGains(gains);
 
   Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), base_height, stance_width);
-  auto context = plant.CreateDefaultContext();
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
-  const auto& com = plant.CalcCenterOfMassPositionInWorld(*context);
-  const auto& mass = plant.CalcTotalMass(*context);
-
   Gait stand;
   stand.period = 1;
   stand.gait_pattern = {{0, 1, drake::Vector<bool, 4>(true, true, true, true)}};
@@ -61,34 +57,31 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
                         {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
                         {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}};
 
-  auto com_trajectory = GenerateComTrajectory(com,
-                                              {{0, 0, 0},
-                                               {vel, 0, 0},
-                                               {vel, 0, 0}},
-                                              {0, 0.5, duration});
-  auto q_trajectory = GenerateGeneralizedPosTrajectory(reference_state.head(plant.num_positions()),
-                                                           reference_state.segment(4, 3) - com,
-                                                           com_trajectory,
-                                                           4);
-  auto v_trajectory = GenerateGeneralizedVelTrajectory(com_trajectory,
-                                                       plant.num_velocities(),
-                                                       3);
-  auto contact_sequence = GenerateModeSequence({stand, walk, walk}, {0, 0.5, duration});
-  auto grf_traj = GenerateGrfReference(contact_sequence, mass);
-  auto contact_traj = GenerateContactPointReference(plant,mpc.CreateContactPoints(plant, 0), q_trajectory, v_trajectory);
-  mpc.AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(grf_traj));
-  mpc.AddGenPosReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(q_trajectory));
-  mpc.AddGenVelReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(v_trajectory));
-  mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(com_trajectory));
-  mpc.AddContactTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(contact_traj));
+  KcmpcReferenceGenerator generator(plant, reference_state.head(plant.num_positions()), mpc.CreateContactPoints(plant,0));
+  std::vector<double> time_points = {0, 0.5, duration};
+  std::vector<Eigen::Vector3d> com_vel = {{{0, 0, 0},
+                                           {vel, 0, 0},
+                                           {vel, 0, 0}}};
+
+  std::vector<Gait> gait_samples = {stand, walk, walk};
+
+  generator.SetComKnotPoints({time_points, com_vel});
+  generator.SetGaitSequence({time_points, gait_samples});
+  generator.Build();
+
+  mpc.AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.grf_traj_));
+  mpc.AddGenPosReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.q_trajectory_));
+  mpc.AddGenVelReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.v_trajectory_));
+  mpc.AddComReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.com_trajectory_));
+  mpc.AddContactTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.contact_traj_));
   mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
-  mpc.SetModeSequence(contact_sequence);
+  mpc.SetModeSequence(generator.contact_sequence_);
 
   mpc.AddInitialStateConstraint(reference_state);
-  mpc.SetRobotStateGuess(q_trajectory, v_trajectory);
-  mpc.SetComPositionGuess(com_trajectory);
-  mpc.SetContactGuess(contact_traj);
-  mpc.SetForceGuess(grf_traj);
+  mpc.SetRobotStateGuess(generator.q_trajectory_, generator.v_trajectory_);
+  mpc.SetComPositionGuess(generator.com_trajectory_);
+  mpc.SetContactGuess(generator.contact_traj_);
+  mpc.SetForceGuess(generator.grf_traj_);
 
   {
     drake::solvers::SolverOptions options;
@@ -97,7 +90,7 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
     options.SetOption(id, "dual_inf_tol", tol);
     options.SetOption(id, "constr_viol_tol", tol);
     options.SetOption(id, "compl_inf_tol", tol);
-    options.SetOption(id, "max_iter", 10);
+    options.SetOption(id, "max_iter", 200);
     options.SetOption(id, "nlp_lower_bound_inf", -1e6);
     options.SetOption(id, "nlp_upper_bound_inf", 1e6);
     options.SetOption(id, "print_timing_statistics", "yes");
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
new file mode 100644
index 0000000000..6a64d5407c
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
@@ -0,0 +1,146 @@
+
+
+#include "cassie_reference_utils.h"
+#include <drake/systems/framework/diagram_builder.h>
+#include <drake/geometry/scene_graph.h>
+#include <drake/multibody/parsing/parser.h>
+#include <drake/multibody/plant/multibody_plant.h>
+#include <iostream>
+#include <drake/systems/analysis/simulator.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
+#include <drake/solvers/solve.h>
+#include "examples/Cassie/cassie_utils.h"
+#include "multibody/visualization_utils.h"
+
+using drake::geometry::SceneGraph;
+using drake::multibody::MultibodyPlant;
+using drake::multibody::Parser;
+
+Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
+                                     double pelvis_height,
+                                     double stance_width,
+                                     bool visualize) {
+  using Eigen::VectorXd;
+  using Eigen::Vector3d;
+  int n_q = plant.num_positions();
+  int n_v = plant.num_velocities();
+  int n_x = n_q + n_v;
+  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
+
+  Eigen::VectorXd q_ik_guess = Eigen::VectorXd::Zero(n_q);
+
+  std::map<std::string, double> pos_value_map;
+  Eigen::Vector4d quat(2000.06, -0.339462, -0.609533, -0.760854);
+  quat.normalize();
+  pos_value_map["base_qw"] = quat(0);
+  pos_value_map["base_qx"] = quat(1);
+  pos_value_map["base_qy"] = quat(2);
+  pos_value_map["base_qz"] = quat(3);
+  pos_value_map["base_x"] = 0.000889849;
+  pos_value_map["base_y"] = 0.000626865;
+  pos_value_map["base_z"] = 1.0009;
+  pos_value_map["hip_roll_left"] = 0.00927845;
+  pos_value_map["hip_roll_right"] = 0.00927845;
+  pos_value_map["hip_yaw_left"] = -0.000895805;
+  pos_value_map["hip_yaw_right"] = 0.000895805;
+  pos_value_map["hip_pitch_left"] = 0.610808;
+  pos_value_map["hip_pitch_right"] = 0.610808;
+  pos_value_map["knee_left"] = -1.35926;
+  pos_value_map["knee_right"] = -1.35926;
+  pos_value_map["ankle_joint_left"] = 1.00716;
+  pos_value_map["ankle_joint_right"] = 1.00716;
+  pos_value_map["toe_left"] = -M_PI / 2;
+  pos_value_map["toe_right"] = -M_PI / 2;
+
+  for (auto pair : pos_value_map) {
+    q_ik_guess(positions_map.at(pair.first)) = pair.second;
+  }
+
+  Eigen::Vector3d heel_rt_toe = {.122, 0, 0};
+
+  Eigen::Vector3d pelvis_pos = {0,0, pelvis_height};
+  Eigen::Vector3d l_toe_pos = {0.06, stance_width/2, 0};
+  Eigen::Vector3d r_toe_pos = {0.06, -stance_width/2, 0};
+
+  Eigen::Vector3d l_heel_pos = l_toe_pos - heel_rt_toe;
+  Eigen::Vector3d r_heel_pos = r_toe_pos-heel_rt_toe;
+
+
+  const auto& world_frame = plant.world_frame();
+  const auto& pelvis_frame = plant.GetFrameByName("pelvis");
+  const auto& toe_left_frame = plant.GetFrameByName("toe_left");
+  const auto& toe_right_frame = plant.GetFrameByName("toe_right");
+
+  drake::multibody::InverseKinematics ik(plant);
+  double eps = 1e-3;
+  Vector3d eps_vec = eps * VectorXd::Ones(3);
+  ik.AddPositionConstraint(pelvis_frame, Vector3d(0, 0, 0), world_frame,
+                           pelvis_pos - eps * VectorXd::Ones(3),
+                           pelvis_pos + eps * VectorXd::Ones(3));
+  ik.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
+                              world_frame, drake::math::RotationMatrix<double>(), eps);
+  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, world_frame,
+                           l_toe_pos - eps_vec,
+                           l_toe_pos + eps_vec);
+  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first, world_frame,
+                           l_heel_pos - eps_vec,
+                           l_heel_pos + eps_vec);
+
+  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first, world_frame,
+                           r_toe_pos - eps_vec, r_toe_pos + eps_vec);
+  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first, world_frame,
+                           r_heel_pos - eps_vec, r_heel_pos + eps_vec);
+
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("hip_yaw_left")) == 0);
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("hip_yaw_right")) == 0);
+  // Four bar linkage constraint (without spring)
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("knee_left")) +
+          (ik.q())(positions_map.at("ankle_joint_left")) ==
+          M_PI * 13 / 180.0);
+  ik.get_mutable_prog()->AddLinearConstraint(
+      (ik.q())(positions_map.at("knee_right")) +
+          (ik.q())(positions_map.at("ankle_joint_right")) ==
+          M_PI * 13 / 180.0);
+
+  ik.get_mutable_prog()->SetInitialGuess(ik.q(), q_ik_guess);
+  const auto result = drake::solvers::Solve(ik.prog());
+  const auto q_sol = result.GetSolution(ik.q());
+  VectorXd q_sol_normd(n_q);
+  q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q - 4);
+  q_ik_guess = q_sol_normd;
+
+  if(visualize){
+    // Build temporary diagram for visualization
+    drake::systems::DiagramBuilder<double> builder_ik;
+    drake::geometry::SceneGraph<double>& scene_graph_ik = *builder_ik.AddSystem<drake::geometry::SceneGraph>();
+    scene_graph_ik.set_name("scene_graph_ik");
+    MultibodyPlant<double> plant_ik(0.0);
+    Parser parser(&plant_ik, &scene_graph_ik);
+    std::string full_name =
+        dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+    parser.AddModelFromFile(full_name);
+    plant_ik.Finalize();
+
+    // Visualize
+    VectorXd x_const = VectorXd::Zero(n_x);
+    x_const.head(n_q) = q_sol;
+    drake::trajectories::PiecewisePolynomial<double> pp_xtraj(x_const);
+
+    dairlib::multibody::ConnectTrajectoryVisualizer(&plant_ik, &builder_ik,
+                                                    &scene_graph_ik, pp_xtraj);
+    auto diagram = builder_ik.Build();
+    drake::systems::Simulator<double> simulator(*diagram);
+    simulator.set_target_realtime_rate(.1);
+    simulator.Initialize();
+    simulator.AdvanceTo(1.0);
+  }
+
+  Eigen::VectorXd rv = Eigen::VectorXd::Zero(n_x);
+  rv.head(n_q) = q_ik_guess;
+  return rv;
+}
+
+
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
new file mode 100644
index 0000000000..71194da7bc
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
@@ -0,0 +1,12 @@
+
+#pragma once
+
+#include <drake/multibody/plant/multibody_plant.h>
+#include <Eigen/Core>
+#include <drake/common/trajectories/piecewise_polynomial.h>
+#include "multibody/kinematic/world_point_evaluator.h"
+
+Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
+                                     double pelvis_height,
+                                     double stance_width,
+                                     bool visualize = false);
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h b/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
deleted file mode 100644
index 5b99f9b63d..0000000000
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.h
+++ /dev/null
@@ -1,86 +0,0 @@
-
-#pragma once
-
-#include <drake/multibody/plant/multibody_plant.h>
-#include <Eigen/Core>
-#include <drake/common/trajectories/piecewise_polynomial.h>
-#include "multibody/kinematic/world_point_evaluator.h"
-
-struct Mode{
-  double start_phase;
-  double end_phase;
-  drake::VectorX<bool> contact_status;
-};
-
-struct Gait{
-  double period;
-  std::vector<Mode> gait_pattern;
-
-  drake::trajectories::PiecewisePolynomial<double> ToTrajectory(double current_time, double end_time) const;
-};
-
-Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
-                                     double pelvis_height,
-                                     double stance_width,
-                                     bool visualize = false);
-
-/*!
- * @brief Constructs a com trajectory given world frame velocity
- * @param current_com current xyz location of com
- * @param vel_ewrt_w vector of velocity commands
- * @param time_points vector of times for velocity commands, start time is first entry, final time of spline is last entry
- * @return spline describing com position
- */
-drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
-                                                              const std::vector<Eigen::Vector3d>& vel_ewrt_w,
-                                                              const std::vector<double>& time_points);
-
-
-/*!
- * @brief Creates a trajectory in generalized position given a nominal state and com trajectory
- * @param nominal_stand nominal generalized state of the robot
- * @param base_rt_com_ewrt_w offset from base to com
- * @param com_traj trajectory of center of mass
- * @param base_pos_start index in generalized state where base position starts
- * @return center of mass trajectory
- */
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
-                                                                                  const Eigen::Vector3d& base_rt_com_ewrt_w,
-                                                                                  const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                  int base_pos_start);
-
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                  int n_v,
-                                                                                  int base_vel_start);
-
-int FindCurrentMode(const Gait& active_gait, double time_now);
-
-drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
-                                                                      const std::vector<double>& time_points);
-
-/*!
- * @brief Calculates the next time a index of the zoh trajectory goes to a specific value
- * @param trajectory piecewise polynomial trajectory that is a zoh
- * @param current_time the time to start the search
- * @param index index of the trajectory output to track
- * @param value value searching for
- * @return current time if trajectory(current_time)[index] = value,
- *         the first time greater than current time and the value matches,
- *         If the trajectory never goes to value, returns trajectory.end_time()
- */
-double TimeNextValue(const drake::trajectories::PiecewisePolynomial<double>& trajectory,
-                     double current_time,
-                     int index,
-                     double value);
-
-
-drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
-                                                                      double m);
-
-drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(const drake::multibody::MultibodyPlant<double> &plant,
-                                                                               const std::vector<dairlib::multibody::WorldPointEvaluator<
-                                                                                   double>> &contacts,
-                                                                               const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &q_traj,
-                                                                               const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &v_traj);
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
index 222d6f5c4b..ea5a7db965 100644
--- a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
+++ b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
@@ -25,3 +25,14 @@ cc_library(
         "@drake//:drake_shared_library",
     ],
 )
+
+cc_library(
+    name = "kinematic_centroidal_reference_generator",
+    srcs = ["kcmpc_reference_generator.cc"],
+    hdrs = ["kcmpc_reference_generator.h"],
+    deps = [
+        "//common",
+        "//multibody/kinematic:kinematic",
+        "@drake//:drake_shared_library",
+    ],
+)
diff --git a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
similarity index 54%
rename from examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
rename to systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
index f7cc10e8b8..a97616b335 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/reference_generator.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
@@ -1,151 +1,10 @@
+#include "kcmpc_reference_generator.h"
+#include "multibody/multibody_utils.h"
 
 
-#include "reference_generator.h"
-#include <drake/systems/framework/diagram_builder.h>
-#include <drake/geometry/scene_graph.h>
-#include <drake/multibody/parsing/parser.h>
-#include <drake/multibody/plant/multibody_plant.h>
-#include <iostream>
-#include <drake/systems/analysis/simulator.h>
-#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
-#include <drake/solvers/solve.h>
-#include "examples/Cassie/cassie_utils.h"
-#include "multibody/visualization_utils.h"
-
-using drake::geometry::SceneGraph;
-using drake::multibody::MultibodyPlant;
-using drake::multibody::Parser;
-
-Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
-                                     double pelvis_height,
-                                     double stance_width,
-                                     bool visualize) {
-  using Eigen::VectorXd;
-  using Eigen::Vector3d;
-  int n_q = plant.num_positions();
-  int n_v = plant.num_velocities();
-  int n_x = n_q + n_v;
-  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
-
-  Eigen::VectorXd q_ik_guess = Eigen::VectorXd::Zero(n_q);
-
-  std::map<std::string, double> pos_value_map;
-  Eigen::Vector4d quat(2000.06, -0.339462, -0.609533, -0.760854);
-  quat.normalize();
-  pos_value_map["base_qw"] = quat(0);
-  pos_value_map["base_qx"] = quat(1);
-  pos_value_map["base_qy"] = quat(2);
-  pos_value_map["base_qz"] = quat(3);
-  pos_value_map["base_x"] = 0.000889849;
-  pos_value_map["base_y"] = 0.000626865;
-  pos_value_map["base_z"] = 1.0009;
-  pos_value_map["hip_roll_left"] = 0.00927845;
-  pos_value_map["hip_roll_right"] = 0.00927845;
-  pos_value_map["hip_yaw_left"] = -0.000895805;
-  pos_value_map["hip_yaw_right"] = 0.000895805;
-  pos_value_map["hip_pitch_left"] = 0.610808;
-  pos_value_map["hip_pitch_right"] = 0.610808;
-  pos_value_map["knee_left"] = -1.35926;
-  pos_value_map["knee_right"] = -1.35926;
-  pos_value_map["ankle_joint_left"] = 1.00716;
-  pos_value_map["ankle_joint_right"] = 1.00716;
-  pos_value_map["toe_left"] = -M_PI / 2;
-  pos_value_map["toe_right"] = -M_PI / 2;
-
-  for (auto pair : pos_value_map) {
-    q_ik_guess(positions_map.at(pair.first)) = pair.second;
-  }
-
-  Eigen::Vector3d heel_rt_toe = {.122, 0, 0};
-
-  Eigen::Vector3d pelvis_pos = {0,0, pelvis_height};
-  Eigen::Vector3d l_toe_pos = {0.06, stance_width/2, 0};
-  Eigen::Vector3d r_toe_pos = {0.06, -stance_width/2, 0};
-
-  Eigen::Vector3d l_heel_pos = l_toe_pos - heel_rt_toe;
-  Eigen::Vector3d r_heel_pos = r_toe_pos-heel_rt_toe;
-
-
-  const auto& world_frame = plant.world_frame();
-  const auto& pelvis_frame = plant.GetFrameByName("pelvis");
-  const auto& toe_left_frame = plant.GetFrameByName("toe_left");
-  const auto& toe_right_frame = plant.GetFrameByName("toe_right");
-
-  drake::multibody::InverseKinematics ik(plant);
-  double eps = 1e-3;
-  Vector3d eps_vec = eps * VectorXd::Ones(3);
-  ik.AddPositionConstraint(pelvis_frame, Vector3d(0, 0, 0), world_frame,
-                           pelvis_pos - eps * VectorXd::Ones(3),
-                           pelvis_pos + eps * VectorXd::Ones(3));
-  ik.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
-                              world_frame, drake::math::RotationMatrix<double>(), eps);
-  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, world_frame,
-                           l_toe_pos - eps_vec,
-                           l_toe_pos + eps_vec);
-  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first, world_frame,
-                           l_heel_pos - eps_vec,
-                           l_heel_pos + eps_vec);
-
-  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first, world_frame,
-                           r_toe_pos - eps_vec, r_toe_pos + eps_vec);
-  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first, world_frame,
-                           r_heel_pos - eps_vec, r_heel_pos + eps_vec);
-
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("hip_yaw_left")) == 0);
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("hip_yaw_right")) == 0);
-  // Four bar linkage constraint (without spring)
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("knee_left")) +
-          (ik.q())(positions_map.at("ankle_joint_left")) ==
-          M_PI * 13 / 180.0);
-  ik.get_mutable_prog()->AddLinearConstraint(
-      (ik.q())(positions_map.at("knee_right")) +
-          (ik.q())(positions_map.at("ankle_joint_right")) ==
-          M_PI * 13 / 180.0);
-
-  ik.get_mutable_prog()->SetInitialGuess(ik.q(), q_ik_guess);
-  const auto result = drake::solvers::Solve(ik.prog());
-  const auto q_sol = result.GetSolution(ik.q());
-  VectorXd q_sol_normd(n_q);
-  q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q - 4);
-  q_ik_guess = q_sol_normd;
-
-  if(visualize){
-    // Build temporary diagram for visualization
-    drake::systems::DiagramBuilder<double> builder_ik;
-    drake::geometry::SceneGraph<double>& scene_graph_ik = *builder_ik.AddSystem<drake::geometry::SceneGraph>();
-    scene_graph_ik.set_name("scene_graph_ik");
-    MultibodyPlant<double> plant_ik(0.0);
-    Parser parser(&plant_ik, &scene_graph_ik);
-    std::string full_name =
-        dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
-    parser.AddModelFromFile(full_name);
-    plant_ik.Finalize();
-
-    // Visualize
-    VectorXd x_const = VectorXd::Zero(n_x);
-    x_const.head(n_q) = q_sol;
-    drake::trajectories::PiecewisePolynomial<double> pp_xtraj(x_const);
-
-    dairlib::multibody::ConnectTrajectoryVisualizer(&plant_ik, &builder_ik,
-                                                    &scene_graph_ik, pp_xtraj);
-    auto diagram = builder_ik.Build();
-    drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(.1);
-    simulator.Initialize();
-    simulator.AdvanceTo(1.0);
-  }
-
-  Eigen::VectorXd rv = Eigen::VectorXd::Zero(n_x);
-  rv.head(n_q) = q_ik_guess;
-  return rv;
-}
-
 drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
-                                                              const std::vector<Eigen::Vector3d>& vel_ewrt_w,
-                                                              const std::vector<double>& time_points){
+                                                                       const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+                                                                       const std::vector<double>& time_points){
   DRAKE_DEMAND(vel_ewrt_w.size() == time_points.size());
   auto n_points = vel_ewrt_w.size();
 
@@ -283,9 +142,50 @@ drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(c
     for(int i = 0; i <n_contact; i++){
       knot_point_value.block(i * 3, 0,  3, 1) = contacts[i].EvalFull(*context);
       knot_point_value.coeffRef(i*3 + 2) = 0;
-      //TODO fix velocity reference
       knot_point_value.block(3 * n_contact + i * 3, 0, 3, 1) = contacts[i].EvalFullJacobianDotTimesV(*context);
     }
   }
   return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(break_points, samples);
-}
\ No newline at end of file
+}
+
+KcmpcReferenceGenerator::KcmpcReferenceGenerator(const drake::multibody::MultibodyPlant<double> &plant,
+                                                 const Eigen::VectorXd& nominal_stand,
+                                                 const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &contacts):
+                                                 plant_(plant),
+                                                 contacts_(contacts),
+                                                 nominal_stand_(nominal_stand),
+                                                 context_(plant.CreateDefaultContext()){
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, nominal_stand_, context_.get());
+  base_rt_com_ewrt_w = nominal_stand_.segment(4, 3) - plant_.CalcCenterOfMassPositionInWorld(*context_);
+  m_ = plant_.CalcTotalMass(*context_);
+}
+
+
+void KcmpcReferenceGenerator::Build() {
+  Build(nominal_stand_.segment(4, 3) - base_rt_com_ewrt_w);
+}
+
+void KcmpcReferenceGenerator::Build(const Eigen::Vector3d& com) {
+  com_trajectory_ = GenerateComTrajectory(com,
+                                              com_vel_knot_points_.samples,
+                                              com_vel_knot_points_.times);
+  q_trajectory_ = GenerateGeneralizedPosTrajectory(nominal_stand_,
+                                                       base_rt_com_ewrt_w,
+                                                       com_trajectory_,
+                                                       4);
+  v_trajectory_ = GenerateGeneralizedVelTrajectory(com_trajectory_,
+                                                       plant_.num_velocities(),
+                                                       3);
+  contact_sequence_ = GenerateModeSequence(gait_knot_points_.samples, gait_knot_points_.times);
+  grf_traj_ = GenerateGrfReference(contact_sequence_, m_);
+  contact_traj_ = GenerateContactPointReference(plant_,contacts_, q_trajectory_, v_trajectory_);
+  built_ = true;
+}
+
+void KcmpcReferenceGenerator::SetComKnotPoints(const KnotPoints<Eigen::Vector3d> &com_knot_points) {
+  com_vel_knot_points_ = com_knot_points;
+}
+
+void KcmpcReferenceGenerator::SetGaitSequence(const KnotPoints<Gait> &gait_knot_points) {
+  gait_knot_points_ = gait_knot_points;
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
new file mode 100644
index 0000000000..363422176f
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <drake/multibody/plant/multibody_plant.h>
+#include <drake/common/trajectories/piecewise_polynomial.h>
+#include "multibody/kinematic/world_point_evaluator.h"
+
+struct Mode{
+  double start_phase;
+  double end_phase;
+  drake::VectorX<bool> contact_status;
+};
+
+struct Gait{
+  double period;
+  std::vector<Mode> gait_pattern;
+  drake::trajectories::PiecewisePolynomial<double> ToTrajectory(double current_time, double end_time) const;
+};
+
+template <typename T>
+struct KnotPoints{
+  std::vector<double> times;
+  std::vector<T> samples;
+};
+
+class KcmpcReferenceGenerator{
+ public:
+  KcmpcReferenceGenerator(const drake::multibody::MultibodyPlant<double>& plant,
+                          const Eigen::VectorXd& nominal_stand,
+                          const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& contacts);
+
+
+  void SetComKnotPoints(const KnotPoints<Eigen::Vector3d>& com_knot_points);
+
+  void SetGaitSequence(const KnotPoints<Gait>& gait_knot_points);
+
+  void Build();
+
+  void Build(const Eigen::Vector3d& com);
+
+  drake::trajectories::PiecewisePolynomial<double> com_trajectory_;
+  drake::trajectories::PiecewisePolynomial<double> q_trajectory_;
+  drake::trajectories::PiecewisePolynomial<double> v_trajectory_;
+  drake::trajectories::PiecewisePolynomial<double> contact_sequence_;
+  drake::trajectories::PiecewisePolynomial<double> grf_traj_;
+  drake::trajectories::PiecewisePolynomial<double> contact_traj_;
+
+ private:
+  const drake::multibody::MultibodyPlant<double>& plant_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> contacts_;
+  Eigen::VectorXd nominal_stand_;
+  KnotPoints<Eigen::Vector3d> com_vel_knot_points_;
+  KnotPoints<Gait> gait_knot_points_;
+  bool built_ = false;
+  double m_;
+  Eigen::Vector3d base_rt_com_ewrt_w;
+  std::unique_ptr<drake::systems::Context<double>> context_;
+
+};
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index b46ec7a623..6d3a8ef6f3 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -254,7 +254,7 @@ void KinematicCentroidalMPC::SetZeroInitialGuess() {
 }
 
 drake::trajectories::PiecewisePolynomial<double> KinematicCentroidalMPC::Solve() {
-  for(int i = 0; i < 5; i ++){
+  for(int i = 0; i < 1; i ++){
     auto start = std::chrono::high_resolution_clock::now();
     solver_->Solve(*prog_, prog_->initial_guess(),
                    prog_->solver_options(),

From 61de3445820f09ebeadde626d7fbe506a9e2a45a Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 31 Oct 2022 10:27:01 -0400
Subject: [PATCH 26/76] Cleaning up code for eventual push

---
 .../cassie_kcmpc_trajopt.cc                   | 13 +++--
 .../cassie_kinematic_centroidal_mpc.cc        |  1 +
 .../cassie_kinematic_centroidal_mpc.h         | 12 ++++-
 .../cassie_reference_utils.h                  |  8 +++
 .../kcmpc_reference_generator.cc              | 12 ++++-
 .../kcmpc_reference_generator.h               | 51 +++++++++++++++++--
 .../kinematic_centroidal_mpc.h                | 42 ++++++++++++---
 7 files changed, 121 insertions(+), 18 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index fe80021103..cd7ef2cbef 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -42,10 +42,11 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
   plant.Finalize();
   plant_vis.Finalize();
 
+  // Create MPC and set gains
   CassieKinematicCentroidalMPC mpc (plant, n_knot_points, duration/(n_knot_points-1), 0.4);
   mpc.SetGains(gains);
 
-  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), base_height, stance_width);
+  // Create gaits
   Gait stand;
   stand.period = 1;
   stand.gait_pattern = {{0, 1, drake::Vector<bool, 4>(true, true, true, true)}};
@@ -57,18 +58,22 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
                         {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
                         {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}};
 
+  // Create reference
+  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), base_height, stance_width);
   KcmpcReferenceGenerator generator(plant, reference_state.head(plant.num_positions()), mpc.CreateContactPoints(plant,0));
+
+
+  // Specify knot points
   std::vector<double> time_points = {0, 0.5, duration};
   std::vector<Eigen::Vector3d> com_vel = {{{0, 0, 0},
                                            {vel, 0, 0},
                                            {vel, 0, 0}}};
-
   std::vector<Gait> gait_samples = {stand, walk, walk};
-
   generator.SetComKnotPoints({time_points, com_vel});
   generator.SetGaitSequence({time_points, gait_samples});
   generator.Build();
 
+  // Add reference and mode sequence
   mpc.AddForceTrackingReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.grf_traj_));
   mpc.AddGenPosReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.q_trajectory_));
   mpc.AddGenVelReference(std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(generator.v_trajectory_));
@@ -77,6 +82,7 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
   mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
   mpc.SetModeSequence(generator.contact_sequence_);
 
+  // Set initial guess
   mpc.AddInitialStateConstraint(reference_state);
   mpc.SetRobotStateGuess(generator.q_trajectory_, generator.v_trajectory_);
   mpc.SetComPositionGuess(generator.com_trajectory_);
@@ -108,7 +114,6 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
     mpc.Build(options);
   }
 
-  std::cout<<"Adding visualization callback"<<std::endl;
   double alpha = .2;
   mpc.CreateVisualizationCallback(
       "examples/Cassie/urdf/cassie_fixed_springs.urdf", alpha);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 6054cb05a0..31c8ef1896 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -39,6 +39,7 @@ std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCent
 
   return {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval};
 }
+
 void CassieKinematicCentroidalMPC::AddLoopClosure() {
   loop_closure_evaluators.add_evaluator(&l_loop_evaluator_);
   loop_closure_evaluators.add_evaluator(&r_loop_evaluator_);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 4459b29ee9..cfcc96540c 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -5,7 +5,7 @@
 #include "examples/Cassie/cassie_utils.h"
 
 /*!
- * @brief Cassie specific child class for kinematic centroidal mpc.
+ * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop closure, joint limits, and cassie contact points
  */
 class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
  public:
@@ -18,8 +18,18 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
     AddLoopClosure();
   }
 
+  /*!
+   * @brief creates vector of world point evaluators for cassie
+   * @param plant cassie plant
+   * @param mu coefficient of friction
+   * @return
+   */
   std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
  private:
+
+  /*!
+   * @brief Adds loop closure constraints to the mpc
+   */
   void AddLoopClosure();
 
   dairlib::multibody::DistanceEvaluator<double> l_loop_evaluator_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
index 71194da7bc..2950f65324 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
@@ -6,6 +6,14 @@
 #include <drake/common/trajectories/piecewise_polynomial.h>
 #include "multibody/kinematic/world_point_evaluator.h"
 
+/*!
+ * @brief Construct a nominal stand for cassie
+ * @param plant cassie plant
+ * @param pelvis_height
+ * @param stance_width
+ * @param visualize true if the stand should be visualized
+ * @return vector of state [nq + nv]
+ */
 Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
                                      double pelvis_height,
                                      double stance_width,
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
index a97616b335..1fcce03b43 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
@@ -89,6 +89,14 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double curre
   samples.push_back(samples[samples.size()-1]);
   return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(break_points, samples);
 }
+void Gait::Is_Valid() const {
+  DRAKE_ASSERT(period > 0);
+  DRAKE_ASSERT(gait_pattern[0].start_phase == 0);
+  DRAKE_ASSERT(gait_pattern[gait_pattern.size()-1].end_phase == 1);
+  for(int i = 0; i < gait_pattern.size() - 1; i++){
+    DRAKE_ASSERT(gait_pattern[i].end_phase == gait_pattern[i+1].start_phase);
+  }
+}
 
 double TimeNextValue(const drake::trajectories::PiecewisePolynomial<double>& trajectory,
                      double current_time,
@@ -179,7 +187,6 @@ void KcmpcReferenceGenerator::Build(const Eigen::Vector3d& com) {
   contact_sequence_ = GenerateModeSequence(gait_knot_points_.samples, gait_knot_points_.times);
   grf_traj_ = GenerateGrfReference(contact_sequence_, m_);
   contact_traj_ = GenerateContactPointReference(plant_,contacts_, q_trajectory_, v_trajectory_);
-  built_ = true;
 }
 
 void KcmpcReferenceGenerator::SetComKnotPoints(const KnotPoints<Eigen::Vector3d> &com_knot_points) {
@@ -187,5 +194,8 @@ void KcmpcReferenceGenerator::SetComKnotPoints(const KnotPoints<Eigen::Vector3d>
 }
 
 void KcmpcReferenceGenerator::SetGaitSequence(const KnotPoints<Gait> &gait_knot_points) {
+  for(const auto& gait : gait_knot_points.samples){
+    gait.Is_Valid();
+  }
   gait_knot_points_ = gait_knot_points;
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
index 363422176f..bcf3dbd1c1 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
@@ -4,18 +4,41 @@
 #include <drake/common/trajectories/piecewise_polynomial.h>
 #include "multibody/kinematic/world_point_evaluator.h"
 
+/*!
+ * @brief Struct for a given mode as part of a mode sequence
+ */
 struct Mode{
   double start_phase;
   double end_phase;
-  drake::VectorX<bool> contact_status;
+  drake::VectorX<bool> contact_status; /// Vector describing which contacts are active
 };
 
+/*!
+ * @brief Struct for defining a gait consisting of a vector of Modes, and a period for the whole gait.
+ */
 struct Gait{
   double period;
-  std::vector<Mode> gait_pattern;
+  std::vector<Mode> gait_pattern; /// Vector of modes. Start phase of the first mode must be 0, end phase of the last mode
+                                  /// must be 1, and no time gaps between start and end of sequential modes
+
+  /*!
+   * @brief converts the gait into a trajectory of of the contact timing
+   * @param current_time time for the trajectory to start
+   * @param end_time time for the trajectory to end
+   * @return the trajectory
+   */
   drake::trajectories::PiecewisePolynomial<double> ToTrajectory(double current_time, double end_time) const;
+
+  /*!
+   * @brief Checks to make sure the gait is valid, Asserts if not valid
+   */
+  void Is_Valid() const;
 };
 
+/*!
+ * @brief struct consisting of a collection of times and samples used for passing inputs to reference generator
+ * @tparam T
+ */
 template <typename T>
 struct KnotPoints{
   std::vector<double> times;
@@ -24,17 +47,38 @@ struct KnotPoints{
 
 class KcmpcReferenceGenerator{
  public:
+
+  /*!
+   * @brief constructor for reference generator
+   * @param plant
+   * @param nominal_stand nominal stand of size [nq]
+   * @param contacts Vector of world point evalator describing contacts
+   */
   KcmpcReferenceGenerator(const drake::multibody::MultibodyPlant<double>& plant,
                           const Eigen::VectorXd& nominal_stand,
                           const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& contacts);
 
-
+  /*!
+   * @brief specify com velocity at specific times
+   * @param com_knot_points
+   */
   void SetComKnotPoints(const KnotPoints<Eigen::Vector3d>& com_knot_points);
 
+  /*!
+   * @brief specify gait at specific times
+   * @param gait_knot_points
+   */
   void SetGaitSequence(const KnotPoints<Gait>& gait_knot_points);
 
+  /*!
+   * @brief construct references assuming initial com is the from the nominal stand
+   */
   void Build();
 
+  /*!
+   * @brief Constructs references based on the time of the knot points
+   * @param com initial location of the com
+   */
   void Build(const Eigen::Vector3d& com);
 
   drake::trajectories::PiecewisePolynomial<double> com_trajectory_;
@@ -50,7 +94,6 @@ class KcmpcReferenceGenerator{
   Eigen::VectorXd nominal_stand_;
   KnotPoints<Eigen::Vector3d> com_vel_knot_points_;
   KnotPoints<Gait> gait_knot_points_;
-  bool built_ = false;
   double m_;
   Eigen::Vector3d base_rt_com_ewrt_w;
   std::unique_ptr<drake::systems::Context<double>> context_;
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index bb1e358094..969543753a 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -18,15 +18,15 @@
  * Humanoid Robots (November 2014).
  *
  * The optimization contains two coupled problems. The centroidal problem optimizes over:
- *      Body orientation
+ *      Angular momentum
+ *      Linear momentum
  *      Center of mass position
- *      Body angular velocity
- *      Center of mass velocity
  *      Contact position
  *      Contact velocity
  *      Contact force
  *  While the kinematics problem optimzes over:
- *      Robot state (positions and velocity, including floating base)
+ *      Generalized positions (including floating base)
+ *      Generalized velocities (including floating boase)
  *
  * A series of constraints couple the kinematic state to the centroidal state ensuring the contact point's, com position,
  * com velocity, body orientation, body angular velocity all align up
@@ -47,6 +47,10 @@ class KinematicCentroidalMPC {
                          const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& contact_points);
 
 
+  /*!
+   * @brief Sets the cost for the mpc problem using a gains struct
+   * @param gains
+   */
   void SetGains(const KinematicCentroidalGains& gains);
 
   /*!
@@ -62,17 +66,21 @@ class KinematicCentroidalMPC {
   void AddGenVelReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
 
   /*!
-   * @brief Adds a cost and reference for the centroidal state of the robot (x - x_ref)^T Q (x - x_ref)
+   * @brief Adds a cost and reference for the center of mass position of the robot (x - x_ref)^T Q (x - x_ref)
    * @param ref_traj trajectory in time
    */
   void AddComReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
 
   /*!
    * @brief Adds a cost and reference for the contact position and velocity (x - x_ref)^T Q (x - x_ref)
-   * @param contact_ref_traj trajectory in time
+   * @param contact_ref_traj trajectory in time, order is all contact pos, all contact vel
    */
   void AddContactTrackingReference(std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj);
 
+  /*!
+   * @brief Adds a cost and reference for the angular and linear momentum of the robot (x - x_ref)^T Q (x - x_ref)
+   * @param ref_traj
+   */
   void AddMomentumReference(std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
 
   /*!
@@ -217,16 +225,36 @@ class KinematicCentroidalMPC {
    */
   void AddPlantJointLimits(const std::vector<std::string>& joints_to_limit);
 
+  /*!
+   * @brief Adds a kinematic position constraint to the optimization
+   * @param con a shared pointer to the constraint
+   * @param vars the decision variables for the constraint
+   */
   void AddKinematicConstraint(std::shared_ptr<dairlib::multibody::KinematicPositionConstraint<double>> con,
                               const Eigen::Ref<const drake::solvers::VectorXDecisionVariable>& vars);
 
+  /*!
+   * @brief Adds a constraint on com height to all knot points
+   * @param lb
+   * @param ub
+   */
   void AddComHeightBoundingConstraint(double lb, double ub);
 
   int num_knot_points() const{
     return n_knot_points_;
   }
+
+  /*!
+   * @brief Set the mode sequence
+   * @param contact_sequence vector where `contact_sequence[knot_point][contact_index]` tells you if at `knot_point` is
+   *                            `contact_index` active
+   */
   void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
 
+  /*!
+   * @brief Set the mode sequence via 
+   * @param contact_sequence
+   */
   void SetModeSequence(const drake::trajectories::PiecewisePolynomial<double>& contact_sequence);
 
   void AddInitialStateConstraint(const Eigen::VectorXd& state);
@@ -264,8 +292,6 @@ class KinematicCentroidalMPC {
   void AddFrictionConeConstraints();
 
 //  void AddTorqueLimits();
-//
-//  void AddStateLimits();
 
   /*!
    * @brief Add costs from internally stored variables

From 79238246dc1b14e370da0e64e09d3c65a6a8c84e Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 31 Oct 2022 14:08:12 -0400
Subject: [PATCH 27/76] Added more comments

---
 .../kinematic_centroidal_mpc/kinematic_centroidal_mpc.h        | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 969543753a..a33aaf1140 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -252,7 +252,8 @@ class KinematicCentroidalMPC {
   void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
 
   /*!
-   * @brief Set the mode sequence via 
+   * @brief Set the mode sequence via a trajectory. The value of the trajectory at each time, cast to a bool is if a contact
+   *            point is active or not
    * @param contact_sequence
    */
   void SetModeSequence(const drake::trajectories::PiecewisePolynomial<double>& contact_sequence);

From bd78dd3f798f49101de4218e1437df95cfbee132 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 31 Oct 2022 22:33:15 -0400
Subject: [PATCH 28/76] Added tons of comments

---
 .../cassie_kcmpc_trajopt.cc                   | 20 ++--
 .../cassie_reference_utils.cc                 |  2 -
 .../kinematic_centroidal_mpc_gains.yaml       |  8 +-
 .../kinematic_centroidal_mpc/BUILD.bazel      |  8 +-
 .../kinematic_centroidal_mpc/gait.cc          | 48 +++++++++
 .../kinematic_centroidal_mpc/gait.h           | 49 ++++++++++
 .../kcmpc_reference_generator.cc              | 98 +------------------
 .../kcmpc_reference_generator.h               | 32 +-----
 .../kinematic_centroidal_mpc.cc               |  4 +-
 .../reference_generation_utils.cc             | 52 ++++++++++
 .../reference_generation_utils.h              | 50 ++++++++++
 11 files changed, 222 insertions(+), 149 deletions(-)
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/gait.cc
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/gait.h
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index cd7ef2cbef..d45a4d4a03 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -47,16 +47,14 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
   mpc.SetGains(gains);
 
   // Create gaits
-  Gait stand;
-  stand.period = 1;
-  stand.gait_pattern = {{0, 1, drake::Vector<bool, 4>(true, true, true, true)}};
+  Gait stand ({{0, 1, drake::Vector<bool, 4>(true, true, true, true)}});
+  stand.SetPeriod(1);
 
-  Gait walk;
-  walk.period = 1;
-  walk.gait_pattern = {{0, 0.4, drake::Vector<bool, 4>(true, true, false, false)},
-                        {0.4, 0.5, drake::Vector<bool, 4>(true, true, true, true)},
-                        {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
-                        {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}};
+  Gait walk({{0, 0.4, drake::Vector<bool, 4>(true, true, false, false)},
+             {0.4, 0.5, drake::Vector<bool, 4>(true, true, true, true)},
+             {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
+             {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}});
+  walk.SetPeriod(1);
 
   // Create reference
   Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), base_height, stance_width);
@@ -141,12 +139,12 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(1.0);
+    simulator.set_target_realtime_rate(0.5);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
   }
 }
 
 int main(int argc, char* argv[]) {
-  DoMain(36, 3, 1.2, 0.2, 0.5, 1e-2);
+  DoMain(36, 3, 1.0, 0.3, 0.5, 1e-2);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
index 6a64d5407c..49ef856c53 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
@@ -1,5 +1,3 @@
-
-
 #include "cassie_reference_utils.h"
 #include <drake/systems/framework/diagram_builder.h>
 #include <drake/geometry/scene_graph.h>
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
index 0487437c36..c39069269f 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
@@ -14,9 +14,9 @@ generalized_positions:
   ankle_joint_: 0.0001
   toe_: 0.0001
 generalized_velocities:
-  base_wx: 0.01
-  base_wy: 0.01
-  base_wz: 0.01
+  base_wx: 0.5
+  base_wy: 0.5
+  base_wz: 0.5
   base_vx: 0.1
   base_vy: 0.1
   base_vz: 0.1
@@ -27,7 +27,7 @@ generalized_velocities:
   ankle_joint_: 0.002
   toe_: 0.002
 
-contact_pos: [0.1, 2, 0.001]
+contact_pos: [0.1, 8, 0.001]
 contact_vel: [0.02, 0.02, 0.02]
 contact_force: [0.0001, 0.0001, 0.0001]
 lin_momentum: [0.00001, 0.00001, 0.00001]
diff --git a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
index ea5a7db965..9e2f8c1fa2 100644
--- a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
+++ b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
@@ -28,8 +28,12 @@ cc_library(
 
 cc_library(
     name = "kinematic_centroidal_reference_generator",
-    srcs = ["kcmpc_reference_generator.cc"],
-    hdrs = ["kcmpc_reference_generator.h"],
+    srcs = ["kcmpc_reference_generator.cc",
+            "reference_generation_utils.cc",
+            "gait.cc"],
+    hdrs = ["kcmpc_reference_generator.h",
+            "reference_generation_utils.h",
+            "gait.h"],
     deps = [
         "//common",
         "//multibody/kinematic:kinematic",
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.cc b/systems/controllers/kinematic_centroidal_mpc/gait.cc
new file mode 100644
index 0000000000..f351e35cf9
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/gait.cc
@@ -0,0 +1,48 @@
+#include "gait.h"
+
+int Gait::CurrentMode(double time_now) const{
+  double phase_now = fmod(time_now/period_, 1);
+  for(int i = 0; i < gait_pattern_.size(); i++){
+    const auto& mode = gait_pattern_[i];
+    if(mode.start_phase <= phase_now && mode.end_phase > phase_now){
+      return i;
+    }
+  }
+  DRAKE_ASSERT(false);
+  return 0;
+}
+
+drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double current_time, double end_time) const {
+  std::vector<double> break_points;
+  std::vector<drake::MatrixX<double>> samples;
+
+  // Calculate initial mode index, and phase
+  int current_mode = CurrentMode( current_time);
+  double current_phase = fmod(current_time/period_, 1);
+
+  // Loop until time is greater than end time
+  while(current_time < end_time){
+    // Add the break point for the current time
+    break_points.push_back(current_time);
+    samples.emplace_back(gait_pattern_[current_mode].contact_status.cast<double>());
+
+    // Update time based on how much longer the current mode will last
+    current_time += (gait_pattern_[current_mode].end_phase - current_phase) * period_;
+    // Update the mode and mod if necessary
+    current_mode = (current_mode + 1) % gait_pattern_.size();
+    // The new phase is the start phase of the updated mode
+    current_phase = gait_pattern_[current_mode].start_phase;
+  }
+  break_points.push_back(end_time);
+  samples.push_back(samples[samples.size()-1]);
+  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(break_points, samples);
+}
+
+void Gait::Is_Valid() const {
+  DRAKE_ASSERT(period_ > 0);
+  DRAKE_ASSERT(gait_pattern_[0].start_phase == 0);
+  DRAKE_ASSERT(gait_pattern_[gait_pattern_.size()-1].end_phase == 1);
+  for(int i = 0; i < gait_pattern_.size() - 1; i++){
+    DRAKE_ASSERT(gait_pattern_[i].end_phase == gait_pattern_[i+1].start_phase);
+  }
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.h b/systems/controllers/kinematic_centroidal_mpc/gait.h
new file mode 100644
index 0000000000..d2573a55b5
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/gait.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <drake/common/trajectories/piecewise_polynomial.h>
+
+/*!
+ * @brief Struct for a given mode as part of a mode sequence
+ */
+struct Mode{
+  double start_phase;
+  double end_phase;
+  drake::VectorX<bool> contact_status; /// Vector describing which contacts are active
+};
+
+/*!
+ * @brief Clss for defining a gait consisting of a vector of Modes, and a period for the whole gait.
+ */
+class Gait{
+ public:
+  Gait(const std::vector<Mode>& gait_pattern, double period = 0): gait_pattern_(gait_pattern),
+  period_(period){};
+
+/*!
+ * @brief converts the gait into a trajectory from current time to end time
+ * @param current_time
+ * @param end_time
+ * @return trajectory where value at time t converted to a bool is a vector of which feet are in stance
+ */
+  drake::trajectories::PiecewisePolynomial<double> ToTrajectory(double current_time, double end_time) const;
+
+  /*!
+   * @brief Checks to make sure the gait is valid, Asserts if not valid
+   */
+  void Is_Valid() const;
+
+  void SetPeriod(double period){period_ = period;}
+
+ private:
+  /*!
+   * @brief find the index for the current mode that the gait is in
+   * @param time_now
+   * @return
+   */
+  int CurrentMode(double time_now) const;
+
+  double period_;
+  std::vector<Mode> gait_pattern_; /// Vector of modes. Start phase of the first mode must be 0, end phase of the last mode
+  /// must be 1, and no time gaps between start and end of sequential modes
+
+};
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
index 1fcce03b43..c1f670ebf1 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
@@ -1,102 +1,6 @@
 #include "kcmpc_reference_generator.h"
 #include "multibody/multibody_utils.h"
-
-
-drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
-                                                                       const std::vector<Eigen::Vector3d>& vel_ewrt_w,
-                                                                       const std::vector<double>& time_points){
-  DRAKE_DEMAND(vel_ewrt_w.size() == time_points.size());
-  auto n_points = vel_ewrt_w.size();
-
-  std::vector<drake::MatrixX<double>> samples(n_points);
-  samples[0] = current_com;
-  for(int i = 1; i<n_points; i++){
-    samples[i] = samples[i-1] + (time_points[i] - time_points[i-1]) * vel_ewrt_w[i-1];
-  }
-  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points, samples);
-}
-
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
-                                                                                  const Eigen::Vector3d& base_rt_com_ewrt_w,
-                                                                                  const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                  int base_pos_start){
-  auto n_points = com_traj.get_segment_times().size();
-  std::vector<drake::MatrixX<double>> samples(n_points);
-  for(int i = 0; i < n_points; i++){
-    samples[i] = nominal_stand;
-    samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(com_traj.get_segment_times()[i]) + base_rt_com_ewrt_w;
-  }
-  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(com_traj.get_segment_times(), samples);
-}
-
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                  int n_v,
-                                                                                  int base_vel_start){
-  auto n_points = com_traj.get_segment_times().size();
-  std::vector<drake::MatrixX<double>> samples(n_points);
-  for(int i = 0; i < n_points; i++){
-    samples[i] = drake::VectorX<double>::Zero(n_v);
-    samples[i].block<3,1>(base_vel_start,0, 3, 1) = com_traj.derivative().value(com_traj.get_segment_times()[i]);
-  }
-  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(com_traj.get_segment_times(), samples);
-}
-
-int FindCurrentMode(const Gait& active_gait, double time_now){
-  double phase_now = fmod(time_now/active_gait.period, 1);
-  for(int i = 0; i < active_gait.gait_pattern.size(); i++){
-    const auto& mode = active_gait.gait_pattern[i];
-    if(mode.start_phase <= phase_now && mode.end_phase > phase_now){
-      return i;
-    }
-  }
-  DRAKE_ASSERT(false);
-  return 0;
-}
-
-drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
-                                                                      const std::vector<double>& time_points){
-  DRAKE_DEMAND(gait_sequence.size() == time_points.size());
-
-  auto traj = gait_sequence[0].ToTrajectory(time_points[0], time_points[1]);
-  for(int i = 1; i<gait_sequence.size() - 1 ; i++){
-    traj.ConcatenateInTime(gait_sequence[i].ToTrajectory(time_points[i], time_points[i + 1]));
-  }
-  return traj;
-}
-
-drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double current_time, double end_time) const {
-  std::vector<double> break_points;
-  std::vector<drake::MatrixX<double>> samples;
-
-  // Calculate initial mode index, and phase
-  int current_mode = FindCurrentMode(*this, current_time);
-  double current_phase = fmod(current_time/period, 1);
-
-  // Loop until time is greater than end time
-  while(current_time < end_time){
-    // Add the break point for the current time
-    break_points.push_back(current_time);
-    samples.emplace_back(gait_pattern[current_mode].contact_status.cast<double>());
-
-    // Update time based on how much longer the current mode will last
-    current_time += (gait_pattern[current_mode].end_phase - current_phase) * period;
-    // Update the mode and mod if necessary
-    current_mode = (current_mode + 1) % gait_pattern.size();
-    // The new phase is the start phase of the updated mode
-    current_phase = gait_pattern[current_mode].start_phase;
-  }
-  break_points.push_back(end_time);
-  samples.push_back(samples[samples.size()-1]);
-  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(break_points, samples);
-}
-void Gait::Is_Valid() const {
-  DRAKE_ASSERT(period > 0);
-  DRAKE_ASSERT(gait_pattern[0].start_phase == 0);
-  DRAKE_ASSERT(gait_pattern[gait_pattern.size()-1].end_phase == 1);
-  for(int i = 0; i < gait_pattern.size() - 1; i++){
-    DRAKE_ASSERT(gait_pattern[i].end_phase == gait_pattern[i+1].start_phase);
-  }
-}
+#include "systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h"
 
 double TimeNextValue(const drake::trajectories::PiecewisePolynomial<double>& trajectory,
                      double current_time,
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
index bcf3dbd1c1..7e636aaafb 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
@@ -3,37 +3,7 @@
 #include <drake/multibody/plant/multibody_plant.h>
 #include <drake/common/trajectories/piecewise_polynomial.h>
 #include "multibody/kinematic/world_point_evaluator.h"
-
-/*!
- * @brief Struct for a given mode as part of a mode sequence
- */
-struct Mode{
-  double start_phase;
-  double end_phase;
-  drake::VectorX<bool> contact_status; /// Vector describing which contacts are active
-};
-
-/*!
- * @brief Struct for defining a gait consisting of a vector of Modes, and a period for the whole gait.
- */
-struct Gait{
-  double period;
-  std::vector<Mode> gait_pattern; /// Vector of modes. Start phase of the first mode must be 0, end phase of the last mode
-                                  /// must be 1, and no time gaps between start and end of sequential modes
-
-  /*!
-   * @brief converts the gait into a trajectory of of the contact timing
-   * @param current_time time for the trajectory to start
-   * @param end_time time for the trajectory to end
-   * @return the trajectory
-   */
-  drake::trajectories::PiecewisePolynomial<double> ToTrajectory(double current_time, double end_time) const;
-
-  /*!
-   * @brief Checks to make sure the gait is valid, Asserts if not valid
-   */
-  void Is_Valid() const;
-};
+#include "systems/controllers/kinematic_centroidal_mpc/gait.h"
 
 /*!
  * @brief struct consisting of a collection of times and samples used for passing inputs to reference generator
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 6d3a8ef6f3..7d63f14cc8 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -116,7 +116,7 @@ void KinematicCentroidalMPC::AddKinematicsIntegrator() {
 }
 
 void KinematicCentroidalMPC::AddContactConstraints() {
-  for (int knot_point = 1; knot_point < n_knot_points_; knot_point++) {
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
       //Make sure feet in stance are not moving and on the ground
       if (contact_sequence_[knot_point][contact_index]) {
@@ -130,7 +130,7 @@ void KinematicCentroidalMPC::AddContactConstraints() {
         // Feet are above the ground
         double lb = 0;
         if(knot_point > 0 and knot_point + 1 < n_knot_points_ and (!contact_sequence_[knot_point-1][contact_index] or !contact_sequence_[knot_point+1][contact_index])){
-          lb = 0.05;
+          lb = 0.02;
         }
         prog_->AddBoundingBoxConstraint(lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
       }
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
new file mode 100644
index 0000000000..eccb4cca89
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
@@ -0,0 +1,52 @@
+#include "reference_generation_utils.h"
+
+drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
+                                                                       const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+                                                                       const std::vector<double>& time_points){
+  DRAKE_DEMAND(vel_ewrt_w.size() == time_points.size());
+  auto n_points = vel_ewrt_w.size();
+
+  std::vector<drake::MatrixX<double>> samples(n_points);
+  samples[0] = current_com;
+  for(int i = 1; i<n_points; i++){
+    samples[i] = samples[i-1] + (time_points[i] - time_points[i-1]) * vel_ewrt_w[i-1];
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(time_points, samples);
+}
+
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
+                                                                                  const Eigen::Vector3d& base_rt_com_ewrt_w,
+                                                                                  const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int base_pos_start){
+  auto n_points = com_traj.get_segment_times().size();
+  std::vector<drake::MatrixX<double>> samples(n_points);
+  for(int i = 0; i < n_points; i++){
+    samples[i] = nominal_stand;
+    samples[i].block<3,1>(base_pos_start,0, 3, 1) = com_traj.value(com_traj.get_segment_times()[i]) + base_rt_com_ewrt_w;
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(com_traj.get_segment_times(), samples);
+}
+
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int n_v,
+                                                                                  int base_vel_start){
+  auto n_points = com_traj.get_segment_times().size();
+  std::vector<drake::MatrixX<double>> samples(n_points);
+  for(int i = 0; i < n_points; i++){
+    samples[i] = drake::VectorX<double>::Zero(n_v);
+    samples[i].block<3,1>(base_vel_start,0, 3, 1) = com_traj.derivative().value(com_traj.get_segment_times()[i]);
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(com_traj.get_segment_times(), samples);
+}
+
+
+drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
+                                                                      const std::vector<double>& time_points){
+  DRAKE_DEMAND(gait_sequence.size() == time_points.size());
+
+  auto traj = gait_sequence[0].ToTrajectory(time_points[0], time_points[1]);
+  for(int i = 1; i<gait_sequence.size() - 1 ; i++){
+    traj.ConcatenateInTime(gait_sequence[i].ToTrajectory(time_points[i], time_points[i + 1]));
+  }
+  return traj;
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
new file mode 100644
index 0000000000..0138fd9bbe
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
@@ -0,0 +1,50 @@
+#pragma once
+
+#include <drake/multibody/plant/multibody_plant.h>
+#include <drake/common/trajectories/piecewise_polynomial.h>
+#include "multibody/kinematic/world_point_evaluator.h"
+#include "systems/controllers/kinematic_centroidal_mpc/gait.h"
+
+/*!
+ * @brief given an initial com and velocity of com emrt w, calculate a com trajectory
+ * @param current_com
+ * @param vel_ewrt_w velocity of the com ewrt. Given a zoh between time points, must be the same length as time_points
+ * @param time_points time values correspeding to velocities, trajectory start at the first, and end at the last, must be the same length as vel_ewrt_w
+ * @return trajectory of com velocity
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
+                                                                       const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+                                                                       const std::vector<double>& time_points);
+
+/*!
+ * @brief Constructs a trajectory for the generalized positions of a constant joint state and floating base position from com trajectory
+ * @param nominal_stand [nq] generalized state
+ * @param base_rt_com_ewrt_w vector from com to base in world frame
+ * @param com_traj trajectory for the center of mass
+ * @param base_pos_start index where the base position starts in generalized state
+ * @return trajectory of generalized state
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
+                                                                                  const Eigen::Vector3d& base_rt_com_ewrt_w,
+                                                                                  const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int base_pos_start);
+
+/*!
+ * @brief constructs a trajectory for the generalized velocity where the joint velocity is 0 and floating base val from com trajectory
+ * @param com_traj
+ * @param n_v number of velocity states
+ * @param base_vel_start index where base vel starts
+ * @return
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+                                                                                  int n_v,
+                                                                                  int base_vel_start);
+
+/*!
+ * @brief given a vector of gaits and time points corresponding to when the gaits are active
+ * @param gait_sequence vector of gaits
+ * @param time_points vector of time points when each gait is active
+ * @return trajectory of mode status start at time_point(0) and ending at time_point.end()-1
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
+                                                                      const std::vector<double>& time_points);
\ No newline at end of file

From 39f7413baed202726e08a6c6cd158a797620c2bb Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 2 Nov 2022 15:55:29 -0400
Subject: [PATCH 29/76] Switched ik to com based and add minimum swing foot
 height to gains

---
 .../cassie_kcmpc_trajopt.cc                   | 10 +++----
 .../cassie_reference_utils.cc                 | 19 +++++++++-----
 .../cassie_reference_utils.h                  |  4 +--
 .../kinematic_centroidal_mpc_gains.yaml       | 26 ++++++++++---------
 .../kinematic_centroidal_gains.h              |  2 ++
 .../kinematic_centroidal_mpc.cc               |  4 ++-
 .../kinematic_centroidal_mpc.h                |  2 ++
 7 files changed, 41 insertions(+), 26 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index d45a4d4a03..13a36a78af 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -22,7 +22,7 @@ using drake::multibody::MultibodyPlant;
 using drake::multibody::Parser;
 using drake::geometry::DrakeVisualizer;
 
-void DoMain(int n_knot_points, double duration, double base_height, double stance_width, double vel, double tol){
+void DoMain(int n_knot_points, double duration, double com_height, double stance_width, double vel, double tol){
   auto gains = drake::yaml::LoadYamlFile<KinematicCentroidalGains>("examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml");
   // Create fix-spring Cassie MBP
   drake::systems::DiagramBuilder<double> builder;
@@ -54,10 +54,10 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
              {0.4, 0.5, drake::Vector<bool, 4>(true, true, true, true)},
              {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
              {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}});
-  walk.SetPeriod(1);
+  walk.SetPeriod(1.25);
 
   // Create reference
-  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), base_height, stance_width);
+  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), com_height, stance_width, true);
   KcmpcReferenceGenerator generator(plant, reference_state.head(plant.num_positions()), mpc.CreateContactPoints(plant,0));
 
 
@@ -139,12 +139,12 @@ void DoMain(int n_knot_points, double duration, double base_height, double stanc
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(0.5);
+    simulator.set_target_realtime_rate(1.0);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
   }
 }
 
 int main(int argc, char* argv[]) {
-  DoMain(36, 3, 1.0, 0.3, 0.5, 1e-2);
+  DoMain(45, 5, 0.8, 0.3, 0.5, 1e-2);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
index 49ef856c53..2b5215dbee 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
@@ -9,13 +9,14 @@
 #include <drake/solvers/solve.h>
 #include "examples/Cassie/cassie_utils.h"
 #include "multibody/visualization_utils.h"
+#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
 using drake::multibody::Parser;
 
 Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
-                                     double pelvis_height,
+                                     double com_height,
                                      double stance_width,
                                      bool visualize) {
   using Eigen::VectorXd;
@@ -56,7 +57,7 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
 
   Eigen::Vector3d heel_rt_toe = {.122, 0, 0};
 
-  Eigen::Vector3d pelvis_pos = {0,0, pelvis_height};
+  Eigen::Vector3d pelvis_pos = {0, 0, com_height};
   Eigen::Vector3d l_toe_pos = {0.06, stance_width/2, 0};
   Eigen::Vector3d r_toe_pos = {0.06, -stance_width/2, 0};
 
@@ -69,12 +70,10 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
   const auto& toe_left_frame = plant.GetFrameByName("toe_left");
   const auto& toe_right_frame = plant.GetFrameByName("toe_right");
 
-  drake::multibody::InverseKinematics ik(plant);
+  auto context = plant.CreateDefaultContext();
+  drake::multibody::InverseKinematics ik(plant, context.get());
   double eps = 1e-3;
   Vector3d eps_vec = eps * VectorXd::Ones(3);
-  ik.AddPositionConstraint(pelvis_frame, Vector3d(0, 0, 0), world_frame,
-                           pelvis_pos - eps * VectorXd::Ones(3),
-                           pelvis_pos + eps * VectorXd::Ones(3));
   ik.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
                               world_frame, drake::math::RotationMatrix<double>(), eps);
   ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, world_frame,
@@ -103,7 +102,15 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
           (ik.q())(positions_map.at("ankle_joint_right")) ==
           M_PI * 13 / 180.0);
 
+  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(&plant, std::nullopt, plant.world_frame(), context.get());
+  auto r = ik.get_mutable_prog()->NewContinuousVariables(3);
+  ik.get_mutable_prog()->AddConstraint(constraint, {ik.q(),r});
+  Eigen::Vector3d rdes = {0, 0, com_height};
+  ik.get_mutable_prog()->AddBoundingBoxConstraint(rdes, rdes, r);
+
   ik.get_mutable_prog()->SetInitialGuess(ik.q(), q_ik_guess);
+  ik.get_mutable_prog()->SetInitialGuess(r, rdes);
+
   const auto result = drake::solvers::Solve(ik.prog());
   const auto q_sol = result.GetSolution(ik.q());
   VectorXd q_sol_normd(n_q);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
index 2950f65324..d5fe476d4b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
@@ -9,12 +9,12 @@
 /*!
  * @brief Construct a nominal stand for cassie
  * @param plant cassie plant
- * @param pelvis_height
+ * @param com_height
  * @param stance_width
  * @param visualize true if the stand should be visualized
  * @return vector of state [nq + nv]
  */
 Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
-                                     double pelvis_height,
+                                     double com_height,
                                      double stance_width,
                                      bool visualize = false);
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
index c39069269f..af72e8e1c2 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
@@ -1,4 +1,4 @@
-com_position: [1, 1, 1]
+com_position: [1, 1, 10]
 generalized_positions:
   base_qw: 1.0
   base_qx: 1.0
@@ -7,12 +7,12 @@ generalized_positions:
   base_x: 0
   base_y: 0
   base_z: 0
-  hip_roll_: 0.0001
-  hip_yaw_: 0.0001
-  hip_pitch_: 0.0001
-  knee_: 0.0001
-  ankle_joint_: 0.0001
-  toe_: 0.0001
+  hip_roll_: 0.01
+  hip_yaw_: 0.01
+  hip_pitch_: 0.001
+  knee_: 0.001
+  ankle_joint_: 0.001
+  toe_: 0.0000
 generalized_velocities:
   base_wx: 0.5
   base_wy: 0.5
@@ -20,15 +20,17 @@ generalized_velocities:
   base_vx: 0.1
   base_vy: 0.1
   base_vz: 0.1
-  hip_roll_: 0.002
-  hip_yaw_: 0.002
+  hip_roll_: 0.02
+  hip_yaw_: 0.02
   hip_pitch_: 0.002
   knee_: 0.002
   ankle_joint_: 0.002
-  toe_: 0.002
+  toe_: 0.004
 
-contact_pos: [0.1, 8, 0.001]
+contact_pos: [0.1, 16, 0.001]
 contact_vel: [0.02, 0.02, 0.02]
 contact_force: [0.0001, 0.0001, 0.0001]
 lin_momentum: [0.00001, 0.00001, 0.00001]
-ang_momentum: [0.00001, 0.00001, 0.00001]
+ang_momentum: [0.0000001, 0.0000001, 0.0000001]
+
+swing_foot_minimum_height: 0.015
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
index 46901e5fa4..36e5dcba5c 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
@@ -13,6 +13,7 @@ struct KinematicCentroidalGains{
   Eigen::Vector3d lin_momentum;
   Eigen::Vector3d ang_momentum;
 
+  double swing_foot_minimum_height;
   template <typename Archive>
   void Serialize(Archive* a) {
     a->Visit(DRAKE_NVP(com_position));
@@ -23,5 +24,6 @@ struct KinematicCentroidalGains{
     a->Visit(DRAKE_NVP(contact_force));
     a->Visit(DRAKE_NVP(lin_momentum));
     a->Visit(DRAKE_NVP(ang_momentum));
+    a->Visit(DRAKE_NVP(swing_foot_minimum_height));
   }
 };
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 7d63f14cc8..a1dafab53b 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -130,7 +130,7 @@ void KinematicCentroidalMPC::AddContactConstraints() {
         // Feet are above the ground
         double lb = 0;
         if(knot_point > 0 and knot_point + 1 < n_knot_points_ and (!contact_sequence_[knot_point-1][contact_index] or !contact_sequence_[knot_point+1][contact_index])){
-          lb = 0.02;
+          lb = swing_foot_minimum_height_;
         }
         prog_->AddBoundingBoxConstraint(lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
       }
@@ -540,4 +540,6 @@ void KinematicCentroidalMPC::SetGains(const KinematicCentroidalGains &gains) {
 
   //contact force
   Q_force_ = gains.contact_force.replicate(n_contact_points_, 1).asDiagonal();
+
+  swing_foot_minimum_height_ = gains.swing_foot_minimum_height;
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index a33aaf1140..9e6d687a56 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -356,5 +356,7 @@ class KinematicCentroidalMPC {
   std::unique_ptr<dairlib::multibody::MultiposeVisualizer> callback_visualizer_;
 
   const std::set<int> full_constraint_relative_ = {0, 1, 2};
+
+  double swing_foot_minimum_height_;
 };
 

From e82efcbce4f3db95fe66e673f113cbc03d5b54a0 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 4 Nov 2022 11:31:11 -0400
Subject: [PATCH 30/76] Cleaning up code for PR

---
 examples/Cassie/cassie_utils.cc               | 30 +++++--------------
 .../cassie_kcmpc_trajopt.cc                   |  2 +-
 .../cassie_kinematic_centroidal_mpc.h         |  2 +-
 .../kinematic_centroidal_mpc/gait.cc          |  6 ++--
 .../kinematic_centroidal_mpc/gait.h           |  4 +--
 .../kcmpc_reference_generator.cc              |  2 +-
 6 files changed, 15 insertions(+), 31 deletions(-)

diff --git a/examples/Cassie/cassie_utils.cc b/examples/Cassie/cassie_utils.cc
index 7fd6f66728..d9d1ac0548 100644
--- a/examples/Cassie/cassie_utils.cc
+++ b/examples/Cassie/cassie_utils.cc
@@ -231,31 +231,15 @@ const systems::GearedMotor& AddMotorModel(
 }
 
 std::vector<std::string> JointNames(){
-  // create joint/motor names
-  std::vector<std::pair<std::string, std::string>> l_r_pairs{
-      std::pair<std::string, std::string>("_left", "_right"),
-      std::pair<std::string, std::string>("_right", "_left"),
-  };
-  std::vector<std::string> asy_joint_names{
-      "hip_roll",
-      "hip_yaw",
-  };
-  std::vector<std::string> sym_joint_names{"hip_pitch", "knee", "ankle_joint", "toe"};
-  std::vector<std::string> joint_names{};
-  std::vector<std::string> motor_names{};
-  for (auto &l_r_pair : l_r_pairs) {
-    for (auto & asy_joint_name : asy_joint_names) {
-      joint_names.push_back(asy_joint_name + l_r_pair.first);
-      motor_names.push_back(asy_joint_name + l_r_pair.first + "_motor");
-    }
-    for (unsigned int i = 0; i < sym_joint_names.size(); i++) {
-      joint_names.push_back(sym_joint_names[i] + l_r_pair.first);
-      if (sym_joint_names[i].compare("ankle_joint") != 0) {
-        motor_names.push_back(sym_joint_names[i] + l_r_pair.first + "_motor");
-      }
+  std::vector<std::string> side_names{"_left", "_right"};
+  std::vector<std::string> joint_names{"hip_pitch", "knee", "ankle_joint", "toe", "hip_roll", "hip_yaw"};
+  std::vector<std::string> full_joint_names;
+  for(const auto& name : joint_names){
+    for(const auto& side:side_names){
+      full_joint_names.push_back(name+side);
     }
   }
-  return  joint_names;
+  return  full_joint_names;
 }
 
 template std::pair<const Vector3d, const Frame<double>&> LeftToeFront(
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 13a36a78af..1a11f09180 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -57,7 +57,7 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   walk.SetPeriod(1.25);
 
   // Create reference
-  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), com_height, stance_width, true);
+  Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), com_height, stance_width, false);
   KcmpcReferenceGenerator generator(plant, reference_state.head(plant.num_positions()), mpc.CreateContactPoints(plant,0));
 
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index cfcc96540c..20571c808b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -1,7 +1,7 @@
 #pragma once
 #include <iostream>
 #include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h"
-#include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/multibody/plant/multibody_plant.h"
+#include "drake/multibody/plant/multibody_plant.h"
 #include "examples/Cassie/cassie_utils.h"
 
 /*!
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.cc b/systems/controllers/kinematic_centroidal_mpc/gait.cc
index f351e35cf9..1c61342df7 100644
--- a/systems/controllers/kinematic_centroidal_mpc/gait.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/gait.cc
@@ -1,6 +1,6 @@
 #include "gait.h"
 
-int Gait::CurrentMode(double time_now) const{
+int Gait::GetCurrentMode(double time_now) const{
   double phase_now = fmod(time_now/period_, 1);
   for(int i = 0; i < gait_pattern_.size(); i++){
     const auto& mode = gait_pattern_[i];
@@ -17,7 +17,7 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double curre
   std::vector<drake::MatrixX<double>> samples;
 
   // Calculate initial mode index, and phase
-  int current_mode = CurrentMode( current_time);
+  int current_mode = GetCurrentMode(current_time);
   double current_phase = fmod(current_time/period_, 1);
 
   // Loop until time is greater than end time
@@ -38,7 +38,7 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double curre
   return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(break_points, samples);
 }
 
-void Gait::Is_Valid() const {
+void Gait::check_valid() const {
   DRAKE_ASSERT(period_ > 0);
   DRAKE_ASSERT(gait_pattern_[0].start_phase == 0);
   DRAKE_ASSERT(gait_pattern_[gait_pattern_.size()-1].end_phase == 1);
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.h b/systems/controllers/kinematic_centroidal_mpc/gait.h
index d2573a55b5..d4a38e5bc7 100644
--- a/systems/controllers/kinematic_centroidal_mpc/gait.h
+++ b/systems/controllers/kinematic_centroidal_mpc/gait.h
@@ -30,7 +30,7 @@ class Gait{
   /*!
    * @brief Checks to make sure the gait is valid, Asserts if not valid
    */
-  void Is_Valid() const;
+  void check_valid() const;
 
   void SetPeriod(double period){period_ = period;}
 
@@ -40,7 +40,7 @@ class Gait{
    * @param time_now
    * @return
    */
-  int CurrentMode(double time_now) const;
+  int GetCurrentMode(double time_now) const;
 
   double period_;
   std::vector<Mode> gait_pattern_; /// Vector of modes. Start phase of the first mode must be 0, end phase of the last mode
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
index c1f670ebf1..a870c39d83 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
@@ -99,7 +99,7 @@ void KcmpcReferenceGenerator::SetComKnotPoints(const KnotPoints<Eigen::Vector3d>
 
 void KcmpcReferenceGenerator::SetGaitSequence(const KnotPoints<Gait> &gait_knot_points) {
   for(const auto& gait : gait_knot_points.samples){
-    gait.Is_Valid();
+    gait.check_valid();
   }
   gait_knot_points_ = gait_knot_points;
 }

From dbee5cb12c14c12f9281c4cc3db3cb6043baff7f Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 4 Nov 2022 12:02:25 -0400
Subject: [PATCH 31/76] Switched to yaml for gaits

---
 .../kinematic_centroidal_mpc/BUILD.bazel      |  4 ++-
 .../cassie_kcmpc_trajopt.cc                   | 10 ++-----
 .../gaits/BUILD.bazel                         |  5 ++++
 .../kinematic_centroidal_mpc/gaits/stand.yaml |  5 ++++
 .../kinematic_centroidal_mpc/gaits/walk.yaml  | 14 +++++++++
 .../kinematic_centroidal_mpc/gait.cc          | 26 ++++++++---------
 .../kinematic_centroidal_mpc/gait.h           | 29 ++++++++++++-------
 .../kinematic_centroidal_gains.h              |  1 -
 8 files changed, 60 insertions(+), 34 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml

diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
index a789723f80..b8f2675772 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
@@ -14,7 +14,9 @@ cc_library(
 cc_binary(
     name = "cassie_kcmpc_trajopt",
     srcs = ["cassie_kcmpc_trajopt.cc"],
-    data = ["//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_mpc_gains.yaml"],
+    data = ["//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_mpc_gains.yaml",
+            "//examples/Cassie/kinematic_centroidal_mpc/gaits:stand.yaml",
+            "//examples/Cassie/kinematic_centroidal_mpc/gaits:walk.yaml"],
     deps = [
         "//examples/Cassie:cassie_utils",
         "//common",
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 1a11f09180..98bfa84a1c 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -47,14 +47,8 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
   mpc.SetGains(gains);
 
   // Create gaits
-  Gait stand ({{0, 1, drake::Vector<bool, 4>(true, true, true, true)}});
-  stand.SetPeriod(1);
-
-  Gait walk({{0, 0.4, drake::Vector<bool, 4>(true, true, false, false)},
-             {0.4, 0.5, drake::Vector<bool, 4>(true, true, true, true)},
-             {0.5, 0.9, drake::Vector<bool, 4>(false, false, true, true)},
-             {0.9, 1.0, drake::Vector<bool, 4>(true, true, true, true)}});
-  walk.SetPeriod(1.25);
+  auto stand = drake::yaml::LoadYamlFile<Gait>("examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml");
+  auto walk = drake::yaml::LoadYamlFile<Gait>("examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml");
 
   // Create reference
   Eigen::VectorXd reference_state = GenerateNominalStand(mpc.Plant(), com_height, stance_width, false);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel
new file mode 100644
index 0000000000..1aea1be4e7
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel
@@ -0,0 +1,5 @@
+package(default_visibility = ["//visibility:public"])
+
+exports_files(["stand.yaml"])
+
+exports_files(["walk.yaml"])
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml b/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml
new file mode 100644
index 0000000000..cc525ff774
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml
@@ -0,0 +1,5 @@
+gait_pattern:
+  - start_phase: 0.0
+    end_phase: 1.0
+    contact_status: [true, true, true, true]
+period: 1.0
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml b/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml
new file mode 100644
index 0000000000..b1f566edd9
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml
@@ -0,0 +1,14 @@
+gait_pattern:
+  - start_phase: 0.0
+    end_phase: 0.4
+    contact_status: [true, true, false, false]
+  - start_phase: 0.4
+    end_phase: 0.5
+    contact_status: [true, true, true, true]
+  - start_phase: 0.5
+    end_phase: 0.9
+    contact_status: [false, false, true, true]
+  - start_phase: 0.9
+    end_phase: 1.0
+    contact_status: [true, true, true, true]
+period: 1.25
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.cc b/systems/controllers/kinematic_centroidal_mpc/gait.cc
index 1c61342df7..da119592b3 100644
--- a/systems/controllers/kinematic_centroidal_mpc/gait.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/gait.cc
@@ -1,9 +1,9 @@
 #include "gait.h"
 
 int Gait::GetCurrentMode(double time_now) const{
-  double phase_now = fmod(time_now/period_, 1);
-  for(int i = 0; i < gait_pattern_.size(); i++){
-    const auto& mode = gait_pattern_[i];
+  double phase_now = fmod(time_now/period, 1);
+  for(int i = 0; i < gait_pattern.size(); i++){
+    const auto& mode = gait_pattern[i];
     if(mode.start_phase <= phase_now && mode.end_phase > phase_now){
       return i;
     }
@@ -18,20 +18,20 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double curre
 
   // Calculate initial mode index, and phase
   int current_mode = GetCurrentMode(current_time);
-  double current_phase = fmod(current_time/period_, 1);
+  double current_phase = fmod(current_time/period, 1);
 
   // Loop until time is greater than end time
   while(current_time < end_time){
     // Add the break point for the current time
     break_points.push_back(current_time);
-    samples.emplace_back(gait_pattern_[current_mode].contact_status.cast<double>());
+    samples.emplace_back(gait_pattern[current_mode].contact_status.cast<double>());
 
     // Update time based on how much longer the current mode will last
-    current_time += (gait_pattern_[current_mode].end_phase - current_phase) * period_;
+    current_time += (gait_pattern[current_mode].end_phase - current_phase) * period;
     // Update the mode and mod if necessary
-    current_mode = (current_mode + 1) % gait_pattern_.size();
+    current_mode = (current_mode + 1) % gait_pattern.size();
     // The new phase is the start phase of the updated mode
-    current_phase = gait_pattern_[current_mode].start_phase;
+    current_phase = gait_pattern[current_mode].start_phase;
   }
   break_points.push_back(end_time);
   samples.push_back(samples[samples.size()-1]);
@@ -39,10 +39,10 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double curre
 }
 
 void Gait::check_valid() const {
-  DRAKE_ASSERT(period_ > 0);
-  DRAKE_ASSERT(gait_pattern_[0].start_phase == 0);
-  DRAKE_ASSERT(gait_pattern_[gait_pattern_.size()-1].end_phase == 1);
-  for(int i = 0; i < gait_pattern_.size() - 1; i++){
-    DRAKE_ASSERT(gait_pattern_[i].end_phase == gait_pattern_[i+1].start_phase);
+  DRAKE_ASSERT(period > 0);
+  DRAKE_ASSERT(gait_pattern[0].start_phase == 0);
+  DRAKE_ASSERT(gait_pattern[gait_pattern.size()-1].end_phase == 1);
+  for(int i = 0; i < gait_pattern.size() - 1; i++){
+    DRAKE_ASSERT(gait_pattern[i].end_phase == gait_pattern[i+1].start_phase);
   }
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.h b/systems/controllers/kinematic_centroidal_mpc/gait.h
index d4a38e5bc7..a0dda58cea 100644
--- a/systems/controllers/kinematic_centroidal_mpc/gait.h
+++ b/systems/controllers/kinematic_centroidal_mpc/gait.h
@@ -1,6 +1,8 @@
 #pragma once
 
 #include <drake/common/trajectories/piecewise_polynomial.h>
+#include "drake/common/yaml/yaml_read_archive.h"
+#include "yaml-cpp/yaml.h"
 
 /*!
  * @brief Struct for a given mode as part of a mode sequence
@@ -9,16 +11,19 @@ struct Mode{
   double start_phase;
   double end_phase;
   drake::VectorX<bool> contact_status; /// Vector describing which contacts are active
+
+  template <typename Archive>
+  void Serialize(Archive* a) {
+    a->Visit(DRAKE_NVP(start_phase));
+    a->Visit(DRAKE_NVP(end_phase));
+    a->Visit(DRAKE_NVP(contact_status));
+  }
 };
 
 /*!
- * @brief Clss for defining a gait consisting of a vector of Modes, and a period for the whole gait.
+ * @brief Struct for defining a gait consisting of a vector of Modes, and a period for the whole gait.
  */
-class Gait{
- public:
-  Gait(const std::vector<Mode>& gait_pattern, double period = 0): gait_pattern_(gait_pattern),
-  period_(period){};
-
+struct Gait{
 /*!
  * @brief converts the gait into a trajectory from current time to end time
  * @param current_time
@@ -32,9 +37,11 @@ class Gait{
    */
   void check_valid() const;
 
-  void SetPeriod(double period){period_ = period;}
-
- private:
+  template <typename Archive>
+  void Serialize(Archive* a) {
+    a->Visit(DRAKE_NVP(gait_pattern));
+    a->Visit(DRAKE_NVP(period));
+  }
   /*!
    * @brief find the index for the current mode that the gait is in
    * @param time_now
@@ -42,8 +49,8 @@ class Gait{
    */
   int GetCurrentMode(double time_now) const;
 
-  double period_;
-  std::vector<Mode> gait_pattern_; /// Vector of modes. Start phase of the first mode must be 0, end phase of the last mode
+  double period;
+  std::vector<Mode> gait_pattern; /// Vector of modes. Start phase of the first mode must be 0, end phase of the last mode
   /// must be 1, and no time gaps between start and end of sequential modes
 
 };
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
index 36e5dcba5c..97ba599ae3 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
@@ -1,7 +1,6 @@
 #pragma once
 #include "drake/common/yaml/yaml_read_archive.h"
 #include "yaml-cpp/yaml.h"
-#include "include/_usr_include_eigen3/Eigen/Core"
 
 struct KinematicCentroidalGains{
   Eigen::Vector3d com_position;

From e7f0843787c813aca34fb561c1ed0858056497e8 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 4 Nov 2022 12:13:11 -0400
Subject: [PATCH 32/76] Switched to reletive path to file and try catch

---
 .../kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc       | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 98bfa84a1c..dbef16465d 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -112,8 +112,11 @@ void DoMain(int n_knot_points, double duration, double com_height, double stance
 
   std::cout << "Solving optimization\n\n";
   const auto pp_xtraj = mpc.Solve();
-  mpc.SaveSolutionToFile(std::string(getenv("HOME")) + "/workspace/dairlib/examples/Cassie/saved_trajectories/kcmpc_solution");
-
+  try{
+    mpc.SaveSolutionToFile("examples/Cassie/saved_trajectories/kcmpc_solution");
+  } catch(...){
+    std::cout<<"Unable to save trajectory, try running binary manually rather than using bazel run" << std::endl;
+  }
   auto traj_source =
       builder.AddSystem<drake::systems::TrajectorySource>(pp_xtraj);
   auto passthrough = builder.AddSystem<dairlib::systems::SubvectorPassThrough>(

From a3380c72ec1bb9ebe44ae69c0641ae822afb1d02 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Sun, 6 Nov 2022 23:06:27 -0500
Subject: [PATCH 33/76] Responded to more PR feedback

---
 .../kcmpc_reference_generator.cc              | 58 -------------------
 .../kinematic_centroidal_constraints.cc       | 40 ++++++-------
 .../kinematic_centroidal_mpc.cc               |  8 ++-
 .../kinematic_centroidal_mpc.h                |  4 ++
 .../reference_generation_utils.cc             | 43 ++++++++++++++
 .../reference_generation_utils.h              | 42 ++++++++++++--
 6 files changed, 108 insertions(+), 87 deletions(-)

diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
index a870c39d83..2ebe8a3815 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
@@ -2,64 +2,6 @@
 #include "multibody/multibody_utils.h"
 #include "systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h"
 
-double TimeNextValue(const drake::trajectories::PiecewisePolynomial<double>& trajectory,
-                     double current_time,
-                     int index,
-                     double value){
-  if (trajectory.value(current_time).coeff(index) == value)
-    return current_time;
-
-  const auto& segment_times = trajectory.get_segment_times();
-
-  for(int i = trajectory.get_segment_index(current_time) + 1; i<segment_times.size(); i++){
-    if(trajectory.value(segment_times[i]).coeff(i) == value)
-      return segment_times[i];
-  }
-  return trajectory.end_time();
-}
-
-drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
-                                                                      double m){
-  std::vector<drake::MatrixX<double>> samples;
-  const int n_contact_points = mode_trajectory.rows();
-
-  for(const auto& time: mode_trajectory.get_segment_times()){
-    const auto& mode = mode_trajectory.value(time);
-    double num_in_contact = mode.sum();
-    auto& grf = samples.emplace_back(Eigen::VectorXd::Zero(3 * n_contact_points));
-    for(int i = 0; i<n_contact_points; i++){
-      if(mode.coeff(i)){
-        grf.coeffRef(2 + 3 * i) = m * 9.81 /num_in_contact;
-      }
-    }
-  }
-  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(mode_trajectory.get_segment_times(), samples);
-}
-
-drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(const drake::multibody::MultibodyPlant<double> &plant,
-                                                                               const std::vector<dairlib::multibody::WorldPointEvaluator<
-                                                                                   double>> &contacts,
-                                                                               const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &q_traj,
-                                                                               const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &v_traj){
-  auto context =  plant.CreateDefaultContext();
-  std::vector<double> break_points = q_traj.get_segment_times();
-  std::vector<drake::MatrixX<double>> samples;
-  int n_contact = contacts.size();
-  for(const auto& time : break_points){
-    dairlib::multibody::SetPositionsIfNew<double>(plant, q_traj.value(time), context.get());
-    dairlib::multibody::SetVelocitiesIfNew<double>(plant, v_traj.value(time), context.get());
-    auto& knot_point_value = samples.emplace_back(Eigen::VectorXd::Zero(6 * n_contact));
-    for(int i = 0; i <n_contact; i++){
-      knot_point_value.block(i * 3, 0,  3, 1) = contacts[i].EvalFull(*context);
-      knot_point_value.coeffRef(i*3 + 2) = 0;
-      knot_point_value.block(3 * n_contact + i * 3, 0, 3, 1) = contacts[i].EvalFullJacobianDotTimesV(*context);
-    }
-  }
-  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(break_points, samples);
-}
-
 KcmpcReferenceGenerator::KcmpcReferenceGenerator(const drake::multibody::MultibodyPlant<double> &plant,
                                                  const Eigen::VectorXd& nominal_stand,
                                                  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &contacts):
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
index 303f647327..fd0d6651b7 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
@@ -9,7 +9,7 @@ CentroidalDynamicsConstraint<T>::CentroidalDynamicsConstraint(const drake::multi
                                                               int n_contact,
                                                               double dt,
                                                               int knot_index): dairlib::solvers::NonlinearConstraint<T>(
-    6,  2 * 6 + 2 * (3 + 2 * 3 * n_contact),
+    6,  2 * (6  + 3 + 2 * 3 * n_contact), // Number of inputs = 2 * (momentum + com + contact_pos + contact_vel)
     Eigen::VectorXd::Zero(6),
     Eigen::VectorXd::Zero(6),
     "momentum_collocation[" +
@@ -22,34 +22,34 @@ CentroidalDynamicsConstraint<T>::CentroidalDynamicsConstraint(const drake::multi
 
 
 /// The format of the input to the eval() function is in the order
-///   - xMom0, momentum state at time k
-///   - xMom1, momentum state at time k+1
+///   - momentum0, momentum state at time k
+///   - momentum1, momentum state at time k+1
 ///   - com0, location of com at time k
-///   - cj0, contact locations time k
-///   - Fj0, contact forces at time k
+///   - x_contact0, contact locations time k
+///   - f0, contact forces at time k
 ///   - com1, location of com at time k+1
-///   - cj1, contact locations time k+1
-///   - Fj1, contact forces at time k+1
+///   - x_contact1, contact locations time k+1
+///   - f1, contact forces at time k+1
 
 template <typename T>
 void CentroidalDynamicsConstraint<T>::EvaluateConstraint(
     const Eigen::Ref<const drake::VectorX<T>>& x, drake::VectorX<T>* y) const {
   // Extract decision variables
-  const auto& xMom0 = x.segment(0, n_mom_);
-  const auto& xMom1 = x.segment(n_mom_, n_mom_);
+  const auto& momentum0 = x.segment(0, n_mom_);
+  const auto& momentum1 = x.segment(n_mom_, n_mom_);
   const auto& com0 = x.segment(2 * n_mom_, 3);
-  const auto& cj0 = x.segment(2 * n_mom_ + 3, 3 * n_contact_);
-  const auto& Fj0 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_, 3 * n_contact_);
+  const auto& x_contact0 = x.segment(2 * n_mom_ + 3, 3 * n_contact_);
+  const auto& f0 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_, 3 * n_contact_);
   const auto& com1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_, 3);
-  const auto& cj1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_+ 3, 3 * n_contact_);
-  const auto& Fj1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_+ 3 + 3 * n_contact_, 3 * n_contact_);
+  const auto& x_contact1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_+ 3, 3 * n_contact_);
+  const auto& f1 = x.segment(2 * n_mom_ + 3 + 3 * n_contact_ + 3 * n_contact_+ 3 + 3 * n_contact_, 3 * n_contact_);
 
-  drake::Vector<T, 6> xdot0Mom = CalcTimeDerivativesWithForce(com0, cj0, Fj0);
-  drake::Vector<T, 6> xdot1Mom = CalcTimeDerivativesWithForce(com1, cj1, Fj1);
+  drake::Vector<T, 6> xdot0Mom = CalcTimeDerivativesWithForce(com0, x_contact0, f0);
+  drake::Vector<T, 6> xdot1Mom = CalcTimeDerivativesWithForce(com1, x_contact1, f1);
 
   // Predict state and return error
-  const auto x1Predict = xMom0 + 0.5 * dt_ * (xdot0Mom + xdot1Mom);
-  *y = xMom1 - x1Predict;
+  const auto x1Predict = momentum0 + 0.5 * dt_ * (xdot0Mom + xdot1Mom);
+  *y = momentum1 - x1Predict;
 }
 
 template<typename T>
@@ -155,16 +155,16 @@ CentroidalMomentumConstraint<T>::CentroidalMomentumConstraint(const drake::multi
 /// The format of the input to the eval() function is in the order
 ///   - q, generalized positions
 ///   - v, generalized velocities
-///   - r, location of the com
+///   - com, location of the com
 ///   - h_WC, angular momentum, linear momentum in the wf about the com
 template<typename T>
 void CentroidalMomentumConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
                                                            drake::VectorX<T> *y) const {
   const auto& x0 = x.head(n_x_);
-  const auto& r = x.segment(n_x_, 3);
+  const auto& com = x.segment(n_x_, 3);
   const auto& h_WC = x.segment(n_x_ + 3, 6);
   dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, x0,  context_);
-  const auto& spatial_momentum = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, r);
+  const auto& spatial_momentum = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com);
   *y = spatial_momentum.get_coeffs() - h_WC;
 }
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index a1dafab53b..2a1c4c6e2d 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -129,7 +129,9 @@ void KinematicCentroidalMPC::AddContactConstraints() {
       } else {
         // Feet are above the ground
         double lb = 0;
-        if(knot_point > 0 and knot_point + 1 < n_knot_points_ and (!contact_sequence_[knot_point-1][contact_index] or !contact_sequence_[knot_point+1][contact_index])){
+        // Check if at least one of the time points before or after is also in 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 (!contact_sequence_[knot_point-1][contact_index] or !contact_sequence_[knot_point+1][contact_index])){
           lb = swing_foot_minimum_height_;
         }
         prog_->AddBoundingBoxConstraint(lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
@@ -216,8 +218,8 @@ void KinematicCentroidalMPC::AddConstantMomentumReference(const drake::VectorX<d
 
 void KinematicCentroidalMPC::AddCosts() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    const double terminal_gain = knot_point == n_knot_points_-1 ? 100 : 1;
-    const double collocation_gain = (knot_point == 0 or knot_point == n_knot_points_ -1) ? 0.5 : 1;
+    const double terminal_gain = is_last_knot(knot_point) ? 100 : 1;
+    const double collocation_gain = (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
     double t = dt_ * knot_point;
     if(q_ref_traj_){
       prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_q_, q_ref_traj_->value(t), state_vars(knot_point).head(n_q_));
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 9e6d687a56..4a42dbe591 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -197,6 +197,7 @@ class KinematicCentroidalMPC {
 
   void SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double>& state_trajectory);
 
+  //TODO remove once drake has trajectory stacking
   void SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double>& q_traj, const drake::trajectories::PiecewisePolynomial<double>& v_traj);
 
   void SetComPositionGuess(const drake::Vector3<double>& state);
@@ -299,6 +300,9 @@ class KinematicCentroidalMPC {
    */
   void AddCosts();
 
+  bool is_first_knot(int knot_point_index){return knot_point_index == 0;};
+  bool is_last_knot(int knot_point_index){return knot_point_index == n_knot_points_ - 1;};
+
   const drake::multibody::MultibodyPlant<double>& plant_;
 
   int n_knot_points_;
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
index eccb4cca89..1201d1dc36 100644
--- a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
@@ -1,4 +1,5 @@
 #include "reference_generation_utils.h"
+#include "multibody/multibody_utils.h"
 
 drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
                                                                        const std::vector<Eigen::Vector3d>& vel_ewrt_w,
@@ -50,3 +51,45 @@ drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std:
   }
   return traj;
 }
+
+drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
+                                                                      double m){
+  std::vector<drake::MatrixX<double>> samples;
+  const int n_contact_points = mode_trajectory.rows();
+
+  for(const auto& time: mode_trajectory.get_segment_times()){
+    const auto& mode = mode_trajectory.value(time);
+    double num_in_contact = mode.sum();
+    auto& grf = samples.emplace_back(Eigen::VectorXd::Zero(3 * n_contact_points));
+    for(int i = 0; i<n_contact_points; i++){
+      if(mode.coeff(i)){
+        grf.coeffRef(2 + 3 * i) = m * 9.81 /num_in_contact;
+      }
+    }
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(mode_trajectory.get_segment_times(), samples);
+}
+
+drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(const drake::multibody::MultibodyPlant<double> &plant,
+                                                                               const std::vector<dairlib::multibody::WorldPointEvaluator<
+                                                                                   double>> &contacts,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &q_traj,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &v_traj){
+  auto context =  plant.CreateDefaultContext();
+  std::vector<double> break_points = q_traj.get_segment_times();
+  std::vector<drake::MatrixX<double>> samples;
+  int n_contact = contacts.size();
+  for(const auto& time : break_points){
+    dairlib::multibody::SetPositionsIfNew<double>(plant, q_traj.value(time), context.get());
+    dairlib::multibody::SetVelocitiesIfNew<double>(plant, v_traj.value(time), context.get());
+    auto& knot_point_value = samples.emplace_back(Eigen::VectorXd::Zero(6 * n_contact));
+    for(int i = 0; i <n_contact; i++){
+      knot_point_value.block(i * 3, 0,  3, 1) = contacts[i].EvalFull(*context);
+      knot_point_value.coeffRef(i*3 + 2) = 0;
+      knot_point_value.block(3 * n_contact + i * 3, 0, 3, 1) = contacts[i].EvalFullTimeDerivative(*context);
+    }
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(break_points, samples);
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
index 0138fd9bbe..83acd5589c 100644
--- a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
+++ b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
@@ -19,10 +19,10 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eig
 /*!
  * @brief Constructs a trajectory for the generalized positions of a constant joint state and floating base position from com trajectory
  * @param nominal_stand [nq] generalized state
- * @param base_rt_com_ewrt_w vector from com to base in world frame
+ * @param base_rt_com_ewrt_w vector from com to floating base in world frame
  * @param com_traj trajectory for the center of mass
- * @param base_pos_start index where the base position starts in generalized state
- * @return trajectory of generalized state
+ * @param base_pos_start index where the floating base position starts in generalized state
+ * @return trajectory of generalized position
  */
 drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
                                                                                   const Eigen::Vector3d& base_rt_com_ewrt_w,
@@ -34,17 +34,47 @@ drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajector
  * @param com_traj
  * @param n_v number of velocity states
  * @param base_vel_start index where base vel starts
- * @return
+ * @return trajectory of generalized velocity
  */
 drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
                                                                                   int n_v,
                                                                                   int base_vel_start);
 
 /*!
- * @brief given a vector of gaits and time points corresponding to when the gaits are active
+ * @brief given a vector of gaits and time points corresponding to when the gaits are active generate a mode sequence.
+ *      The transition between gaits can be awkward depending on where the gaits are in phase space during the transition
+ *      {TODO SRL} make transition clean by attempting to shift phase so mode sequence lines up
  * @param gait_sequence vector of gaits
  * @param time_points vector of time points when each gait is active
  * @return trajectory of mode status start at time_point(0) and ending at time_point.end()-1
  */
 drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
-                                                                      const std::vector<double>& time_points);
\ No newline at end of file
+                                                                      const std::vector<double>& time_points);
+
+/*!
+ * @brief given a trajectory which describes the mode, and the mass of the robot calculate a nominal grf trajectory where the weight of the robot
+ *          is distributed over the active contact points
+ * @param mode_trajectory
+ * @param m
+ * @return trajectory of grf for reference
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
+                                                                      double m);
+
+/*!
+ * @brief Calculate trajectory of world point evaluators from generalized state trajectory. This assumes first order hold
+ * between knot points in state trajectories.
+ * TODO If we start using more complicated state references with this function sample time more coarsely
+ * @param plant
+ * @param contacts vector of world point evaluators
+ * @param q_traj generalized position trajectory
+ * @param v_traj generalized velocity trajectory
+ * @return trajectory of contact points stacked [contact1_pos, contact1_vel, ... contact_n_pos, contact_n_vel]
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(const drake::multibody::MultibodyPlant<double> &plant,
+                                                                               const std::vector<dairlib::multibody::WorldPointEvaluator<
+                                                                                   double>> &contacts,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &q_traj,
+                                                                               const drake::trajectories::PiecewisePolynomial<
+                                                                                   double> &v_traj);
\ No newline at end of file

From d44119500642b8bb28059e18e8b4cf773255e648 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 9 Nov 2022 10:46:14 -0500
Subject: [PATCH 34/76] Started owrk on defining a lifting and reduction
 function

---
 .../simple_models/BUILD.bazel                 |  14 ++
 .../simple_models/planar_slip.cc              | 223 ++++++++++++++++++
 .../simple_models/planar_slip.h               | 100 ++++++++
 multibody/kinematic/world_point_evaluator.h   |   2 +
 .../kinematic_centroidal_constraints.cc       |   2 +-
 5 files changed, 340 insertions(+), 1 deletion(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
new file mode 100644
index 0000000000..664c5484d4
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
@@ -0,0 +1,14 @@
+package(default_visibility = ["//visibility:public"])
+
+
+cc_library(
+    name = "planar_slip",
+    srcs = ["planar_slip.cc",],
+    hdrs = ["planar_slip.h",],
+    deps = [
+        "//solvers:constraints",
+        "//multibody/kinematic:kinematic",
+        "//multibody/kinematic:constraints",
+        "@drake//:drake_shared_library",
+    ],
+)
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
new file mode 100644
index 0000000000..879095d7b5
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
@@ -0,0 +1,223 @@
+#include <drake/solvers/solve.h>
+#include "planar_slip.h"
+#include "multibody/multibody_utils.h"
+#include "multibody/kinematic/kinematic_constraints.h"
+
+template<typename T>
+PlanarSlipReductionConstraint<T>::PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<T>& plant,
+                                                                drake::systems::Context<T> *context,
+                                                                const std::vector<dairlib::multibody::WorldPointEvaluator<
+                                                                    T>> &slip_feet,
+                                                                int complex_state_size,
+                                                                T m,
+                                                                int knot_index):dairlib::solvers::NonlinearConstraint<T>(
+                                                                    3+3+2*2*slip_feet.size(), 3+3+2*2*slip_feet.size() + complex_state_size,
+                                                                    Eigen::VectorXd::Zero(3+3+2*2*slip_feet.size()),
+                                                                    Eigen::VectorXd::Zero(3+3+2*2*slip_feet.size()),
+                                                                    "PlanarSlipReductionConstraint[" +
+                                                                        std::to_string(knot_index) + "]"),
+                                                                        context_(context),
+                                                                        plant_(plant),
+                                                                        m_(m),
+                                                                        nx_(plant.num_positions()+plant.num_velocities()),
+                                                                        slip_feet_(slip_feet)
+                                                                    {}
+
+
+/// Input is of the form:
+///     slip_com
+///     slip_velocity
+///     slip_contact_pos
+///     slip_contact_vel
+///     complex_com
+///     complex_ang_momentum
+///     complex_lin_momentum
+///     complex_contact_pos
+///     complex_contact_vel
+///     complex_contact_force
+///     complex_gen_pos
+///     complex_gen_vel
+template<typename T>
+void PlanarSlipReductionConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
+                                                          drake::VectorX<T> *y) const {
+  const auto& slip_com = x.head(kSLIP_DIM);
+  const auto& slip_vel = x.segment(kSLIP_DIM, kSLIP_DIM);
+  const auto& slip_contact_pos = x.segment(kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_feet_.size());
+  const auto& slip_contact_vel = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size(), kSLIP_DIM * slip_feet_.size());
+  const auto& complex_com = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size() + kSLIP_DIM * slip_feet_.size(), 3);
+  const auto& complex_ang_momentum = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size() + kSLIP_DIM * slip_feet_.size() + 3, 3);
+  const auto& complex_lin_momentum = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size() + kSLIP_DIM * slip_feet_.size() + 3 + 3, 3);
+  const auto& complex_gen_state = x.tail(nx_);
+
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, complex_gen_state,  context_);
+
+  *y = drake::VectorX<T>::Zero(3+3+2*kSLIP_DIM*slip_feet_.size());
+
+  y->head(kSLIP_DIM) = slip_com - complex_com({0,2});
+  y->segment(kSLIP_DIM, kSLIP_DIM) = slip_vel * m_ - complex_lin_momentum({0,2});
+  for(int i = 0; i<slip_feet_.size(); i++){
+    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_pos - slip_feet_[i].EvalFull(context_)({0,2});
+    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * slip_feet_.size()+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_vel - slip_feet_[i].EvalFullTimeDerivative(context_)({0,2});
+  }
+}
+
+template<typename T>
+PlanarSlipLifter<T>::PlanarSlipLifter(const drake::multibody::MultibodyPlant<T> &plant,
+                                      drake::systems::Context<T> *context,
+                                      const std::vector<dairlib::multibody::WorldPointEvaluator<T>> &slip_contact_points,
+                                      const std::vector<dairlib::multibody::WorldPointEvaluator<T>> &complex_contact_points,
+                                      const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
+                                      const drake::VectorX<T> &nominal_stand,
+                                      T m,
+                                      double k,
+                                      double r0,
+                                      const drake::VectorX<T> &stance_widths):
+    plant_(plant),
+    context_(context),
+    ik_(plant, context),
+    slip_contact_points_(slip_contact_points),
+    complex_contact_points_(complex_contact_points),
+    simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
+    m_(m),
+    k_(k),
+    r0_(r0),
+    stance_widths_(stance_widths),
+    n_q_(plant.num_positions()),
+    n_v_(plant.num_velocities()){
+  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), nominal_stand);
+  com_vars_ = ik_.get_mutable_prog()->NewContinuousVariables(3);
+  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(&plant, std::nullopt, plant.world_frame(), context);
+  ik_.get_mutable_prog()->AddConstraint(constraint, {ik_.q(),com_vars_});
+
+  const auto& world_frame = plant.world_frame();
+  const auto& pelvis_frame = plant.GetFrameByName("pelvis");
+
+  ik_.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
+                               world_frame, drake::math::RotationMatrix<double>(), 1e-4);
+
+  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("hip_yaw_left")) == 0);
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("hip_yaw_right")) == 0);
+}
+
+template<typename T>
+drake::VectorX<T> PlanarSlipLifter<T>::LiftGeneralizedPosition(const drake::Vector3<T> &com_position,
+                                                               const drake::VectorX<T> &slip_feet_positions) {
+  DRAKE_DEMAND(slip_feet_positions.size() == 2*slip_contact_points_.size());
+  //Add com position constraint
+  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(com_position, com_position, com_vars_);
+  //Add feet position constraint
+  std::vector<drake::solvers::Binding<drake::solvers::Constraint>> foot_constraints;
+  for(int i = 0; i < slip_contact_points_.size(); i++){
+    foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), drake::VectorX<T>::Zero(3), plant_.world_frame(),
+                                                         {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]},
+                                                         {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]}));
+  }
+  //Set initial guess for com
+  ik_.get_mutable_prog()->SetInitialGuess(com_vars_,com_position);
+  //Solve
+  const auto result = drake::solvers::Solve(ik_.prog());
+  const auto q_sol = result.GetSolution(ik_.q());
+  //Normalize quaternion
+  drake::VectorX<T> q_sol_normd(n_q_);
+  q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q_ - 4);
+  //Set initial guess for next time
+  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
+  //Remove added constraints
+  ik_.get_mutable_prog()->RemoveConstraint(com_constraint);
+  for(const auto& constraint : foot_constraints){
+    ik_.get_mutable_prog()->RemoveConstraint(constraint);
+  }
+  // TODO figure out what to do about toe
+  return q_sol_normd;
+}
+template<typename T>
+drake::VectorX<T> PlanarSlipLifter<T>::LiftGeneralizedVelocity(const drake::VectorX<T>& generalized_pos,
+                                                               const drake::Vector3<T>& linear_momentum,
+                                                               const drake::Vector3<T>& com_pos,
+                                                               const drake::VectorX<T>& slip_feet_velocities) {
+  DRAKE_DEMAND(slip_feet_velocities.size() == 2*slip_contact_points_.size());
+  // Preallocate linear constraint
+  drake::MatrixX<T> A(6 + 3 * slip_contact_points_.size() ,n_v_); // 6 rows for momentum, 3 rows for each slip foot
+  drake::VectorX<T> b(6 + 3 * slip_contact_points_.size());
+
+  b.segment(3 * slip_contact_points_.size(), 3) = drake::VectorX<T>::Zero(3);
+  b.tail(3) = linear_momentum;
+
+  dairlib::multibody::SetPositionsIfNew(plant_, generalized_pos, context_);
+
+
+  for(int i = 0; i < slip_contact_points_.size(); i++){
+    b.segment(3 * i, 3) = {slip_feet_velocities[2 * i] , 0 , slip_feet_velocities[2 * i + 1]};
+    slip_contact_points_[i].EvalFullJacobian(context_ , &A.middleRows(3 * i, 3));
+  }
+
+  // Finite difference to calculate momentum jacobian
+  drake::VectorX<T> x_val = drake::VectorX<T>::Zero(n_v_);
+  drake::VectorX<T> yi(6);
+  dairlib::multibody::SetVelocitiesIfNew(plant_,x_val,context_);
+  auto y0 = plant_.CalcSpatialMomentumInWorldAboutPoint(context_, com_pos).get_coeffs();
+  const T eps = 1e-7;
+  for (int i = 0; i < n_v_; i++) {
+    x_val(i) += eps;
+    dairlib::multibody::SetVelocitiesIfNew(plant_,x_val,context_);
+    x_val(i) -= eps;
+    A.col(i).tail(6) = (plant_.CalcSpatialMomentumInWorldAboutPoint(context_, com_pos).get_coeffs() - y0) / eps;
+  }
+
+  // TODO figure out what to do about toe
+  return A.template bdcSvd<Eigen::ComputeThinU | Eigen::ComputeThinV>().solve(b);
+}
+
+template<typename T>
+drake::VectorX<T> PlanarSlipLifter<T>::LiftContactPos(const drake::VectorX<T> &generalized_position) {
+  dairlib::multibody::SetPositionsIfNew(plant_, generalized_position, context_);
+  drake::VectorX<T> rv(complex_contact_points_.size() * 3);
+  for(int i = 0; i < complex_contact_points_.size(); i++){
+    rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFull(context_);
+  }
+  return rv;
+}
+
+template<typename T>
+drake::VectorX<T> PlanarSlipLifter<T>::LiftContactVel(const drake::VectorX<T> &generalized_state) {
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew(plant_, generalized_state, context_);
+  drake::VectorX<T> rv(complex_contact_points_.size() * 3);
+  for(int i = 0; i < complex_contact_points_.size(); i++){
+    rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFullTimeDerivative(context_);
+  }
+  return rv;
+}
+
+template<typename T>
+drake::VectorX<T> PlanarSlipLifter<T>::LiftGrf(const drake::VectorX<T> &com_pos,
+                                               const drake::VectorX<T> &slip_feet_pos,
+                                               const drake::VectorX<T> &complex_contact_point_pos) {
+  drake::VectorX<T> rv(complex_contact_points_.size() * 3);
+
+  // Loop through the slip feet
+  for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
+    // Calculate the slip grf
+    double r = (slip_feet_pos - com_pos({0, 2})).norm();
+    double slip_grf_mag = k_ * (r - r0_);
+
+    // Find the average location for all of the complex contact points that make up the SLIP foot
+    drake::Vector3<T> average_pos = drake::VectorX<T>::Zero(3);
+    const auto& complex_feet_list = simple_foot_index_to_complex_foot_index_.at(simple_index);
+    for(const auto& complex_index : complex_feet_list){
+      average_pos = average_pos + complex_contact_point_pos.segment(3 * complex_index, complex_index);
+    }
+    average_pos = average_pos/complex_feet_list.size();
+
+    // Direction of all the grf for this slip foot must be parallel to not create internal forces
+    // direction is the vector from average contact point to com, so no moment from sum of grf
+    const auto dir = (com_pos - average_pos).normalized();
+    // Distribute grf magnitude across all of the complex contact points
+    for(const auto& complex_index : complex_feet_list){
+      rv.segment(3 * complex_index, complex_index) = dir * slip_grf_mag/complex_feet_list.size();
+    }
+  }
+  return rv;
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h
new file mode 100644
index 0000000000..a4f0f05359
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h
@@ -0,0 +1,100 @@
+#pragma  once
+
+#include <drake/multibody/plant/multibody_plant.h>
+#include "drake/common/drake_copyable.h"
+#include "drake/common/symbolic.h"
+#include "drake/solvers/constraint.h"
+#include "drake/systems/trajectory_optimization/multiple_shooting.h"
+#include "solvers/nonlinear_constraint.h"
+#include "multibody/kinematic/kinematic_evaluator_set.h"
+#include "multibody/kinematic/world_point_evaluator.h"
+#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
+
+template <typename T>
+class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstraint<T> {
+
+ public:
+  PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<T>& plant,
+                                drake::systems::Context<T>* context,
+                                const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_feet,
+                                int complex_state_size,
+                                T m, int knot_index);
+
+ private:
+  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
+                          drake::VectorX<T>* y) const override;
+
+  const T m_;
+  drake::systems::Context<T>* context_;
+  const drake::multibody::MultibodyPlant<T>& plant_;
+  int nx_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_feet_;
+  const int kSLIP_DIM = 2;
+};
+
+template <typename T>
+class PlanarSlipLifter {
+ public:
+  PlanarSlipLifter(const drake::multibody::MultibodyPlant<T>& plant,
+                   drake::systems::Context<T>* context,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_contact_points,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& complex_contact_points,
+                   const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
+                   const drake::VectorX<T>& nominal_stand,
+                   T m,
+                   double k,
+                   double r0,
+                   const drake::VectorX<T>& stance_widths);
+
+ private:
+  /*!
+   * @brief given the center of mass position and slip feet position solve for the generalized position of the full robot
+   * @note assumes identity orientation
+   * @note implements numerical ik (can be slow)
+   * @param com_position center of mass position in the world
+   * @param slip_feet_positions [2*n_slip_feet] locations of the planar slip feet
+   * @return the generalized positions
+   */
+  drake::VectorX<T> LiftGeneralizedPosition(const drake::Vector3<T>& com_position, const drake::VectorX<T>& slip_feet_positions);
+
+  /*!
+   * @brief Given a generalized position calculate the generalized velocity that is the least squares solution to tracking the
+   * centroidal momentum and foot velocity.
+   * @note Assumes 0 angular momentum
+   * @note Jacobian for momentum is calculated numerically
+   * @param generalized_pos
+   * @param linear_momentum
+   * @param com_pos
+   * @param slip_feet_velocities [2*n_slip_feet] the foot velocity
+   * @return generalized velocities
+   */
+  drake::VectorX<T> LiftGeneralizedVelocity(const drake::VectorX<T>& generalized_pos,
+                                            const drake::Vector3<T>& linear_momentum,
+                                            const drake::Vector3<T>& com_pos,
+                                            const drake::VectorX<T>& slip_feet_velocities);
+
+  drake::VectorX<T> LiftContactPos(const drake::VectorX<T>& generalized_position);
+
+  drake::VectorX<T> LiftContactVel(const drake::VectorX<T>& generalized_state);
+
+  drake::VectorX<T> LiftGrf(const drake::VectorX<T>& com_pos,
+                            const drake::VectorX<T>& slip_feet_pos,
+                            const drake::VectorX<T>& complex_contact_point_pos);
+
+
+  const drake::multibody::MultibodyPlant<T>& plant_;
+  drake::systems::Context<T>* context_;
+  drake::multibody::InverseKinematics ik_;
+  const T m_;
+  const double k_;
+  const double r0_;
+  const drake::VectorX<T>& stance_widths_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_contact_points_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& complex_contact_points_;
+  const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index_;
+  const int n_q_;
+  const int n_v_;
+
+  drake::solvers::VectorXDecisionVariable com_vars_;
+};
diff --git a/multibody/kinematic/world_point_evaluator.h b/multibody/kinematic/world_point_evaluator.h
index 420aaf22da..04ac3b1a34 100644
--- a/multibody/kinematic/world_point_evaluator.h
+++ b/multibody/kinematic/world_point_evaluator.h
@@ -93,6 +93,8 @@ class WorldPointEvaluator : public KinematicEvaluator<T> {
   /// evaluator's output.
   void set_frictional() { is_frictional_ = true; };
 
+  const drake::multibody::Frame<T>& get_frame(){return frame_A_;};
+
  private:
   const Eigen::Vector3d pt_A_;
   const drake::multibody::Frame<T>& frame_A_;
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
index fd0d6651b7..03d8f344b5 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
@@ -104,7 +104,7 @@ void KinematicIntegratorConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
   const auto& v1 = x.segment(n_q_ + n_q_ + n_v_, n_v_);
 
   dairlib::multibody::SetPositionsIfNew<T>(plant_, q0, context0_);
-  dairlib::multibody::SetPositionsIfNew<T>(plant_, q0, context0_);
+  dairlib::multibody::SetPositionsIfNew<T>(plant_, q1, context1_);
 
   drake::VectorX<T> qdot0(n_q_);
   drake::VectorX<T> qdot1(n_q_);

From ef4a4f035f76dd26a0af6abfd526e7a030db2e34 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 9 Nov 2022 22:25:20 -0500
Subject: [PATCH 35/76] Having weird com velocity error

---
 .../simple_models/BUILD.bazel                 |  22 +-
 .../simple_models/planar_slip.cc              | 177 +-------------
 .../simple_models/planar_slip.h               |  74 +-----
 .../simple_models/planar_slip_lifter.cc       | 231 ++++++++++++++++++
 .../simple_models/planar_slip_lifter.h        |  81 ++++++
 .../simple_models/planar_slip_lifting_test.cc | 133 ++++++++++
 multibody/kinematic/world_point_evaluator.h   |   2 +-
 7 files changed, 478 insertions(+), 242 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
index 664c5484d4..1d699d8d20 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
@@ -3,8 +3,10 @@ package(default_visibility = ["//visibility:public"])
 
 cc_library(
     name = "planar_slip",
-    srcs = ["planar_slip.cc",],
-    hdrs = ["planar_slip.h",],
+    srcs = ["planar_slip.cc",
+            "planar_slip_lifter.cc"],
+    hdrs = ["planar_slip.h",
+            "planar_slip_lifter.h"],
     deps = [
         "//solvers:constraints",
         "//multibody/kinematic:kinematic",
@@ -12,3 +14,19 @@ cc_library(
         "@drake//:drake_shared_library",
     ],
 )
+
+cc_binary(
+    name = "cassie_planar_slip_lifting_test",
+    srcs = ["planar_slip_lifting_test.cc"],
+    deps = [
+        "//examples/Cassie:cassie_utils",
+        "//examples/Cassie/kinematic_centroidal_mpc/simple_models:planar_slip",
+        "//common",
+        "//multibody:visualization_utils",
+        "//multibody/kinematic",
+        "//systems/primitives",
+        "//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_reference_generator",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
index 879095d7b5..eaffee4423 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
@@ -1,7 +1,6 @@
-#include <drake/solvers/solve.h>
+#include <iostream>
 #include "planar_slip.h"
 #include "multibody/multibody_utils.h"
-#include "multibody/kinematic/kinematic_constraints.h"
 
 template<typename T>
 PlanarSlipReductionConstraint<T>::PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<T>& plant,
@@ -9,16 +8,15 @@ PlanarSlipReductionConstraint<T>::PlanarSlipReductionConstraint(const drake::mul
                                                                 const std::vector<dairlib::multibody::WorldPointEvaluator<
                                                                     T>> &slip_feet,
                                                                 int complex_state_size,
-                                                                T m,
                                                                 int knot_index):dairlib::solvers::NonlinearConstraint<T>(
                                                                     3+3+2*2*slip_feet.size(), 3+3+2*2*slip_feet.size() + complex_state_size,
                                                                     Eigen::VectorXd::Zero(3+3+2*2*slip_feet.size()),
                                                                     Eigen::VectorXd::Zero(3+3+2*2*slip_feet.size()),
                                                                     "PlanarSlipReductionConstraint[" +
                                                                         std::to_string(knot_index) + "]"),
+                                                                        m_(plant.CalcTotalMass(*context)),
                                                                         context_(context),
                                                                         plant_(plant),
-                                                                        m_(m),
                                                                         nx_(plant.num_positions()+plant.num_velocities()),
                                                                         slip_feet_(slip_feet)
                                                                     {}
@@ -51,173 +49,14 @@ void PlanarSlipReductionConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
 
   dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, complex_gen_state,  context_);
 
-  *y = drake::VectorX<T>::Zero(3+3+2*kSLIP_DIM*slip_feet_.size());
+  *y = drake::VectorX<T>::Zero(2+2+2*kSLIP_DIM*slip_feet_.size());
+  y->head(kSLIP_DIM) = slip_com - slip_index_.unaryExpr(complex_com);
 
-  y->head(kSLIP_DIM) = slip_com - complex_com({0,2});
-  y->segment(kSLIP_DIM, kSLIP_DIM) = slip_vel * m_ - complex_lin_momentum({0,2});
+  y->segment(kSLIP_DIM, kSLIP_DIM) = slip_vel * m_ - slip_index_.unaryExpr(complex_lin_momentum);
   for(int i = 0; i<slip_feet_.size(); i++){
-    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_pos - slip_feet_[i].EvalFull(context_)({0,2});
-    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * slip_feet_.size()+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_vel - slip_feet_[i].EvalFullTimeDerivative(context_)({0,2});
+    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_pos.segment(kSLIP_DIM * i, kSLIP_DIM) - slip_index_.unaryExpr(slip_feet_[i].EvalFull(*context_));
+    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * slip_feet_.size()+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_vel.segment(kSLIP_DIM * i, kSLIP_DIM) - slip_index_.unaryExpr(slip_feet_[i].EvalFullTimeDerivative(*context_));
   }
 }
 
-template<typename T>
-PlanarSlipLifter<T>::PlanarSlipLifter(const drake::multibody::MultibodyPlant<T> &plant,
-                                      drake::systems::Context<T> *context,
-                                      const std::vector<dairlib::multibody::WorldPointEvaluator<T>> &slip_contact_points,
-                                      const std::vector<dairlib::multibody::WorldPointEvaluator<T>> &complex_contact_points,
-                                      const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
-                                      const drake::VectorX<T> &nominal_stand,
-                                      T m,
-                                      double k,
-                                      double r0,
-                                      const drake::VectorX<T> &stance_widths):
-    plant_(plant),
-    context_(context),
-    ik_(plant, context),
-    slip_contact_points_(slip_contact_points),
-    complex_contact_points_(complex_contact_points),
-    simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
-    m_(m),
-    k_(k),
-    r0_(r0),
-    stance_widths_(stance_widths),
-    n_q_(plant.num_positions()),
-    n_v_(plant.num_velocities()){
-  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), nominal_stand);
-  com_vars_ = ik_.get_mutable_prog()->NewContinuousVariables(3);
-  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(&plant, std::nullopt, plant.world_frame(), context);
-  ik_.get_mutable_prog()->AddConstraint(constraint, {ik_.q(),com_vars_});
-
-  const auto& world_frame = plant.world_frame();
-  const auto& pelvis_frame = plant.GetFrameByName("pelvis");
-
-  ik_.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
-                               world_frame, drake::math::RotationMatrix<double>(), 1e-4);
-
-  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
-  ik_.get_mutable_prog()->AddLinearConstraint(
-      (ik_.q())(positions_map.at("hip_yaw_left")) == 0);
-  ik_.get_mutable_prog()->AddLinearConstraint(
-      (ik_.q())(positions_map.at("hip_yaw_right")) == 0);
-}
-
-template<typename T>
-drake::VectorX<T> PlanarSlipLifter<T>::LiftGeneralizedPosition(const drake::Vector3<T> &com_position,
-                                                               const drake::VectorX<T> &slip_feet_positions) {
-  DRAKE_DEMAND(slip_feet_positions.size() == 2*slip_contact_points_.size());
-  //Add com position constraint
-  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(com_position, com_position, com_vars_);
-  //Add feet position constraint
-  std::vector<drake::solvers::Binding<drake::solvers::Constraint>> foot_constraints;
-  for(int i = 0; i < slip_contact_points_.size(); i++){
-    foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), drake::VectorX<T>::Zero(3), plant_.world_frame(),
-                                                         {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]},
-                                                         {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]}));
-  }
-  //Set initial guess for com
-  ik_.get_mutable_prog()->SetInitialGuess(com_vars_,com_position);
-  //Solve
-  const auto result = drake::solvers::Solve(ik_.prog());
-  const auto q_sol = result.GetSolution(ik_.q());
-  //Normalize quaternion
-  drake::VectorX<T> q_sol_normd(n_q_);
-  q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q_ - 4);
-  //Set initial guess for next time
-  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
-  //Remove added constraints
-  ik_.get_mutable_prog()->RemoveConstraint(com_constraint);
-  for(const auto& constraint : foot_constraints){
-    ik_.get_mutable_prog()->RemoveConstraint(constraint);
-  }
-  // TODO figure out what to do about toe
-  return q_sol_normd;
-}
-template<typename T>
-drake::VectorX<T> PlanarSlipLifter<T>::LiftGeneralizedVelocity(const drake::VectorX<T>& generalized_pos,
-                                                               const drake::Vector3<T>& linear_momentum,
-                                                               const drake::Vector3<T>& com_pos,
-                                                               const drake::VectorX<T>& slip_feet_velocities) {
-  DRAKE_DEMAND(slip_feet_velocities.size() == 2*slip_contact_points_.size());
-  // Preallocate linear constraint
-  drake::MatrixX<T> A(6 + 3 * slip_contact_points_.size() ,n_v_); // 6 rows for momentum, 3 rows for each slip foot
-  drake::VectorX<T> b(6 + 3 * slip_contact_points_.size());
-
-  b.segment(3 * slip_contact_points_.size(), 3) = drake::VectorX<T>::Zero(3);
-  b.tail(3) = linear_momentum;
-
-  dairlib::multibody::SetPositionsIfNew(plant_, generalized_pos, context_);
-
-
-  for(int i = 0; i < slip_contact_points_.size(); i++){
-    b.segment(3 * i, 3) = {slip_feet_velocities[2 * i] , 0 , slip_feet_velocities[2 * i + 1]};
-    slip_contact_points_[i].EvalFullJacobian(context_ , &A.middleRows(3 * i, 3));
-  }
-
-  // Finite difference to calculate momentum jacobian
-  drake::VectorX<T> x_val = drake::VectorX<T>::Zero(n_v_);
-  drake::VectorX<T> yi(6);
-  dairlib::multibody::SetVelocitiesIfNew(plant_,x_val,context_);
-  auto y0 = plant_.CalcSpatialMomentumInWorldAboutPoint(context_, com_pos).get_coeffs();
-  const T eps = 1e-7;
-  for (int i = 0; i < n_v_; i++) {
-    x_val(i) += eps;
-    dairlib::multibody::SetVelocitiesIfNew(plant_,x_val,context_);
-    x_val(i) -= eps;
-    A.col(i).tail(6) = (plant_.CalcSpatialMomentumInWorldAboutPoint(context_, com_pos).get_coeffs() - y0) / eps;
-  }
-
-  // TODO figure out what to do about toe
-  return A.template bdcSvd<Eigen::ComputeThinU | Eigen::ComputeThinV>().solve(b);
-}
-
-template<typename T>
-drake::VectorX<T> PlanarSlipLifter<T>::LiftContactPos(const drake::VectorX<T> &generalized_position) {
-  dairlib::multibody::SetPositionsIfNew(plant_, generalized_position, context_);
-  drake::VectorX<T> rv(complex_contact_points_.size() * 3);
-  for(int i = 0; i < complex_contact_points_.size(); i++){
-    rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFull(context_);
-  }
-  return rv;
-}
-
-template<typename T>
-drake::VectorX<T> PlanarSlipLifter<T>::LiftContactVel(const drake::VectorX<T> &generalized_state) {
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew(plant_, generalized_state, context_);
-  drake::VectorX<T> rv(complex_contact_points_.size() * 3);
-  for(int i = 0; i < complex_contact_points_.size(); i++){
-    rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFullTimeDerivative(context_);
-  }
-  return rv;
-}
-
-template<typename T>
-drake::VectorX<T> PlanarSlipLifter<T>::LiftGrf(const drake::VectorX<T> &com_pos,
-                                               const drake::VectorX<T> &slip_feet_pos,
-                                               const drake::VectorX<T> &complex_contact_point_pos) {
-  drake::VectorX<T> rv(complex_contact_points_.size() * 3);
-
-  // Loop through the slip feet
-  for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
-    // Calculate the slip grf
-    double r = (slip_feet_pos - com_pos({0, 2})).norm();
-    double slip_grf_mag = k_ * (r - r0_);
-
-    // Find the average location for all of the complex contact points that make up the SLIP foot
-    drake::Vector3<T> average_pos = drake::VectorX<T>::Zero(3);
-    const auto& complex_feet_list = simple_foot_index_to_complex_foot_index_.at(simple_index);
-    for(const auto& complex_index : complex_feet_list){
-      average_pos = average_pos + complex_contact_point_pos.segment(3 * complex_index, complex_index);
-    }
-    average_pos = average_pos/complex_feet_list.size();
-
-    // Direction of all the grf for this slip foot must be parallel to not create internal forces
-    // direction is the vector from average contact point to com, so no moment from sum of grf
-    const auto dir = (com_pos - average_pos).normalized();
-    // Distribute grf magnitude across all of the complex contact points
-    for(const auto& complex_index : complex_feet_list){
-      rv.segment(3 * complex_index, complex_index) = dir * slip_grf_mag/complex_feet_list.size();
-    }
-  }
-  return rv;
-}
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipReductionConstraint);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h
index a4f0f05359..360c8ba896 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h
@@ -18,8 +18,7 @@ class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstrai
   PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<T>& plant,
                                 drake::systems::Context<T>* context,
                                 const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_feet,
-                                int complex_state_size,
-                                T m, int knot_index);
+                                int complex_state_size, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
@@ -29,72 +28,7 @@ class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstrai
   drake::systems::Context<T>* context_;
   const drake::multibody::MultibodyPlant<T>& plant_;
   int nx_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_feet_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<T>> slip_feet_;
   const int kSLIP_DIM = 2;
-};
-
-template <typename T>
-class PlanarSlipLifter {
- public:
-  PlanarSlipLifter(const drake::multibody::MultibodyPlant<T>& plant,
-                   drake::systems::Context<T>* context,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_contact_points,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& complex_contact_points,
-                   const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
-                   const drake::VectorX<T>& nominal_stand,
-                   T m,
-                   double k,
-                   double r0,
-                   const drake::VectorX<T>& stance_widths);
-
- private:
-  /*!
-   * @brief given the center of mass position and slip feet position solve for the generalized position of the full robot
-   * @note assumes identity orientation
-   * @note implements numerical ik (can be slow)
-   * @param com_position center of mass position in the world
-   * @param slip_feet_positions [2*n_slip_feet] locations of the planar slip feet
-   * @return the generalized positions
-   */
-  drake::VectorX<T> LiftGeneralizedPosition(const drake::Vector3<T>& com_position, const drake::VectorX<T>& slip_feet_positions);
-
-  /*!
-   * @brief Given a generalized position calculate the generalized velocity that is the least squares solution to tracking the
-   * centroidal momentum and foot velocity.
-   * @note Assumes 0 angular momentum
-   * @note Jacobian for momentum is calculated numerically
-   * @param generalized_pos
-   * @param linear_momentum
-   * @param com_pos
-   * @param slip_feet_velocities [2*n_slip_feet] the foot velocity
-   * @return generalized velocities
-   */
-  drake::VectorX<T> LiftGeneralizedVelocity(const drake::VectorX<T>& generalized_pos,
-                                            const drake::Vector3<T>& linear_momentum,
-                                            const drake::Vector3<T>& com_pos,
-                                            const drake::VectorX<T>& slip_feet_velocities);
-
-  drake::VectorX<T> LiftContactPos(const drake::VectorX<T>& generalized_position);
-
-  drake::VectorX<T> LiftContactVel(const drake::VectorX<T>& generalized_state);
-
-  drake::VectorX<T> LiftGrf(const drake::VectorX<T>& com_pos,
-                            const drake::VectorX<T>& slip_feet_pos,
-                            const drake::VectorX<T>& complex_contact_point_pos);
-
-
-  const drake::multibody::MultibodyPlant<T>& plant_;
-  drake::systems::Context<T>* context_;
-  drake::multibody::InverseKinematics ik_;
-  const T m_;
-  const double k_;
-  const double r0_;
-  const drake::VectorX<T>& stance_widths_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_contact_points_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& complex_contact_points_;
-  const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index_;
-  const int n_q_;
-  const int n_v_;
-
-  drake::solvers::VectorXDecisionVariable com_vars_;
-};
+  const Eigen::Vector2i slip_index_{0,2};
+};
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
new file mode 100644
index 0000000000..ef16df85ad
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -0,0 +1,231 @@
+#include <drake/solvers/solve.h>
+#include <iostream>
+#include "planar_slip_lifter.h"
+#include "multibody/multibody_utils.h"
+
+PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double> &plant,
+                                      drake::systems::Context<double> *context,
+                                      const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
+                                      const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
+                                      const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
+                                      const drake::VectorX<double> &nominal_stand,
+                                      double k,
+                                      double r0,
+                                      const std::vector<double> &stance_widths):
+    plant_(plant),
+    context_(context),
+    ik_(plant, context),
+    m_(plant.CalcTotalMass(*context)),
+    k_(k),
+    r0_(r0),
+    stance_widths_(stance_widths),
+    slip_contact_points_(slip_contact_points),
+    complex_contact_points_(complex_contact_points),
+    simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
+    n_q_(plant.num_positions()),
+    n_v_(plant.num_velocities()){
+  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), nominal_stand);
+  com_vars_ = ik_.get_mutable_prog()->NewContinuousVariables(3);
+  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(&plant, std::nullopt, plant.world_frame(), context);
+  ik_.get_mutable_prog()->AddConstraint(constraint, {ik_.q(),com_vars_});
+
+  const auto& world_frame = plant.world_frame();
+  const auto& pelvis_frame = plant.GetFrameByName("pelvis");
+
+  ik_.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
+                               world_frame, drake::math::RotationMatrix<double>(), 1e-4);
+
+  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("hip_yaw_left")) == 0);
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("hip_yaw_right")) == 0);
+
+  // Four bar linkage constraint (without spring)
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("knee_left")) +
+          (ik_.q())(positions_map.at("ankle_joint_left")) ==
+          M_PI * 13 / 180.0);
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("knee_right")) +
+          (ik_.q())(positions_map.at("ankle_joint_right")) ==
+          M_PI * 13 / 180.0);
+
+  // Keep the foot flat (breaks if orientation changes)
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("hip_pitch_right")) +
+          (ik_.q())(positions_map.at("knee_right")) +
+          (ik_.q())(positions_map.at("ankle_joint_right"))+
+          (ik_.q())(positions_map.at("toe_right")) ==
+          -0.862002);
+  ik_.get_mutable_prog()->AddLinearConstraint(
+      (ik_.q())(positions_map.at("hip_pitch_left")) +
+          (ik_.q())(positions_map.at("knee_left")) +
+          (ik_.q())(positions_map.at("ankle_joint_left"))+
+          (ik_.q())(positions_map.at("toe_left")) ==
+          -0.862002);
+
+}
+
+drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Vector3<double> &com_position,
+                                                               const drake::VectorX<double> &slip_feet_positions) {
+  DRAKE_DEMAND(slip_feet_positions.size() == 2*slip_contact_points_.size());
+  //Add com position constraint
+  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(com_position, com_position, com_vars_);
+  //Add feet position constraint
+  std::vector<drake::solvers::Binding<drake::solvers::Constraint>> foot_constraints;
+  for(int i = 0; i < slip_contact_points_.size(); i++){
+    const drake::Vector3<double> slip_spatial_foot_pos =  {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]};
+    foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), drake::VectorX<double>::Zero(3), plant_.world_frame(),
+                                                         std::nullopt,
+                                                         slip_spatial_foot_pos,
+                                                         slip_spatial_foot_pos));
+  }
+  //Set initial guess for com
+  ik_.get_mutable_prog()->SetInitialGuess(com_vars_,com_position);
+  //Solve
+  const auto result = drake::solvers::Solve(ik_.prog());
+  const auto q_sol = result.GetSolution(ik_.q());
+  //Normalize quaternion
+  drake::VectorX<double> q_sol_normd(n_q_);
+  q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q_ - 4);
+  //Set initial guess for next time
+  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
+  //Remove added constraints
+  ik_.get_mutable_prog()->RemoveConstraint(com_constraint);
+  for(const auto& constraint : foot_constraints){
+    ik_.get_mutable_prog()->RemoveConstraint(constraint);
+  }
+  // TODO figure out what to do about toe
+  return q_sol_normd;
+}
+drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::VectorX<double>& generalized_pos,
+                                                               const drake::Vector3<double>& linear_momentum,
+                                                               const drake::Vector3<double>& com_pos,
+                                                               const drake::VectorX<double>& slip_feet_velocities) {
+  DRAKE_DEMAND(slip_feet_velocities.size() == 2*slip_contact_points_.size());
+  // Preallocate linear constraint
+  drake::MatrixX<double> A(3 + 3 * slip_contact_points_.size() ,n_v_); // 3 rows for linear momentum, 3 rows for each slip foot
+  drake::VectorX<double> b(3 + 3 * slip_contact_points_.size());
+
+  b.segment(3 * slip_contact_points_.size(), 3) = drake::VectorX<double>::Zero(3);
+  b.tail(3) = linear_momentum;
+
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
+
+  slip_contact_points_[0].EvalFull(*context_ );
+
+  for(int i = 0; i < slip_contact_points_.size(); i++){
+    b.segment(3 * i, 3) = Eigen::Vector3d({slip_feet_velocities[2 * i] , 0 , slip_feet_velocities[2 * i + 1]});
+    slip_contact_points_[i].EvalFullJacobian(*context_ );
+    A.middleRows(3 * i, 3) = slip_contact_points_[i].EvalFullJacobian(*context_ );
+  }
+
+  // Finite difference to calculate momentum jacobian
+  drake::VectorX<double> x_val = drake::VectorX<double>::Zero(n_v_);
+  drake::VectorX<double> yi(6);
+  dairlib::multibody::SetVelocitiesIfNew<double>(plant_,x_val,context_);
+  auto y0 = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos).translational();
+  const double eps = 1e-7;
+  for (int i = 0; i < n_v_; i++) {
+    x_val(i) += eps;
+    dairlib::multibody::SetVelocitiesIfNew<double>(plant_,x_val,context_);
+    x_val(i) -= eps;
+    A.col(i).tail(3) = (plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos).translational() - y0) / eps;
+  }
+
+  // TODO figure out what to do about toe velocity
+
+  // solve
+  drake::VectorX<double> rv(n_v_);
+  // Set base angular velocity to zero
+  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).bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+  return rv;
+}
+
+drake::VectorX<double> PlanarSlipLifter::LiftContactPos(const drake::VectorX<double> &generalized_position) {
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_position, context_);
+  drake::VectorX<double> rv(complex_contact_points_.size() * 3);
+  for(int i = 0; i < complex_contact_points_.size(); i++){
+    rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFull(*context_);
+  }
+  return rv;
+}
+
+drake::VectorX<double> PlanarSlipLifter::LiftContactVel(const drake::VectorX<double> &generalized_pos,
+                                                        const drake::VectorX<double> &generalized_vel) {
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
+  dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel, context_);
+  drake::VectorX<double> rv(complex_contact_points_.size() * 3);
+  for(int i = 0; i < complex_contact_points_.size(); i++){
+    rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFullTimeDerivative(*context_);
+  }
+  return rv;
+}
+
+drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &com_pos,
+                                               const drake::VectorX<double> &slip_feet_pos,
+                                               const drake::VectorX<double> &complex_contact_point_pos) {
+  drake::VectorX<double> rv(complex_contact_points_.size() * 3);
+  // Loop through the slip feet
+  for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
+    // Calculate the slip grf
+    double r = (slip_feet_pos - slip_index_.unaryExpr(com_pos)).norm();
+    double slip_grf_mag = k_ * (r - r0_);
+
+    // Find the average location for all of the complex contact points that make up the SLIP foot
+    drake::Vector3<double> average_pos = drake::VectorX<double>::Zero(3);
+
+    const auto& complex_feet_list = simple_foot_index_to_complex_foot_index_.at(simple_index);
+    for(const auto& complex_index : complex_feet_list){
+      average_pos = average_pos + complex_contact_point_pos.segment(3 * complex_index, complex_index);
+    }
+    average_pos = average_pos/complex_feet_list.size();
+
+    // Direction of all the grf for this slip foot must be parallel to not create internal forces
+    // direction is the vector from average contact point to com, so no moment from sum of grf
+    const auto dir = (com_pos - average_pos).normalized();
+    // Distribute grf magnitude across all of the complex contact points
+    for(const auto& complex_index : complex_feet_list){
+      rv.segment(3 * complex_index, complex_index) = dir * slip_grf_mag/complex_feet_list.size();
+    }
+  }
+  return rv;
+}
+
+/// Input is of the form:
+///     slip_com
+///     slip_velocity
+///     slip_contact_pos
+///     slip_contact_vel
+/// Output is of the form:
+///     complex_com
+///     complex_ang_momentum
+///     complex_lin_momentum
+///     complex_contact_pos
+///     complex_contact_vel
+///     complex_contact_force
+///     complex_gen_pos
+///     complex_gen_vel
+void PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
+                               drake::VectorX<double> *complex_state) {
+  const auto& slip_com = slip_state.head(kSLIP_DIM);
+  const auto& slip_vel = slip_state.segment(kSLIP_DIM, kSLIP_DIM);
+  const auto& slip_contact_pos = slip_state.segment(kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_contact_points_.size());
+  const auto& slip_contact_vel = slip_state.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size(), kSLIP_DIM * slip_contact_points_.size());
+
+  const drake::Vector3<double> com = {slip_com[0], 0, slip_com[1]};
+  const auto& generalized_pos = LiftGeneralizedPosition(com, slip_contact_pos);
+  const auto& generalized_vel = LiftGeneralizedVelocity(generalized_pos, slip_vel * m_, com, slip_contact_vel);
+
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
+  dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel, context_);
+  const auto& complex_contact_pos = LiftContactPos(generalized_pos);
+  (*complex_state) << com, plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com)
+      .get_coeffs(), complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel), LiftGrf(com,
+                                                                                                    slip_contact_pos,
+                                                                                                    complex_contact_pos),
+      generalized_pos, generalized_vel;
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
new file mode 100644
index 0000000000..a8328817c7
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -0,0 +1,81 @@
+#pragma once
+#include <drake/multibody/plant/multibody_plant.h>
+#include "drake/common/drake_copyable.h"
+#include "drake/common/symbolic.h"
+#include "drake/solvers/constraint.h"
+#include "drake/systems/trajectory_optimization/multiple_shooting.h"
+#include "solvers/nonlinear_constraint.h"
+#include "multibody/kinematic/kinematic_evaluator_set.h"
+#include "multibody/kinematic/world_point_evaluator.h"
+#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
+
+class PlanarSlipLifter {
+ public:
+  PlanarSlipLifter(const drake::multibody::MultibodyPlant<double>& plant,
+                   drake::systems::Context<double>* context,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& slip_contact_points,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& complex_contact_points,
+                   const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
+                   const drake::VectorX<double>& nominal_stand,
+                   double k,
+                   double r0,
+                   const std::vector<double>& stance_widths);
+
+  void Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
+            drake::VectorX<double> *complex_state);
+
+ private:
+  /*!
+   * @brief given the center of mass position and slip feet position solve for the generalized position of the full robot
+   * @note assumes identity orientation
+   * @note implements numerical ik (can be slow)
+   * @param com_position center of mass position in the world
+   * @param slip_feet_positions [2*n_slip_feet] locations of the planar slip feet
+   * @return the generalized positions
+   */
+  drake::VectorX<double> LiftGeneralizedPosition(const drake::Vector3<double>& com_position, const drake::VectorX<double>& slip_feet_positions);
+
+  /*!
+   * @brief Given a generalized position calculate the generalized velocity that is the least squares solution to tracking the
+   * linear momentum and foot velocity with 0 floating base angular velocity.
+   * @note Jacobian for momentum is calculated numerically
+   * @param generalized_pos
+   * @param linear_momentum
+   * @param com_pos
+   * @param slip_feet_velocities [2*n_slip_feet] the foot velocity
+   * @return generalized velocities
+   */
+  drake::VectorX<double> LiftGeneralizedVelocity(const drake::VectorX<double>& generalized_pos,
+                                            const drake::Vector3<double>& linear_momentum,
+                                            const drake::Vector3<double>& com_pos,
+                                            const drake::VectorX<double>& slip_feet_velocities);
+
+  drake::VectorX<double> LiftContactPos(const drake::VectorX<double>& generalized_position);
+
+  drake::VectorX<double> LiftContactVel(const drake::VectorX<double>& generalized_pos, const drake::VectorX<double>& generalized_vel);
+
+  drake::VectorX<double> LiftGrf(const drake::VectorX<double>& com_pos,
+                            const drake::VectorX<double>& slip_feet_pos,
+                            const drake::VectorX<double>& complex_contact_point_pos);
+
+
+  const drake::multibody::MultibodyPlant<double>& plant_;
+  drake::systems::Context<double>* context_;
+  drake::multibody::InverseKinematics ik_;
+  const double m_;
+  const double k_;
+  const double r0_;
+  const std::vector<double> stance_widths_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> complex_contact_points_;
+  const std::map<int, std::vector<int>> simple_foot_index_to_complex_foot_index_;
+  const int n_q_;
+  const int n_v_;
+
+  drake::solvers::VectorXDecisionVariable com_vars_;
+
+  const int kSLIP_DIM = 2;
+  const Eigen::VectorXi slip_index_ = {0,2};
+};
+
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
new file mode 100644
index 0000000000..a410f03f7e
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -0,0 +1,133 @@
+#include <drake/systems/framework/diagram_builder.h>
+#include <drake/geometry/scene_graph.h>
+#include <drake/multibody/parsing/parser.h>
+#include <drake/multibody/plant/multibody_plant.h>
+#include <iostream>
+#include <drake/systems/primitives/trajectory_source.h>
+#include <drake/systems/rendering/multibody_position_to_geometry_pose.h>
+#include <drake/systems/analysis/simulator.h>
+#include <drake/geometry/drake_visualizer.h>
+#include <drake/solvers/solve.h>
+#include <drake/common/yaml/yaml_io.h>
+#include "common/find_resource.h"
+#include "systems/primitives/subvector_pass_through.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
+#include "examples/Cassie/cassie_utils.h"
+#include "multibody/visualization_utils.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h"
+
+using drake::geometry::SceneGraph;
+using drake::multibody::MultibodyPlant;
+using drake::multibody::Parser;
+using drake::geometry::DrakeVisualizer;
+
+int main(int argc, char* argv[]) {
+  // Create fix-spring Cassie MBP
+  drake::systems::DiagramBuilder<double> builder;
+  SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
+  scene_graph.set_name("scene_graph");
+
+  MultibodyPlant<double> plant(0.0);
+  MultibodyPlant<double> plant_vis(0.0);
+
+  Parser parser(&plant);
+  Parser parser_vis(&plant_vis, &scene_graph);
+
+  std::string full_name =
+      dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+  parser.AddModelFromFile(full_name);
+  parser_vis.AddModelFromFile(full_name);
+  plant.Finalize();
+  plant_vis.Finalize();
+  Eigen::VectorXd reference_state = GenerateNominalStand(plant, 0.8, 0.2, false);
+
+  auto context = plant.CreateDefaultContext();
+
+  auto left_toe_pair = dairlib::LeftToeFront(plant);
+  auto left_heel_pair = dairlib::LeftToeRear(plant);
+  auto right_toe_pair = dairlib::RightToeFront(plant);
+  auto right_heel_pair = dairlib::RightToeRear(plant);
+
+  std::vector<int> active_inds{0, 1, 2};
+
+  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, left_toe_pair.first, left_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, left_heel_pair.first, left_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, right_toe_pair.first, right_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, right_heel_pair.first, right_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, {0,0,0}, left_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, {0,0,0}, right_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+
+  PlanarSlipLifter lifter(plant,
+                          context.get(),
+                          {left_slip_eval, right_slip_eval},
+                          {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
+                          {{0, {0, 1}}, {1, {2, 3}}},
+                          reference_state.tail(plant.num_positions()),
+                          2000,
+                          0.5,
+                          {0.1, -0.1});
+
+  Eigen::Vector2d slip_com = {0.2,0.7};
+  Eigen::Vector2d slip_vel = {0.0,0.0};
+  Eigen::Vector4d slip_feet = {0.2,0,0,0};
+  Eigen::Vector4d slip_foot_vel = {0.0,0,0.0,0};
+
+  Eigen::VectorXd slip_state(2+2+4+4);
+  slip_state << slip_com,slip_vel,slip_feet,slip_foot_vel;
+  Eigen::VectorXd complex_state(6 + 3 + 3 * 3 * 4 + plant.num_positions() + plant.num_velocities());
+
+  lifter.Lift(slip_state, &complex_state);
+
+  auto context_2 = plant.CreateDefaultContext();
+  auto constraint = PlanarSlipReductionConstraint<double>(plant, context_2.get(), {left_slip_eval, right_slip_eval}, complex_state.size(), 0);
+
+  drake::VectorX<double> error(slip_state.size());
+  drake::VectorX<double> input(slip_state.size() + complex_state.size());
+  input << slip_state,complex_state;
+  constraint.DoEval(input, &error);
+  std::cout<<error<<std::endl;
+
+  if(true){
+    // Build temporary diagram for visualization
+    drake::systems::DiagramBuilder<double> builder_ik;
+    drake::geometry::SceneGraph<double>& scene_graph_ik = *builder_ik.AddSystem<drake::geometry::SceneGraph>();
+    scene_graph_ik.set_name("scene_graph_ik");
+    MultibodyPlant<double> plant_ik(0.0);
+    Parser parser(&plant_ik, &scene_graph_ik);
+    std::string full_name =
+        dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+    parser.AddModelFromFile(full_name);
+    plant_ik.Finalize();
+
+    // Visualize
+    const auto& x_const = complex_state.tail(plant.num_positions() + plant.num_velocities());
+    drake::trajectories::PiecewisePolynomial<double> pp_xtraj(x_const);
+
+    dairlib::multibody::ConnectTrajectoryVisualizer(&plant_ik, &builder_ik,
+                                                    &scene_graph_ik, pp_xtraj);
+    auto diagram = builder_ik.Build();
+    drake::systems::Simulator<double> simulator(*diagram);
+    simulator.set_target_realtime_rate(.1);
+    simulator.Initialize();
+    simulator.AdvanceTo(0.1);
+  }
+}
+
diff --git a/multibody/kinematic/world_point_evaluator.h b/multibody/kinematic/world_point_evaluator.h
index 04ac3b1a34..3daab08ac2 100644
--- a/multibody/kinematic/world_point_evaluator.h
+++ b/multibody/kinematic/world_point_evaluator.h
@@ -93,7 +93,7 @@ class WorldPointEvaluator : public KinematicEvaluator<T> {
   /// evaluator's output.
   void set_frictional() { is_frictional_ = true; };
 
-  const drake::multibody::Frame<T>& get_frame(){return frame_A_;};
+  const drake::multibody::Frame<T> & get_frame() const {return frame_A_;};
 
  private:
   const Eigen::Vector3d pt_A_;

From 8cfb6c9d36c772b9f7fa3096bcb70bce552e1d75 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 9 Nov 2022 22:50:19 -0500
Subject: [PATCH 36/76] Happy with planar slip lifting function test

---
 .../simple_models/planar_slip_lifter.cc        |  4 +++-
 .../simple_models/planar_slip_lifting_test.cc  | 18 ++++++++++++++----
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index ef16df85ad..bd87aff8aa 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -217,12 +217,14 @@ void PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip
   const auto& slip_contact_vel = slip_state.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size(), kSLIP_DIM * slip_contact_points_.size());
 
   const drake::Vector3<double> com = {slip_com[0], 0, slip_com[1]};
+  const drake::Vector3<double> lin_mom = {m_ * slip_vel[0], 0, m_ * slip_vel[1]};
   const auto& generalized_pos = LiftGeneralizedPosition(com, slip_contact_pos);
-  const auto& generalized_vel = LiftGeneralizedVelocity(generalized_pos, slip_vel * m_, com, slip_contact_vel);
+  const auto& generalized_vel = LiftGeneralizedVelocity(generalized_pos, lin_mom, com, slip_contact_vel);
 
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
   dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel, context_);
   const auto& complex_contact_pos = LiftContactPos(generalized_pos);
+
   (*complex_state) << com, plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com)
       .get_coeffs(), complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel), LiftGrf(com,
                                                                                                     slip_contact_pos,
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index a410f03f7e..509bacc33c 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -86,15 +86,25 @@ int main(int argc, char* argv[]) {
                           {0.1, -0.1});
 
   Eigen::Vector2d slip_com = {0.2,0.7};
-  Eigen::Vector2d slip_vel = {0.0,0.0};
-  Eigen::Vector4d slip_feet = {0.2,0,0,0};
-  Eigen::Vector4d slip_foot_vel = {0.0,0,0.0,0};
+  Eigen::Vector2d slip_vel = {0.1,0.3};
+  Eigen::Vector4d slip_feet = {0.2,0.1,0.2,0.14};
+  Eigen::Vector4d slip_foot_vel = {0.11,0.12,0.15,0.18};
 
   Eigen::VectorXd slip_state(2+2+4+4);
   slip_state << slip_com,slip_vel,slip_feet,slip_foot_vel;
   Eigen::VectorXd complex_state(6 + 3 + 3 * 3 * 4 + plant.num_positions() + plant.num_velocities());
 
+  auto start = std::chrono::high_resolution_clock::now();
   lifter.Lift(slip_state, &complex_state);
+  auto finish = std::chrono::high_resolution_clock::now();
+  std::chrono::duration<double> elapsed = finish - start;
+  std::cout << "Solve time:" << elapsed.count() << std::endl;
+
+  start = std::chrono::high_resolution_clock::now();
+  lifter.Lift(slip_state, &complex_state);
+  finish = std::chrono::high_resolution_clock::now();
+  elapsed = finish - start;
+  std::cout << "Solve time 2:" << elapsed.count() << std::endl;
 
   auto context_2 = plant.CreateDefaultContext();
   auto constraint = PlanarSlipReductionConstraint<double>(plant, context_2.get(), {left_slip_eval, right_slip_eval}, complex_state.size(), 0);
@@ -103,7 +113,7 @@ int main(int argc, char* argv[]) {
   drake::VectorX<double> input(slip_state.size() + complex_state.size());
   input << slip_state,complex_state;
   constraint.DoEval(input, &error);
-  std::cout<<error<<std::endl;
+  std::cout<<"Max Error in inverse test: "<< error.cwiseAbs().maxCoeff() << std::endl;
 
   if(true){
     // Build temporary diagram for visualization

From 609df416f0872a814536a6dd696c3cd42eee63da Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 10 Nov 2022 11:03:21 -0500
Subject: [PATCH 37/76] Added Lifting function

---
 .../simple_models/BUILD.bazel                 |  4 +-
 ...nar_slip.cc => planar_slip_constraints.cc} | 65 +++++++++++++++++--
 ...lanar_slip.h => planar_slip_constraints.h} | 23 +++++++
 .../simple_models/planar_slip_lifter.cc       | 12 ++--
 .../simple_models/planar_slip_lifter.h        | 16 ++---
 .../simple_models/planar_slip_lifting_test.cc |  2 +-
 6 files changed, 101 insertions(+), 21 deletions(-)
 rename examples/Cassie/kinematic_centroidal_mpc/simple_models/{planar_slip.cc => planar_slip_constraints.cc} (54%)
 rename examples/Cassie/kinematic_centroidal_mpc/simple_models/{planar_slip.h => planar_slip_constraints.h} (54%)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
index 1d699d8d20..7ffd709d0d 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
@@ -3,9 +3,9 @@ package(default_visibility = ["//visibility:public"])
 
 cc_library(
     name = "planar_slip",
-    srcs = ["planar_slip.cc",
+    srcs = ["planar_slip_constraints.cc",
             "planar_slip_lifter.cc"],
-    hdrs = ["planar_slip.h",
+    hdrs = ["planar_slip_constraints.h",
             "planar_slip_lifter.h"],
     deps = [
         "//solvers:constraints",
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
similarity index 54%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
rename to examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index eaffee4423..080401e7a7 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -1,5 +1,5 @@
 #include <iostream>
-#include "planar_slip.h"
+#include "planar_slip_constraints.h"
 #include "multibody/multibody_utils.h"
 
 template<typename T>
@@ -9,9 +9,9 @@ PlanarSlipReductionConstraint<T>::PlanarSlipReductionConstraint(const drake::mul
                                                                     T>> &slip_feet,
                                                                 int complex_state_size,
                                                                 int knot_index):dairlib::solvers::NonlinearConstraint<T>(
-                                                                    3+3+2*2*slip_feet.size(), 3+3+2*2*slip_feet.size() + complex_state_size,
-                                                                    Eigen::VectorXd::Zero(3+3+2*2*slip_feet.size()),
-                                                                    Eigen::VectorXd::Zero(3+3+2*2*slip_feet.size()),
+                                                                    2+2+2*2*slip_feet.size(), 2+2+2*2*slip_feet.size() + complex_state_size,
+                                                                    Eigen::VectorXd::Zero(2+2+2*2*slip_feet.size()),
+                                                                    Eigen::VectorXd::Zero(2+2+2*2*slip_feet.size()),
                                                                     "PlanarSlipReductionConstraint[" +
                                                                         std::to_string(knot_index) + "]"),
                                                                         m_(plant.CalcTotalMass(*context)),
@@ -59,4 +59,61 @@ void PlanarSlipReductionConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
   }
 }
 
+PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double> &plant,
+                                                         drake::systems::Context<double> *context,
+                                                         const std::vector<dairlib::multibody::WorldPointEvaluator<
+                                                             double>> &slip_contact_points,
+                                                         const std::vector<dairlib::multibody::WorldPointEvaluator<
+                                                             double>> &complex_contact_points,
+                                                         const std::map<int,
+                                                                        std::vector<int>> &simple_foot_index_to_complex_foot_index,
+                                                         const drake::VectorX<double> &nominal_stand,
+                                                         double k,
+                                                         double r0,
+                                                         const std::vector<double> &stance_widths, int knot_index)
+    : dairlib::solvers::NonlinearConstraint<double>(
+    6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities(),
+    2 + 2 + 2 * 2 * slip_contact_points.size() + 6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions()
+        + plant.num_velocities(),
+    Eigen::VectorXd::Zero(
+        6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities()),
+    Eigen::VectorXd::Zero(
+        6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities()),
+    "PlanarSlipLiftingConstraint[" +
+        std::to_string(knot_index) + "]"),
+      lifting_function_(plant,
+                        context,
+                        slip_contact_points,
+                        complex_contact_points,
+                        simple_foot_index_to_complex_foot_index,
+                        nominal_stand,
+                        k,
+                        r0,
+                        stance_widths),
+      slip_dim_(2 + 2 + 2 * 2 * slip_contact_points.size()),
+      complex_dim_(6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities()){
+
+}
+
+/// Input is of the form and should match the lifting function input and output:
+///     slip_com
+///     slip_velocity
+///     slip_contact_pos
+///     slip_contact_vel
+///     complex_com
+///     complex_ang_momentum
+///     complex_lin_momentum
+///     complex_contact_pos
+///     complex_contact_vel
+///     complex_contact_force
+///     complex_gen_pos
+///     complex_gen_vel
+void PlanarSlipLiftingConstraint::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>> &x,
+                                                     drake::VectorX<double> *y) const {
+  const auto& slip_state = x.head(slip_dim_);
+  const auto& complex_state = x.tail(complex_dim_);
+  lifting_function_.Lift(slip_state, y);
+  *y = *y - complex_state;
+}
+
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipReductionConstraint);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
similarity index 54%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h
rename to examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
index 360c8ba896..6725ff418a 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
@@ -10,6 +10,7 @@
 #include "multibody/kinematic/world_point_evaluator.h"
 #include <drake/multibody/inverse_kinematics/com_position_constraint.h>
 #include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
 
 template <typename T>
 class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstraint<T> {
@@ -31,4 +32,26 @@ class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstrai
   const std::vector<dairlib::multibody::WorldPointEvaluator<T>> slip_feet_;
   const int kSLIP_DIM = 2;
   const Eigen::Vector2i slip_index_{0,2};
+};
+
+class PlanarSlipLiftingConstraint : public dairlib::solvers::NonlinearConstraint<double> {
+
+ public:
+  PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double>& plant,
+                              drake::systems::Context<double>* context,
+                              const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& slip_contact_points,
+                              const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& complex_contact_points,
+                              const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
+                              const drake::VectorX<double>& nominal_stand,
+                              double k,
+                              double r0,
+                              const std::vector<double>& stance_widths, int knot_index);
+
+ private:
+  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
+                          drake::VectorX<double>* y) const override;
+
+  PlanarSlipLifter lifting_function_;
+  const int slip_dim_;
+  const int complex_dim_;
 };
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index bd87aff8aa..3ccf1b90e1 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -68,7 +68,7 @@ PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double
 }
 
 drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Vector3<double> &com_position,
-                                                               const drake::VectorX<double> &slip_feet_positions) {
+                                                               const drake::VectorX<double> &slip_feet_positions) const {
   DRAKE_DEMAND(slip_feet_positions.size() == 2*slip_contact_points_.size());
   //Add com position constraint
   const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(com_position, com_position, com_vars_);
@@ -102,7 +102,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Ve
 drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::VectorX<double>& generalized_pos,
                                                                const drake::Vector3<double>& linear_momentum,
                                                                const drake::Vector3<double>& com_pos,
-                                                               const drake::VectorX<double>& slip_feet_velocities) {
+                                                               const drake::VectorX<double>& slip_feet_velocities)const {
   DRAKE_DEMAND(slip_feet_velocities.size() == 2*slip_contact_points_.size());
   // Preallocate linear constraint
   drake::MatrixX<double> A(3 + 3 * slip_contact_points_.size() ,n_v_); // 3 rows for linear momentum, 3 rows for each slip foot
@@ -145,7 +145,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
   return rv;
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftContactPos(const drake::VectorX<double> &generalized_position) {
+drake::VectorX<double> PlanarSlipLifter::LiftContactPos(const drake::VectorX<double> &generalized_position)const {
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_position, context_);
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
   for(int i = 0; i < complex_contact_points_.size(); i++){
@@ -155,7 +155,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftContactPos(const drake::VectorX<dou
 }
 
 drake::VectorX<double> PlanarSlipLifter::LiftContactVel(const drake::VectorX<double> &generalized_pos,
-                                                        const drake::VectorX<double> &generalized_vel) {
+                                                        const drake::VectorX<double> &generalized_vel)const {
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
   dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel, context_);
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
@@ -167,7 +167,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftContactVel(const drake::VectorX<dou
 
 drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &com_pos,
                                                const drake::VectorX<double> &slip_feet_pos,
-                                               const drake::VectorX<double> &complex_contact_point_pos) {
+                                               const drake::VectorX<double> &complex_contact_point_pos)const {
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
   // Loop through the slip feet
   for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
@@ -210,7 +210,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
 ///     complex_gen_pos
 ///     complex_gen_vel
 void PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
-                               drake::VectorX<double> *complex_state) {
+                               drake::VectorX<double> *complex_state)const {
   const auto& slip_com = slip_state.head(kSLIP_DIM);
   const auto& slip_vel = slip_state.segment(kSLIP_DIM, kSLIP_DIM);
   const auto& slip_contact_pos = slip_state.segment(kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_contact_points_.size());
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
index a8328817c7..0f0e6076c8 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -23,7 +23,7 @@ class PlanarSlipLifter {
                    const std::vector<double>& stance_widths);
 
   void Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
-            drake::VectorX<double> *complex_state);
+            drake::VectorX<double> *complex_state) const;
 
  private:
   /*!
@@ -34,7 +34,7 @@ class PlanarSlipLifter {
    * @param slip_feet_positions [2*n_slip_feet] locations of the planar slip feet
    * @return the generalized positions
    */
-  drake::VectorX<double> LiftGeneralizedPosition(const drake::Vector3<double>& com_position, const drake::VectorX<double>& slip_feet_positions);
+  drake::VectorX<double> LiftGeneralizedPosition(const drake::Vector3<double>& com_position, const drake::VectorX<double>& slip_feet_positions) const;
 
   /*!
    * @brief Given a generalized position calculate the generalized velocity that is the least squares solution to tracking the
@@ -49,20 +49,20 @@ class PlanarSlipLifter {
   drake::VectorX<double> LiftGeneralizedVelocity(const drake::VectorX<double>& generalized_pos,
                                             const drake::Vector3<double>& linear_momentum,
                                             const drake::Vector3<double>& com_pos,
-                                            const drake::VectorX<double>& slip_feet_velocities);
+                                            const drake::VectorX<double>& slip_feet_velocities) const;
 
-  drake::VectorX<double> LiftContactPos(const drake::VectorX<double>& generalized_position);
+  drake::VectorX<double> LiftContactPos(const drake::VectorX<double>& generalized_position) const;
 
-  drake::VectorX<double> LiftContactVel(const drake::VectorX<double>& generalized_pos, const drake::VectorX<double>& generalized_vel);
+  drake::VectorX<double> LiftContactVel(const drake::VectorX<double>& generalized_pos, const drake::VectorX<double>& generalized_vel) const;
 
   drake::VectorX<double> LiftGrf(const drake::VectorX<double>& com_pos,
                             const drake::VectorX<double>& slip_feet_pos,
-                            const drake::VectorX<double>& complex_contact_point_pos);
+                            const drake::VectorX<double>& complex_contact_point_pos) const;
 
 
   const drake::multibody::MultibodyPlant<double>& plant_;
-  drake::systems::Context<double>* context_;
-  drake::multibody::InverseKinematics ik_;
+  mutable drake::systems::Context<double>* context_;
+  mutable drake::multibody::InverseKinematics ik_;
   const double m_;
   const double k_;
   const double r0_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index 509bacc33c..309f8318ae 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -15,7 +15,7 @@
 #include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "multibody/visualization_utils.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h"
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;

From a0545ebd8f07d178cac1c38252b579e1f61e3087 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 10 Nov 2022 13:13:48 -0500
Subject: [PATCH 38/76] Added nonlinear cost

---
 .../simple_models/planar_slip_constraints.cc  | 147 ++++++++++++++----
 .../simple_models/planar_slip_constraints.h   |  89 +++++++++--
 .../simple_models/planar_slip_lifter.cc       |   5 +
 .../simple_models/planar_slip_lifter.h        |   2 +
 .../kinematic_centroidal_mpc/gait.cc          |  10 +-
 .../kinematic_centroidal_mpc.cc               |   6 +-
 6 files changed, 210 insertions(+), 49 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index 080401e7a7..a8b44a5a2b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -1,4 +1,5 @@
 #include <iostream>
+#include <utility>
 #include "planar_slip_constraints.h"
 #include "multibody/multibody_utils.h"
 
@@ -60,38 +61,23 @@ void PlanarSlipReductionConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
 }
 
 PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double> &plant,
-                                                         drake::systems::Context<double> *context,
-                                                         const std::vector<dairlib::multibody::WorldPointEvaluator<
-                                                             double>> &slip_contact_points,
-                                                         const std::vector<dairlib::multibody::WorldPointEvaluator<
-                                                             double>> &complex_contact_points,
-                                                         const std::map<int,
-                                                                        std::vector<int>> &simple_foot_index_to_complex_foot_index,
-                                                         const drake::VectorX<double> &nominal_stand,
-                                                         double k,
-                                                         double r0,
-                                                         const std::vector<double> &stance_widths, int knot_index)
+                                                         std::shared_ptr<PlanarSlipLifter> lifting_function,
+                                                         int n_slip_feet,
+                                                         int n_complex_feet,
+                                                         int knot_index)
     : dairlib::solvers::NonlinearConstraint<double>(
-    6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities(),
-    2 + 2 + 2 * 2 * slip_contact_points.size() + 6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions()
+    6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities(),
+    2 + 2 + 2 * 2 * n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions()
         + plant.num_velocities(),
     Eigen::VectorXd::Zero(
-        6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities()),
+        6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()),
     Eigen::VectorXd::Zero(
-        6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities()),
+        6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()),
     "PlanarSlipLiftingConstraint[" +
         std::to_string(knot_index) + "]"),
-      lifting_function_(plant,
-                        context,
-                        slip_contact_points,
-                        complex_contact_points,
-                        simple_foot_index_to_complex_foot_index,
-                        nominal_stand,
-                        k,
-                        r0,
-                        stance_widths),
-      slip_dim_(2 + 2 + 2 * 2 * slip_contact_points.size()),
-      complex_dim_(6 + 3 + 3 * 3 * complex_contact_points.size() + plant.num_positions() + plant.num_velocities()){
+      lifting_function_(std::move(lifting_function)),
+      slip_dim_(2 + 2 + 2 * 2 * n_slip_feet),
+      complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()){
 
 }
 
@@ -112,8 +98,115 @@ void PlanarSlipLiftingConstraint::EvaluateConstraint(const Eigen::Ref<const drak
                                                      drake::VectorX<double> *y) const {
   const auto& slip_state = x.head(slip_dim_);
   const auto& complex_state = x.tail(complex_dim_);
-  lifting_function_.Lift(slip_state, y);
+  lifting_function_->Lift(slip_state, y);
   *y = *y - complex_state;
 }
 
+template<typename T>
+PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
+                                                              double k,
+                                                              T m,
+                                                              int n_feet,
+                                                              std::vector<bool> contact_mask,
+                                                              double dt,
+                                                              int knot_index):dairlib::solvers::NonlinearConstraint<T>(
+    2 + 2, 2 * (2 + 2 + 2 * n_feet),
+    Eigen::VectorXd::Zero(2 + 2),
+    Eigen::VectorXd::Zero(2 + 2),
+    "PlanarSlipDynamicsConstraint[" +
+        std::to_string(knot_index) + "]"),
+                                                                              r0_(r0),
+                                                                              k_(k),
+                                                                              m_(m),
+                                                                              n_feet_(n_feet),
+                                                                              contact_mask_(std::move(contact_mask)),
+                                                                              dt_(dt) {}
+
+
+/// The format of the input to the eval() function is in the order
+///   - com0, center of mass at time k
+///   - vel0, center of mass velocity at time k
+///   - contact_pos0, active contact positions at time k
+///   - com1, center of mass at time k+1
+///   - vel1, center of mass velocity at time k+1
+///   - contact_pos1, active contact positions at time k+1
+template<typename T>
+void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
+                                                         drake::VectorX<T> *y) const {
+  const auto& com0 = x.head(2);
+  const auto& vel0 = x.segment(2, 2);
+  const auto& contact_pos0 = x.segment(2 + 2, n_feet_ * 2);
+  const auto& com1 = x.segment(2 + 2 + n_feet_ * 2, 2);
+  const auto& vel1 = x.segment(2 + 2 + n_feet_ * 2 + 2, 2);
+  const auto& contact_pos1 = x.segment(2 + 2 + n_feet_ * 2 + 2 + 2, n_feet_ * 2);
+
+  const auto& x0 = x.head(4);
+  const auto& x1 = x.segment(2 + 2 + n_feet_ * 2, 4);
+
+  drake::Vector<T, 4> xdot0 = CalcTimeDerivativesWithForce(com0, vel0,contact_pos0);
+  drake::Vector<T, 4> xdot1 = CalcTimeDerivativesWithForce(com1, vel1,contact_pos1);
+
+  // Predict state and return error
+  const auto x1Predict = x0 + 0.5 * dt_ * (x0 + x1);
+  *y = x1 - x1Predict;
+}
+
+template<typename T>
+drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(const drake::VectorX<T> &com_position,
+                                                                                const drake::VectorX<T> &com_vel,
+                                                                                const drake::VectorX<T> &contact_loc) const {
+  drake::Vector2<T> ddcom = {0, -9.81};
+  for(int foot = 0; foot < n_feet_; foot ++){
+    if(contact_mask_[foot]){
+      drake::Vector2<T> com_rt_foot = com_position - contact_loc.segment(2 * foot, 2);
+      const auto r = com_rt_foot.norm();
+      const auto unit_vec = com_rt_foot/r;
+      const auto F = k_ * (r0_ - r);
+      ddcom = ddcom + F * unit_vec / m_;
+    }
+  }
+  drake::Vector4<T> derivative;
+  derivative << com_vel,ddcom;
+  return derivative;
+}
+
+QuadraticLiftedCost::QuadraticLiftedCost(std::shared_ptr<PlanarSlipLifter> lifting_function,
+                                            QuadraticLiftedCost::cost_element com_cost,
+                                            QuadraticLiftedCost::cost_element momentum_cost,
+                                            QuadraticLiftedCost::cost_element contact_cost,
+                                            QuadraticLiftedCost::cost_element grf_cost,
+                                            QuadraticLiftedCost::cost_element q_cost,
+                                            QuadraticLiftedCost::cost_element v_cost,
+                                            double terminal_gain,
+                                            int n_slip_feet,
+                                            int knot_point):dairlib::solvers::NonlinearCost<double>(
+    2 + 2 + 2 * 2 * n_slip_feet, "LiftedCost[" +
+        std::to_string(knot_point) + "]"),
+                                                            lifting_function_(std::move(lifting_function)),
+                                                            com_cost_(std::move(com_cost)),
+                                                            momentum_cost_(std::move(momentum_cost)),
+                                                            contact_cost_(std::move(contact_cost)),
+                                                            grf_cost_(std::move(grf_cost)),
+                                                            q_cost_(std::move(q_cost)),
+                                                            v_cost_(std::move(v_cost)),
+                                                            terminal_gain_(terminal_gain) {}
+
+void QuadraticLiftedCost::EvaluateCost(const Eigen::Ref<const drake::VectorX<double>> &x, drake::VectorX<double> *y) const {
+  const auto lifted_state = lifting_function_->Lift(x);
+
+  const auto& com = lifted_state.head(3);
+  const auto& momentum = lifted_state.segment(3,6);
+  const auto& contact_info = lifted_state.segment(3 + 6, contact_cost_.ref.size());
+  const auto& grf = lifted_state.segment(3 + 6 + contact_cost_.ref.size(), grf_cost_.ref.size());
+  const auto& q = lifted_state.segment(3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size(), q_cost_.ref.size());
+  const auto& v = lifted_state.segment(3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size() + q_cost_.ref.size(), v_cost_.ref.size());
+}
+
+double QuadraticLiftedCost::CalcCost(const QuadraticLiftedCost::cost_element &cost,
+                                     const Eigen::Ref<const drake::VectorX<double>> &x) {
+  auto error = x-cost.ref;
+  return error.transpose() * cost.Q * error;
+}
+
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipReductionConstraint);
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipDynamicsConstraint);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
index 6725ff418a..3a47966f72 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
@@ -4,12 +4,10 @@
 #include "drake/common/drake_copyable.h"
 #include "drake/common/symbolic.h"
 #include "drake/solvers/constraint.h"
-#include "drake/systems/trajectory_optimization/multiple_shooting.h"
 #include "solvers/nonlinear_constraint.h"
+#include "solvers/nonlinear_cost.h"
 #include "multibody/kinematic/kinematic_evaluator_set.h"
 #include "multibody/kinematic/world_point_evaluator.h"
-#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
-#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 #include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
 
 template <typename T>
@@ -35,23 +33,86 @@ class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstrai
 };
 
 class PlanarSlipLiftingConstraint : public dairlib::solvers::NonlinearConstraint<double> {
-
  public:
   PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double>& plant,
-                              drake::systems::Context<double>* context,
-                              const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& slip_contact_points,
-                              const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& complex_contact_points,
-                              const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
-                              const drake::VectorX<double>& nominal_stand,
-                              double k,
-                              double r0,
-                              const std::vector<double>& stance_widths, int knot_index);
+                              std::shared_ptr<PlanarSlipLifter> lifting_function,
+                              int n_slip_feet, int n_complex_feet,
+                              int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
                           drake::VectorX<double>* y) const override;
 
-  PlanarSlipLifter lifting_function_;
+  std::shared_ptr<PlanarSlipLifter> lifting_function_;
   const int slip_dim_;
   const int complex_dim_;
-};
\ No newline at end of file
+};
+
+template <typename T>
+class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
+
+ public:
+  PlanarSlipDynamicsConstraint(double r0, double k, T m, int n_feet, std::vector<bool>  contact_mask, double dt, int knot_index);
+
+ private:
+  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
+                          drake::VectorX<T>* y) const override;
+
+  drake::VectorX<T> CalcTimeDerivativesWithForce(
+      const drake::VectorX<T>& com_position,
+      const drake::VectorX<T>& com_vel,
+      const drake::VectorX<T>& contact_loc) const;
+
+  const double r0_;
+  const double k_;
+  const T m_;
+  const int n_feet_;
+  const std::vector<bool> contact_mask_;
+  const double dt_;
+};
+
+///     complex_com
+///     complex_ang_momentum
+///     complex_lin_momentum
+///     complex_contact_pos
+///     complex_contact_vel
+///     complex_contact_force
+///     complex_gen_pos
+///     complex_gen_vel
+class QuadraticLiftedCost : public dairlib::solvers::NonlinearCost<double> {
+
+  struct cost_element{
+    const Eigen::MatrixXd& Q;
+    Eigen::MatrixXd ref;
+  };
+
+ public:
+  QuadraticLiftedCost(std::shared_ptr<PlanarSlipLifter> lifting_function,
+                      cost_element  com_cost,
+                      cost_element  momentum_cost,
+                      cost_element  contact_cost,
+                      cost_element  grf_cost,
+                      cost_element  q_cost,
+                      cost_element  v_cost,
+                      double terminal_gain,
+                      int n_slip_feet,
+                      int knot_point);
+
+ private:
+  void EvaluateCost(const Eigen::Ref<const drake::VectorX<double>>& x,
+                          drake::VectorX<double>* y) const override;
+
+  double CalcCost(const cost_element& cost, const Eigen::Ref<const drake::VectorX<double>>& x);
+
+
+  std::shared_ptr<PlanarSlipLifter> lifting_function_;
+  const cost_element com_cost_;
+  const cost_element momentum_cost_;
+  const cost_element contact_cost_;
+  const cost_element grf_cost_;
+  const cost_element q_cost_;
+  const cost_element v_cost_;
+  const double terminal_gain_;
+};
+
+
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index 3ccf1b90e1..2c2774344a 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -231,3 +231,8 @@ void PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip
                                                                                                     complex_contact_pos),
       generalized_pos, generalized_vel;
 }
+drake::VectorX<double> PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state) const {
+  drake::VectorX<double> complex_state(6 + 3 + 3 * 3 * complex_contact_points_.size() + n_q_ + n_v_);
+  Lift(slip_state, &complex_state);
+  return complex_state;
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
index 0f0e6076c8..c113f3c2a1 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -25,6 +25,8 @@ class PlanarSlipLifter {
   void Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
             drake::VectorX<double> *complex_state) const;
 
+  drake::VectorX<double> Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state) const;
+
  private:
   /*!
    * @brief given the center of mass position and slip feet position solve for the generalized position of the full robot
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.cc b/systems/controllers/kinematic_centroidal_mpc/gait.cc
index da119592b3..68c1e2c732 100644
--- a/systems/controllers/kinematic_centroidal_mpc/gait.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/gait.cc
@@ -8,7 +8,7 @@ int Gait::GetCurrentMode(double time_now) const{
       return i;
     }
   }
-  DRAKE_ASSERT(false);
+  DRAKE_DEMAND(false);
   return 0;
 }
 
@@ -39,10 +39,10 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(double curre
 }
 
 void Gait::check_valid() const {
-  DRAKE_ASSERT(period > 0);
-  DRAKE_ASSERT(gait_pattern[0].start_phase == 0);
-  DRAKE_ASSERT(gait_pattern[gait_pattern.size()-1].end_phase == 1);
+  DRAKE_DEMAND(period > 0);
+  DRAKE_DEMAND(gait_pattern[0].start_phase == 0);
+  DRAKE_DEMAND(gait_pattern[gait_pattern.size()-1].end_phase == 1);
   for(int i = 0; i < gait_pattern.size() - 1; i++){
-    DRAKE_ASSERT(gait_pattern[i].end_phase == gait_pattern[i+1].start_phase);
+    DRAKE_DEMAND(gait_pattern[i].end_phase == gait_pattern[i+1].start_phase);
   }
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 2a1c4c6e2d..515a28925e 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -320,9 +320,9 @@ void KinematicCentroidalMPC::SetFromSolution(
     Eigen::MatrixXd* state_samples,
     Eigen::MatrixXd* contact_force_samples,
     std::vector<double>* time_samples) const {
-  DRAKE_ASSERT(state_samples != nullptr);
-  DRAKE_ASSERT(contact_force_samples != nullptr);
-  DRAKE_ASSERT(time_samples->empty());
+  DRAKE_DEMAND(state_samples != nullptr);
+  DRAKE_DEMAND(contact_force_samples != nullptr);
+  DRAKE_DEMAND(time_samples->empty());
 
   *state_samples = MatrixXd(n_q_ + n_v_, n_knot_points_);
   *contact_force_samples = MatrixXd(3 * n_contact_points_, n_knot_points_);

From 1c742fbe40b143e01b746a6f142f6d0ddd2fd3f3 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 10 Nov 2022 13:41:50 -0500
Subject: [PATCH 39/76] Added nonlinear cost for real this time

---
 .../simple_models/planar_slip_constraints.cc             | 9 ++++++---
 .../simple_models/planar_slip_constraints.h              | 2 +-
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index a8b44a5a2b..f226591930 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -200,12 +200,15 @@ void QuadraticLiftedCost::EvaluateCost(const Eigen::Ref<const drake::VectorX<dou
   const auto& grf = lifted_state.segment(3 + 6 + contact_cost_.ref.size(), grf_cost_.ref.size());
   const auto& q = lifted_state.segment(3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size(), q_cost_.ref.size());
   const auto& v = lifted_state.segment(3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size() + q_cost_.ref.size(), v_cost_.ref.size());
+
+  *y = CalcCost(com_cost_, com) + CalcCost(momentum_cost_, momentum) + CalcCost(contact_cost_, contact_info)
+      + CalcCost(grf_cost_, grf) + CalcCost(q_cost_, q) + CalcCost(v_cost_, v);
 }
 
-double QuadraticLiftedCost::CalcCost(const QuadraticLiftedCost::cost_element &cost,
-                                     const Eigen::Ref<const drake::VectorX<double>> &x) {
+Eigen::Matrix<double, -1, 1, 0> QuadraticLiftedCost::CalcCost(const QuadraticLiftedCost::cost_element &cost,
+                                                                    const Eigen::Ref<const drake::VectorX<double>> &x)const {
   auto error = x-cost.ref;
-  return error.transpose() * cost.Q * error;
+  return terminal_gain_ * error.transpose() * cost.Q * error;
 }
 
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipReductionConstraint);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
index 3a47966f72..6a41b128a3 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
@@ -102,7 +102,7 @@ class QuadraticLiftedCost : public dairlib::solvers::NonlinearCost<double> {
   void EvaluateCost(const Eigen::Ref<const drake::VectorX<double>>& x,
                           drake::VectorX<double>* y) const override;
 
-  double CalcCost(const cost_element& cost, const Eigen::Ref<const drake::VectorX<double>>& x);
+  Eigen::Matrix<double, -1, 1, 0> CalcCost(const cost_element& cost, const Eigen::Ref<const drake::VectorX<double>>& x) const;
 
 
   std::shared_ptr<PlanarSlipLifter> lifting_function_;

From 8f943ce049aecb3ab21b73a19bcf67a30c6f09f5 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 10 Nov 2022 14:15:02 -0500
Subject: [PATCH 40/76] Back to walking

---
 .../Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc | 4 +++-
 examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml   | 2 +-
 examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml    | 2 +-
 .../kinematic_centroidal_mpc/trajectory_parameters.yaml     | 6 +++---
 4 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 3ca5ef1bb2..03b930b7ed 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -66,13 +66,15 @@ int DoMain(int argc, char* argv[]) {
   // Create gaits
   auto stand = drake::yaml::LoadYamlFile<Gait>(
       "examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml");
+  auto walk = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml");
   auto step = drake::yaml::LoadYamlFile<Gait>(
       "examples/Cassie/kinematic_centroidal_mpc/gaits/step.yaml");
 
   // Create reference
   // TODO(yangwill): move this into the reference generator
   // Specify knot points
-  std::vector<Gait> gait_samples = {stand, step, stand};
+  std::vector<Gait> gait_samples = {stand, walk, stand};
   DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size());
   std::vector<double> durations = std::vector<double>(gait_samples.size());
   for (int i = 0; i < gait_samples.size(); ++i) {
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml b/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml
index cc525ff774..ef0181b9ca 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml
@@ -2,4 +2,4 @@ gait_pattern:
   - start_phase: 0.0
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 1.0
+period: 0.5
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml b/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml
index b1f566edd9..6036fc59dd 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml
@@ -11,4 +11,4 @@ gait_pattern:
   - start_phase: 0.9
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 1.25
\ No newline at end of file
+period: 1.0
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml b/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
index 549c119580..02bfc91688 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
@@ -1,7 +1,7 @@
-n_knot_points: 15
+n_knot_points: 45
 duration_scaling:
-  [1.0, 1.0, 1.0]
+  [1.0, 3.0, 1.0]
 com_height: 0.8
 stance_width: 0.25
-vel: 0.0
+vel: 0.5
 tol: 1e-2
\ No newline at end of file

From 793d0d3085c00fe1c183c8485b26797be5137904 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 10 Nov 2022 16:54:14 -0500
Subject: [PATCH 41/76] Slowly putting adpative complexity mpc together

---
 .../cassie_kinematic_centroidal_mpc.cc        |  22 ++
 .../cassie_kinematic_centroidal_mpc.h         |  37 ++-
 .../kinematic_centroidal_mpc.cc               | 237 +++++++++---------
 .../kinematic_centroidal_mpc.h                |  29 ++-
 4 files changed, 202 insertions(+), 123 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 31c8ef1896..977361c33b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -54,3 +54,25 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
   }
 
 }
+
+void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
+  // TODO add planar slip constraints
+}
+void CassieKinematicCentroidalMPC::AddPlanarSlipCost(double t, double terminal_gain) {
+  // TODO add planar slip costs
+}
+
+void CassieKinematicCentroidalMPC::SetModeSequence(const std::vector<std::vector<bool>> &contact_sequence) {
+  KinematicCentroidalMPC::SetModeSequence(contact_sequence);
+  MapModeSequence();
+}
+void CassieKinematicCentroidalMPC::SetModeSequence(const drake::trajectories::PiecewisePolynomial<double> &contact_sequence) {
+  KinematicCentroidalMPC::SetModeSequence(contact_sequence);
+  MapModeSequence();
+}
+
+void CassieKinematicCentroidalMPC::MapModeSequence() {
+  for(int knot_point = 0; knot_point < num_knot_points(); knot_point ++){
+    slip_contact_sequence_[knot_point] = complex_mode_to_slip_mode_.at(contact_sequence_[knot_point]);
+  }
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 20571c808b..1cf32636f7 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -13,9 +13,16 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
       KinematicCentroidalMPC(plant, n_knot_points, dt, CreateContactPoints(plant, mu)),
       l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
       r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
-      loop_closure_evaluators(Plant()){
+      loop_closure_evaluators(Plant()),
+      slip_contact_sequence_(n_knot_points){
     AddPlantJointLimits(dairlib::JointNames());
     AddLoopClosure();
+
+    //TODO Add slip decision variables
+    //TODO Create lifter functions
+    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(), stance_mode);
   }
 
   /*!
@@ -25,7 +32,29 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
    * @return
    */
   std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
+
+  /*!
+ * @brief Set the mode sequence
+ * @param contact_sequence vector where
+ * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point`
+ * is `contact_index` active
+ */
+  void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence) override;
+
+  /*!
+   * @brief Set the mode sequence via a trajectory. The value of the trajectory
+   * at each time, cast to a bool is if a contact point is active or not
+   * @param contact_sequence
+   */
+  void SetModeSequence(
+      const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) override;
+
  private:
+  void MapModeSequence();
+
+  void AddPlanarSlipConstraints(int knot_point) override;
+
+  void AddPlanarSlipCost(double t, double terminal_gain) override;
 
   /*!
    * @brief Adds loop closure constraints to the mpc
@@ -36,5 +65,11 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   dairlib::multibody::DistanceEvaluator<double> r_loop_evaluator_;
   dairlib::multibody::KinematicEvaluatorSet<double> loop_closure_evaluators;
 
+  std::vector<std::vector<bool>> slip_contact_sequence_;
+
+  std::map<std::vector<bool>, std::vector<bool>> complex_mode_to_slip_mode_{{{true, true, true, true},{true, true}},
+                                                                            {{true, true, false, false},{true, false}},
+                                                                            {{false, false, true, true},{false, true}},
+                                                                            {{false, false, false, false},{false, false}}};
 };
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 940c258d10..6640fdc67e 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -35,7 +35,8 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(
       Q_force_(
           Eigen::MatrixXd::Zero(3 * n_contact_points_, 3 * n_contact_points_)),
       contact_sequence_(n_knot_points),
-      contexts_(n_knot_points) {
+      contexts_(n_knot_points),
+      complexity_schedule_(n_knot_points){
   n_joint_q_ = n_q_ - kCentroidalPosDim;
   n_joint_v_ = n_v_ - kCentroidalVelDim;
   prog_ = std::make_unique<drake::solvers::MathematicalProgram>();
@@ -65,6 +66,7 @@ KinematicCentroidalMPC::KinematicCentroidalMPC(
   std::vector<bool> stance_mode(n_contact_points_);
   std::fill(stance_mode.begin(), stance_mode.end(), true);
   std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
+  std::fill(complexity_schedule_.begin(), complexity_schedule_.end(), Complexity::KINEMATIC_CENTROIDAL);
 }
 
 void KinematicCentroidalMPC::AddGenPosReference(
@@ -97,8 +99,8 @@ void KinematicCentroidalMPC::AddMomentumReference(
   mom_ref_traj_ = std::move(ref_traj);
 }
 
-void KinematicCentroidalMPC::AddCentroidalDynamics() {
-  for (int knot_point = 0; knot_point < n_knot_points_ - 1; knot_point++) {
+void KinematicCentroidalMPC::AddCentroidalDynamics(int knot_point) {
+  if(!(is_last_knot(knot_point))){
     auto constraint = std::make_shared<CentroidalDynamicsConstraint<double>>(
         plant_, contexts_[knot_point].get(), n_contact_points_, dt_,
         knot_point);
@@ -111,8 +113,8 @@ void KinematicCentroidalMPC::AddCentroidalDynamics() {
   }
 }
 
-void KinematicCentroidalMPC::AddKinematicsIntegrator() {
-  for (int knot_point = 0; knot_point < n_knot_points_ - 1; knot_point++) {
+void KinematicCentroidalMPC::AddKinematicsIntegrator(int knot_point) {
+  if(!is_last_knot(knot_point)) {
     // Integrate generalized velocities to get generalized positions
     auto constraint = std::make_shared<KinematicIntegratorConstraint<double>>(
         plant_, contexts_[knot_point].get(), contexts_[knot_point + 1].get(),
@@ -135,90 +137,82 @@ void KinematicCentroidalMPC::AddKinematicsIntegrator() {
   }
 }
 
-void KinematicCentroidalMPC::AddContactConstraints() {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    for (int contact_index = 0; contact_index < n_contact_points_;
-         contact_index++) {
-      // Make sure feet in stance are not moving and on the ground
-      if (contact_sequence_[knot_point][contact_index]) {
-        prog_->AddBoundingBoxConstraint(
-            Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
-            contact_vel_vars(knot_point, contact_index));
-        if (knot_point != 0) {
-          prog_->AddBoundingBoxConstraint(
-              0, 0, contact_pos_vars(knot_point, contact_index)[2]);
-        }
-      } else {
-        // Feet are above the ground
-        double lb = 0;
-        // Check if at least one of the time points before or after is also in
-        // 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
-            (!contact_sequence_[knot_point - 1][contact_index] or
-             !contact_sequence_[knot_point + 1][contact_index])) {
-          lb = swing_foot_minimum_height_;
-        }
+void KinematicCentroidalMPC::AddContactConstraints(int knot_point) {
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    // Make sure feet in stance are not moving and on the ground
+    if (contact_sequence_[knot_point][contact_index]) {
+      prog_->AddBoundingBoxConstraint(
+          Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
+          contact_vel_vars(knot_point, contact_index));
+      if (knot_point != 0) {
         prog_->AddBoundingBoxConstraint(
-            lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
+            0, 0, contact_pos_vars(knot_point, contact_index)[2]);
       }
+    } else {
+      // Feet are above the ground
+      double lb = 0;
+      // Check if at least one of the time points before or after is also in
+      // 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
+          (!contact_sequence_[knot_point - 1][contact_index] or
+           !contact_sequence_[knot_point + 1][contact_index])) {
+        lb = swing_foot_minimum_height_;
+      }
+      prog_->AddBoundingBoxConstraint(
+          lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
     }
   }
 }
 
-void KinematicCentroidalMPC::AddCentroidalKinematicConsistency() {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    // Ensure linear and angular momentum line up
-    auto centroidal_momentum =
-        std::make_shared<CentroidalMomentumConstraint<double>>(
-            plant_, contexts_[knot_point].get(), knot_point);
-    prog_->AddConstraint(centroidal_momentum,
-                         {state_vars(knot_point), com_pos_vars(knot_point),
-                          momentum_vars(knot_point)});
-    for (int contact_index = 0; contact_index < n_contact_points_;
-         contact_index++) {
-      // Ensure foot position line up with kinematics
-      auto foot_position_constraint = std::make_shared<
-          dairlib::multibody::KinematicPositionConstraint<double>>(
-          plant_, contact_sets_[contact_index], Eigen::Vector3d::Zero(),
-          Eigen::Vector3d::Zero(), full_constraint_relative_);
-      prog_->AddConstraint(foot_position_constraint,
-                           {state_vars(knot_point).head(n_q_),
-                            contact_pos_vars(knot_point, contact_index)});
-    }
-    // Constrain com position
-    auto com_position =
-        std::make_shared<CenterofMassPositionConstraint<double>>(
-            plant_, contexts_[knot_point].get(), knot_point);
-    prog_->AddConstraint(com_position,
-                         {com_pos_vars(knot_point), state_vars(knot_point)});
+void KinematicCentroidalMPC::AddCentroidalKinematicConsistency(int knot_point) {
+  // Ensure linear and angular momentum line up
+  auto centroidal_momentum =
+      std::make_shared<CentroidalMomentumConstraint<double>>(
+          plant_, contexts_[knot_point].get(), knot_point);
+  prog_->AddConstraint(centroidal_momentum,
+                       {state_vars(knot_point), com_pos_vars(knot_point),
+                        momentum_vars(knot_point)});
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    // Ensure foot position line up with kinematics
+    auto foot_position_constraint = std::make_shared<
+        dairlib::multibody::KinematicPositionConstraint<double>>(
+        plant_, contact_sets_[contact_index], Eigen::Vector3d::Zero(),
+        Eigen::Vector3d::Zero(), full_constraint_relative_);
+    prog_->AddConstraint(foot_position_constraint,
+                         {state_vars(knot_point).head(n_q_),
+                          contact_pos_vars(knot_point, contact_index)});
   }
+  // Constrain com position
+  auto com_position =
+      std::make_shared<CenterofMassPositionConstraint<double>>(
+          plant_, contexts_[knot_point].get(), knot_point);
+  prog_->AddConstraint(com_position,
+                       {com_pos_vars(knot_point), state_vars(knot_point)});
 }
 
-void KinematicCentroidalMPC::AddFrictionConeConstraints() {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    for (int contact_index = 0; contact_index < n_contact_points_;
-         contact_index++) {
-      auto force_constraints_vec =
-          contact_points_[contact_index].CreateLinearFrictionConstraints();
-      for (const auto& constraint : force_constraints_vec) {
-        prog_->AddConstraint(constraint,
-                             contact_force_vars(knot_point, contact_index));
-      }
+void KinematicCentroidalMPC::AddFrictionConeConstraints(int knot_point) {
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    auto force_constraints_vec =
+        contact_points_[contact_index].CreateLinearFrictionConstraints();
+    for (const auto& constraint : force_constraints_vec) {
+      prog_->AddConstraint(constraint,
+                           contact_force_vars(knot_point, contact_index));
     }
   }
 }
 
-void KinematicCentroidalMPC::AddFlightContactForceConstraints() {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    for (int contact_index = 0; contact_index < n_contact_points_;
-         contact_index++) {
-      // Feet in flight produce no force
-      if (!contact_sequence_[knot_point][contact_index]) {
-        prog_->AddBoundingBoxConstraint(
-            Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
-            contact_force_vars(knot_point, contact_index));
-      }
+void KinematicCentroidalMPC::AddFlightContactForceConstraints(int knot_point) {
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    // Feet in flight produce no force
+    if (!contact_sequence_[knot_point][contact_index]) {
+      prog_->AddBoundingBoxConstraint(
+          Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
+          contact_force_vars(knot_point, contact_index));
     }
   }
 }
@@ -238,12 +232,21 @@ drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::joint_vel_vars(
 
 void KinematicCentroidalMPC::Build(
     const drake::solvers::SolverOptions& solver_options) {
-  AddCentroidalDynamics();
-  AddKinematicsIntegrator();
-  AddContactConstraints();
-  AddCentroidalKinematicConsistency();
-  AddFrictionConeConstraints();
-  AddFlightContactForceConstraints();
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    switch (complexity_schedule_[knot_point]) {
+      case KINEMATIC_CENTROIDAL:
+        AddCentroidalDynamics(knot_point);
+        AddKinematicsIntegrator(knot_point);
+        AddContactConstraints(knot_point);
+        AddCentroidalKinematicConsistency(knot_point);
+        AddFrictionConeConstraints(knot_point);
+        AddFlightContactForceConstraints(knot_point);
+        break;
+      case PLANAR_SLIP:
+        AddPlanarSlipConstraints(knot_point);
+        break;
+    }
+  }
   AddCosts();
   prog_->SetSolverOptions(solver_options);
 }
@@ -261,36 +264,44 @@ void KinematicCentroidalMPC::AddCosts() {
     const double collocation_gain =
         (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
     double t = dt_ * knot_point;
-    if (q_ref_traj_) {
-      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_q_,
-                                   q_ref_traj_->value(t),
-                                   state_vars(knot_point).head(n_q_));
-    }
-    if (v_ref_traj_) {
-      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_v_,
-                                   v_ref_traj_->value(t),
-                                   state_vars(knot_point).tail(n_v_));
-    }
-    if (com_ref_traj_) {
-      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_com_,
-                                   com_ref_traj_->value(t),
-                                   com_pos_vars(knot_point));
-    }
-    if (mom_ref_traj_) {
-      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_mom_,
-                                   mom_ref_traj_->value(t),
-                                   momentum_vars(knot_point));
-    }
-    if (contact_ref_traj_) {
-      prog_->AddQuadraticErrorCost(
-          collocation_gain * terminal_gain * Q_contact_,
-          contact_ref_traj_->value(t),
-          {contact_pos_[knot_point], contact_vel_[knot_point]});
-    }
-    if (force_ref_traj_) {
-      prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_force_,
-                                   force_ref_traj_->value(t),
-                                   contact_force_[knot_point]);
+
+    switch (complexity_schedule_[knot_point]) {
+      case KINEMATIC_CENTROIDAL:
+        if (q_ref_traj_) {
+          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_q_,
+                                       q_ref_traj_->value(t),
+                                       state_vars(knot_point).head(n_q_));
+        }
+        if (v_ref_traj_) {
+          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_v_,
+                                       v_ref_traj_->value(t),
+                                       state_vars(knot_point).tail(n_v_));
+        }
+        if (com_ref_traj_) {
+          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_com_,
+                                       com_ref_traj_->value(t),
+                                       com_pos_vars(knot_point));
+        }
+        if (mom_ref_traj_) {
+          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_mom_,
+                                       mom_ref_traj_->value(t),
+                                       momentum_vars(knot_point));
+        }
+        if (contact_ref_traj_) {
+          prog_->AddQuadraticErrorCost(
+              collocation_gain * terminal_gain * Q_contact_,
+              contact_ref_traj_->value(t),
+              {contact_pos_[knot_point], contact_vel_[knot_point]});
+        }
+        if (force_ref_traj_) {
+          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_force_,
+                                       force_ref_traj_->value(t),
+                                       contact_force_[knot_point]);
+        }
+        break;
+      case PLANAR_SLIP:
+        AddPlanarSlipCost(t, terminal_gain * collocation_gain);
+        break;
     }
   }
 }
@@ -576,7 +587,7 @@ void KinematicCentroidalMPC::SetModeSequence(
 void KinematicCentroidalMPC::SetModeSequence(
     const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    for (int contact_index = 0; contact_index < 4; contact_index++) {
+    for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
       contact_sequence_[knot_point][contact_index] =
           contact_sequence.value(dt_ * knot_point).coeff(contact_index);
     }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 793f04d6ab..d2809f3051 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -15,6 +15,11 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/systems/framework/context.h"
 
+enum Complexity{
+  KINEMATIC_CENTROIDAL,
+  PLANAR_SLIP
+};
+
 /*!
  * @brief Class for solving nonlinear kinematic centroidal mpc. Implementation
  * is based on Dai, Hongkai, Andres Valenzuela, and Russ Tedrake. “Whole-Body
@@ -302,55 +307,59 @@ class KinematicCentroidalMPC {
    * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point`
    * is `contact_index` active
    */
-  void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
+  virtual void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
 
   /*!
    * @brief Set the mode sequence via a trajectory. The value of the trajectory
    * at each time, cast to a bool is if a contact point is active or not
    * @param contact_sequence
    */
-  void SetModeSequence(
+  virtual void SetModeSequence(
       const drake::trajectories::PiecewisePolynomial<double>& contact_sequence);
 
   void AddInitialStateConstraint(const Eigen::VectorXd& state);
 
   const drake::multibody::MultibodyPlant<double>& Plant() { return plant_; };
 
- private:
+  void SetComplexitySchedule(const std::vector<Complexity>& complexity_schedule){complexity_schedule_ = complexity_schedule;};
+ protected:
   /*!
    * @brief Adds dynamics for centroidal state
    */
-  void AddCentroidalDynamics();
+  void AddCentroidalDynamics(int knot_point);
 
   /*!
    * @brief Enforces zero force for feet in flight
    */
-  void AddFlightContactForceConstraints();
+  void AddFlightContactForceConstraints(int knot_point);
 
   /*!
    * @brief Enforce dynamics for kinematics and location of the contacts
    */
-  void AddKinematicsIntegrator();
+  void AddKinematicsIntegrator(int knot_point);
 
   /*!
    * @brief Feet that in stance are not moving and on the ground, feet in the
    * air are above the ground
    */
-  void AddContactConstraints();
+  void AddContactConstraints(int knot_point);
 
   /*!
    * @brief Ensures that contact point for feet line up with kinematics, and
    * centroidal state lines up with kinematic state
    */
-  void AddCentroidalKinematicConsistency();
+  void AddCentroidalKinematicConsistency(int knot_point);
 
   /*!
    * @brief Ensures feet are not pulling on the ground
    */
-  void AddFrictionConeConstraints();
+  void AddFrictionConeConstraints(int knot_point);
 
   //  void AddTorqueLimits();
 
+  virtual void AddPlanarSlipConstraints(int knot_point){ DRAKE_DEMAND(false);};
+
+  virtual void AddPlanarSlipCost(double t, double terminal_gain){ DRAKE_DEMAND(false);};
   /*!
    * @brief Add costs from internally stored variables
    */
@@ -424,4 +433,6 @@ class KinematicCentroidalMPC {
   // saving and publishing solutions
   std::unique_ptr<drake::lcm::DrakeLcm> lcm_;
   dairlib::LcmTrajectory lcm_trajectory_;
+
+  std::vector<Complexity> complexity_schedule_;
 };

From cb1a29c1a0bb7baa62a3bac8deffddbe87d3b57b Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Mon, 14 Nov 2022 15:06:25 -0500
Subject: [PATCH 42/76] Started to work on fixed complexity

---
 .../kinematic_centroidal_mpc/BUILD.bazel      |   1 +
 .../cassie_kcmpc_trajopt.cc                   |   7 +-
 .../cassie_kinematic_centroidal_mpc.cc        | 117 +++++++++++++++++-
 .../cassie_kinematic_centroidal_mpc.h         |  66 +++++++++-
 .../simple_models/BUILD.bazel                 |   1 +
 .../simple_models/planar_slip_lifting_test.cc |   6 +-
 .../kinematic_centroidal_mpc.cc               |  21 +++-
 .../kinematic_centroidal_mpc.h                |   9 +-
 8 files changed, 211 insertions(+), 17 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
index a6355fd3ca..ad92c2d094 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
@@ -94,6 +94,7 @@ cc_library(
         "//common",
         "//systems/controllers/kinematic_centroidal_mpc",
         "//multibody:visualization_utils",
+        "//examples/Cassie/kinematic_centroidal_mpc/simple_models:planar_slip",
         "@drake//:drake_shared_library",
     ],
 )
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 03b930b7ed..16b279e2ba 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -87,14 +87,15 @@ int DoMain(int argc, char* argv[]) {
     time_points.push_back(time_points.back() + duration);
   }
 
+  Eigen::VectorXd reference_state = GenerateNominalStand(
+      plant, traj_params.com_height, traj_params.stance_width, false);
+
   // Create MPC and set gains
   CassieKinematicCentroidalMPC mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4);
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 2000, 0.6, traj_params.stance_width);
   mpc.SetGains(gains);
 
-  Eigen::VectorXd reference_state = GenerateNominalStand(
-      plant, traj_params.com_height, traj_params.stance_width, false);
   KcmpcReferenceGenerator generator(plant,
                                     reference_state.head(plant.num_positions()),
                                     mpc.CreateContactPoints(plant, 0));
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 977361c33b..803a914d44 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -2,6 +2,7 @@
 #include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/multibody/parsing/parser.h"
 #include "common/find_resource.h"
 #include "examples/Cassie/cassie_utils.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h"
 
 std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalMPC::CreateContactPoints(const drake::multibody::MultibodyPlant<
     double> &plant,
@@ -40,6 +41,27 @@ std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCent
   return {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval};
 }
 
+std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalMPC::CreateSlipContactPoints(const drake::multibody::MultibodyPlant<
+    double> &plant, double mu) {
+  auto left_toe_pair = dairlib::LeftToeFront(plant);
+  auto right_toe_pair = dairlib::RightToeFront(plant);
+  std::vector<int> active_inds{0, 1, 2};
+
+  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, {0,0,0}, left_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  left_slip_eval.set_frictional();
+  left_slip_eval.set_mu(mu);
+
+  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, {0,0,0}, right_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  right_slip_eval.set_frictional();
+  right_slip_eval.set_mu(mu);
+
+  return {left_slip_eval, right_slip_eval};
+}
+
 void CassieKinematicCentroidalMPC::AddLoopClosure() {
   loop_closure_evaluators.add_evaluator(&l_loop_evaluator_);
   loop_closure_evaluators.add_evaluator(&r_loop_evaluator_);
@@ -56,10 +78,50 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
 }
 
 void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
-  // TODO add planar slip constraints
+  for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
+    if(slip_contact_sequence_[knot_point][contact_index]){
+      // Foot isn't moving
+      prog_->AddBoundingBoxConstraint(
+          Eigen::VectorXd::Zero(2), Eigen::VectorXd::Zero(2),
+          slip_contact_vel_vars(knot_point, contact_index));
+      // Foot is on the ground
+      prog_->AddBoundingBoxConstraint(
+          slip_ground_offset_, slip_ground_offset_, slip_contact_pos_vars(knot_point, contact_index)[1]);
+
+      const auto com_to_foot = slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index);
+      prog_->AddConstraint(com_to_foot.norm() < r0_);
+    }else{
+      // Feet are above the ground
+      double lb = 0;
+      // Check if at least one of the time points before or after is also in
+      // 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
+          (!slip_contact_sequence_[knot_point - 1][contact_index] or
+              !slip_contact_sequence_[knot_point + 1][contact_index])) {
+        lb = swing_foot_minimum_height_;
+      }
+      lb +=slip_ground_offset_;
+      prog_->AddBoundingBoxConstraint(
+          lb, 10, slip_contact_pos_vars(knot_point, contact_index)[1]);
+
+    }
+  }
 }
-void CassieKinematicCentroidalMPC::AddPlanarSlipCost(double t, double terminal_gain) {
-  // TODO add planar slip costs
+
+void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double terminal_gain) {
+  const double t = dt_ * knot_point;
+  std::shared_ptr<QuadraticLiftedCost> lifting_cost(new QuadraticLiftedCost(lifters_[knot_point],
+                                                            {Q_com_, com_ref_traj_->value(t)},
+                                                            {Q_mom_, mom_ref_traj_->value(t)},
+                                                            {Q_contact_, contact_ref_traj_->value(t)},
+                                                            {Q_force_, force_ref_traj_->value(t)},
+                                                            {Q_q_,q_ref_traj_->value(t)},
+                                                            {Q_v_,v_ref_traj_->value(t)},
+                                                            terminal_gain,
+                                                            slip_contact_points_.size(),
+                                                            knot_point));
+  prog_->AddCost(lifting_cost, {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point]});
 }
 
 void CassieKinematicCentroidalMPC::SetModeSequence(const std::vector<std::vector<bool>> &contact_sequence) {
@@ -72,7 +134,54 @@ void CassieKinematicCentroidalMPC::SetModeSequence(const drake::trajectories::Pi
 }
 
 void CassieKinematicCentroidalMPC::MapModeSequence() {
-  for(int knot_point = 0; knot_point < num_knot_points(); knot_point ++){
+  for (int knot_point = 0; knot_point < num_knot_points(); knot_point++) {
     slip_contact_sequence_[knot_point] = complex_mode_to_slip_mode_.at(contact_sequence_[knot_point]);
   }
 }
+void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
+  auto reduction_constraint =
+      std::make_shared<PlanarSlipReductionConstraint<double>>(
+          plant_, contexts_[knot_point].get(), slip_contact_points_,6 + 3 + 3 * 3 * n_contact_points_ + n_q_ + n_v_,knot_point);
+  prog_->AddConstraint(reduction_constraint,
+                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],
+                        com_pos_vars(knot_point), momentum_vars(knot_point), contact_pos_[knot_point], contact_vel_[knot_point], contact_force_[knot_point],
+                        state_vars(knot_point)});
+}
+
+void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
+  auto lifting_constraint =
+      std::make_shared<PlanarSlipLiftingConstraint>(
+          plant_, lifters_[knot_point], 2, n_contact_points_,knot_point);
+  prog_->AddConstraint(lifting_constraint,
+                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],
+                        com_pos_vars(knot_point), momentum_vars(knot_point), contact_pos_[knot_point], contact_vel_[knot_point], contact_force_[knot_point],
+                        state_vars(knot_point)});
+}
+
+void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
+  if(!is_last_knot(knot_point)) {
+    auto slip_com_dynamics =
+        std::make_shared<PlanarSlipDynamicsConstraint<double>>(
+            r0_, k_, m_, 2, slip_contact_sequence_[knot_point], dt_, knot_point);
+
+    prog_->AddConstraint(slip_com_dynamics,
+                         {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],
+                          slip_com_vars_[knot_point+1], slip_vel_vars_[knot_point+1], slip_contact_pos_vars_[knot_point+1]});
+    prog_->AddConstraint(
+        slip_contact_pos_vars_[knot_point + 1] ==
+            slip_contact_pos_vars_[knot_point] +
+                0.5 * dt_ *
+                    (slip_contact_vel_vars_[knot_point] +
+                        slip_contact_vel_vars_[knot_point + 1]));
+
+  }
+}
+
+drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_pos_vars(int knot_point_index,
+                                                                                            int slip_foot_index) {
+  return slip_contact_pos_vars_[knot_point_index].segment(2 * slip_foot_index, slip_foot_index);
+}
+drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_vel_vars(int knot_point_index,
+                                                                                            int slip_foot_index) {
+  return slip_contact_vel_vars_[knot_point_index].segment(2 * slip_foot_index, slip_foot_index);
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 1cf32636f7..cb46d80780 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -3,26 +3,56 @@
 #include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "examples/Cassie/cassie_utils.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
 
 /*!
  * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop closure, joint limits, and cassie contact points
  */
 class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
  public:
-  CassieKinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points, double dt, double mu) :
+  CassieKinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double>& plant,
+                               int n_knot_points,
+                               double dt,
+                               double mu,
+                               const drake::VectorX<double>& nominal_stand,
+                               double k = 1000,
+                               double r0 = 0.5,
+                               double stance_width = 0.2) :
       KinematicCentroidalMPC(plant, n_knot_points, dt, CreateContactPoints(plant, mu)),
       l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
       r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
       loop_closure_evaluators(Plant()),
-      slip_contact_sequence_(n_knot_points){
+      slip_contact_sequence_(n_knot_points),
+      k_(k),
+      r0_(r0){
     AddPlantJointLimits(dairlib::JointNames());
     AddLoopClosure();
 
-    //TODO Add slip decision variables
-    //TODO Create lifter functions
+    slip_contact_points_ = CreateSlipContactPoints(plant,mu);
+    for (int knot = 0; knot < n_knot_points; knot++) {
+      slip_com_vars_.push_back(prog_->NewContinuousVariables(
+          2, "slip_com_" + std::to_string(knot)));
+      slip_vel_vars_.push_back(prog_->NewContinuousVariables(
+          2, "slip_vel_" + std::to_string(knot)));
+      slip_contact_pos_vars_.push_back(prog_->NewContinuousVariables(
+          2*2, "slip_contact_pos_" + std::to_string(knot)));
+      slip_contact_vel_vars_.push_back(prog_->NewContinuousVariables(
+          2*2, "slip_contact_vel_" + std::to_string(knot)));
+      lifters_.emplace_back(new PlanarSlipLifter(plant,
+                                                 contexts_[knot].get(),
+                                                 slip_contact_points_,
+                                                 CreateContactPoints(plant, mu),
+                                                 {{0, {0, 1}}, {1, {2, 3}}},
+                                                 nominal_stand,
+                                                 k,
+                                                 r0,
+                                                 {stance_width / 2, -stance_width / 2}));
+    }
     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(), stance_mode);
+
+    m_=plant_.CalcTotalMass(*contexts_[0]);
   }
 
   /*!
@@ -33,6 +63,8 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
    */
   std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
 
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateSlipContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
+
   /*!
  * @brief Set the mode sequence
  * @param contact_sequence vector where
@@ -49,12 +81,22 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   void SetModeSequence(
       const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) override;
 
+  drake::solvers::VectorXDecisionVariable slip_contact_pos_vars(int knot_point_index, int slip_foot_index);
+
+  drake::solvers::VectorXDecisionVariable slip_contact_vel_vars(int knot_point_index, int slip_foot_index);
+
  private:
   void MapModeSequence();
 
   void AddPlanarSlipConstraints(int knot_point) override;
 
-  void AddPlanarSlipCost(double t, double terminal_gain) override;
+  void AddPlanarSlipCost(int knot_point, double terminal_gain) override;
+
+  void AddSlipReductionConstraint(int knot_point) override;
+
+  void AddSlipLiftingConstraint(int knot_point)override;
+
+  void AddSlipDynamics(int knot_point)override;
 
   /*!
    * @brief Adds loop closure constraints to the mpc
@@ -66,6 +108,20 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   dairlib::multibody::KinematicEvaluatorSet<double> loop_closure_evaluators;
 
   std::vector<std::vector<bool>> slip_contact_sequence_;
+  double k_;
+  double r0_;
+  double m_;
+
+  const double slip_ground_offset_ = 0.0562;
+
+  std::vector<drake::solvers::VectorXDecisionVariable> slip_com_vars_;
+  std::vector<drake::solvers::VectorXDecisionVariable> slip_vel_vars_;
+  std::vector<drake::solvers::VectorXDecisionVariable> slip_contact_pos_vars_;
+  std::vector<drake::solvers::VectorXDecisionVariable> slip_contact_vel_vars_;
+
+  std::vector<std::shared_ptr<PlanarSlipLifter>> lifters_;
+
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
 
   std::map<std::vector<bool>, std::vector<bool>> complex_mode_to_slip_mode_{{{true, true, true, true},{true, true}},
                                                                             {{true, true, false, false},{true, false}},
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
index 7ffd709d0d..0547e75e96 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
@@ -11,6 +11,7 @@ cc_library(
         "//solvers:constraints",
         "//multibody/kinematic:kinematic",
         "//multibody/kinematic:constraints",
+        "//solvers:nonlinear_cost",
         "@drake//:drake_shared_library",
     ],
 )
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index 309f8318ae..0ecccb3871 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -75,19 +75,21 @@ int main(int argc, char* argv[]) {
       plant, {0,0,0}, right_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
 
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
+  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
   PlanarSlipLifter lifter(plant,
                           context.get(),
                           {left_slip_eval, right_slip_eval},
                           {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
                           {{0, {0, 1}}, {1, {2, 3}}},
-                          reference_state.tail(plant.num_positions()),
+                          reference_state.head(plant.num_positions()),
                           2000,
                           0.5,
                           {0.1, -0.1});
 
   Eigen::Vector2d slip_com = {0.2,0.7};
   Eigen::Vector2d slip_vel = {0.1,0.3};
-  Eigen::Vector4d slip_feet = {0.2,0.1,0.2,0.14};
+  Eigen::Vector4d slip_feet = {0.2,0.0,0.2,0.05};
   Eigen::Vector4d slip_foot_vel = {0.11,0.12,0.15,0.18};
 
   Eigen::VectorXd slip_state(2+2+4+4);
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 6640fdc67e..0ca68a6ffd 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -235,15 +235,32 @@ void KinematicCentroidalMPC::Build(
   for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
     switch (complexity_schedule_[knot_point]) {
       case KINEMATIC_CENTROIDAL:
-        AddCentroidalDynamics(knot_point);
-        AddKinematicsIntegrator(knot_point);
+        //Add complex constraints
         AddContactConstraints(knot_point);
         AddCentroidalKinematicConsistency(knot_point);
         AddFrictionConeConstraints(knot_point);
         AddFlightContactForceConstraints(knot_point);
+        //Add complex dynamics
+        AddCentroidalDynamics(knot_point);
+        AddKinematicsIntegrator(knot_point);
+        if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
+          AddSlipReductionConstraint(knot_point + 1);
+        }
         break;
       case PLANAR_SLIP:
         AddPlanarSlipConstraints(knot_point);
+        if(!is_last_knot(knot_point)){
+          switch (complexity_schedule_[knot_point+1]) {
+            case KINEMATIC_CENTROIDAL:
+              AddCentroidalDynamics(knot_point);
+              AddKinematicsIntegrator(knot_point);
+              AddSlipLiftingConstraint(knot_point);
+              break;
+            case PLANAR_SLIP:
+              AddSlipDynamics(knot_point);
+              break;
+          }
+        }
         break;
     }
   }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index d2809f3051..2206e70b54 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -359,7 +359,14 @@ class KinematicCentroidalMPC {
 
   virtual void AddPlanarSlipConstraints(int knot_point){ DRAKE_DEMAND(false);};
 
-  virtual void AddPlanarSlipCost(double t, double terminal_gain){ DRAKE_DEMAND(false);};
+  virtual void AddPlanarSlipCost(int knot_point, double terminal_gain){ DRAKE_DEMAND(false);};
+
+  virtual void AddSlipReductionConstraint(int knot_point){ DRAKE_DEMAND(false);};
+
+  virtual void AddSlipLiftingConstraint(int knot_point){ DRAKE_DEMAND(false);};
+
+  virtual void AddSlipDynamics(int knot_point){ DRAKE_DEMAND(false);};
+
   /*!
    * @brief Add costs from internally stored variables
    */

From ad3a2323a25ed0f770d9ff3d1011f633a4549e59 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 15 Nov 2022 12:32:50 -0500
Subject: [PATCH 43/76] Committing some  code

---
 .../kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc       | 7 +++++++
 .../cassie_kinematic_centroidal_mpc.cc                     | 7 +++----
 .../kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc   | 2 +-
 3 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 16b279e2ba..79a36d8091 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -96,6 +96,13 @@ int DoMain(int argc, char* argv[]) {
       time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 2000, 0.6, traj_params.stance_width);
   mpc.SetGains(gains);
 
+  std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
+  std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::PLANAR_SLIP);
+  for(int i = 0; i < 25; i++){
+    complexity_schedule.push_back(Complexity::KINEMATIC_CENTROIDAL);
+  }
+  mpc.SetComplexitySchedule(complexity_schedule);
+
   KcmpcReferenceGenerator generator(plant,
                                     reference_state.head(plant.num_positions()),
                                     mpc.CreateContactPoints(plant, 0));
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 803a914d44..fcddc01f23 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -88,8 +88,7 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
       prog_->AddBoundingBoxConstraint(
           slip_ground_offset_, slip_ground_offset_, slip_contact_pos_vars(knot_point, contact_index)[1]);
 
-      const auto com_to_foot = slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index);
-      prog_->AddConstraint(com_to_foot.norm() < r0_);
+      prog_->AddConstraint((slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() <= r0_);
     }else{
       // Feet are above the ground
       double lb = 0;
@@ -179,9 +178,9 @@ void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
 
 drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_pos_vars(int knot_point_index,
                                                                                             int slip_foot_index) {
-  return slip_contact_pos_vars_[knot_point_index].segment(2 * slip_foot_index, slip_foot_index);
+  return slip_contact_pos_vars_[knot_point_index].segment(2 * slip_foot_index, 2);
 }
 drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_vel_vars(int knot_point_index,
                                                                                             int slip_foot_index) {
-  return slip_contact_vel_vars_[knot_point_index].segment(2 * slip_foot_index, slip_foot_index);
+  return slip_contact_vel_vars_[knot_point_index].segment(2 * slip_foot_index, 2);
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 0ca68a6ffd..2c985ab901 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -317,7 +317,7 @@ void KinematicCentroidalMPC::AddCosts() {
         }
         break;
       case PLANAR_SLIP:
-        AddPlanarSlipCost(t, terminal_gain * collocation_gain);
+        AddPlanarSlipCost(knot_point, terminal_gain * collocation_gain);
         break;
     }
   }

From 8d843671b3a3b714c7936a951cf4edea9b407267 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 15 Nov 2022 14:02:34 -0500
Subject: [PATCH 44/76] Added momentum reference and cost

---
 .../cassie_kcmpc_trajopt.cc                      |  7 +++++--
 .../kcmpc_reference_generator.cc                 |  3 +++
 .../kcmpc_reference_generator.h                  |  2 ++
 .../kinematic_centroidal_mpc.cc                  |  9 +++++++++
 .../kinematic_centroidal_mpc.h                   |  5 ++++-
 .../reference_generation_utils.cc                | 16 ++++++++++++++++
 .../reference_generation_utils.h                 |  4 ++++
 7 files changed, 43 insertions(+), 3 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 79a36d8091..52a03e40e4 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -101,7 +101,7 @@ int DoMain(int argc, char* argv[]) {
   for(int i = 0; i < 25; i++){
     complexity_schedule.push_back(Complexity::KINEMATIC_CENTROIDAL);
   }
-  mpc.SetComplexitySchedule(complexity_schedule);
+//  mpc.SetComplexitySchedule(complexity_schedule);
 
   KcmpcReferenceGenerator generator(plant,
                                     reference_state.head(plant.num_positions()),
@@ -129,7 +129,9 @@ int DoMain(int argc, char* argv[]) {
   mpc.AddContactTrackingReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.contact_traj_));
-  mpc.AddConstantMomentumReference(Eigen::VectorXd::Zero(6));
+  mpc.AddMomentumReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          generator.momentum_trajectory_));
   mpc.SetModeSequence(generator.contact_sequence_);
 
   // Set initial guess
@@ -138,6 +140,7 @@ int DoMain(int argc, char* argv[]) {
   mpc.SetComPositionGuess(generator.com_trajectory_);
   mpc.SetContactGuess(generator.contact_traj_);
   mpc.SetForceGuess(generator.grf_traj_);
+  mpc.SetMomentumGuess(generator.momentum_trajectory_);
 
   {
     drake::solvers::SolverOptions options;
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
index 800d7f2a9b..38b8d28012 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
@@ -26,6 +26,9 @@ void KcmpcReferenceGenerator::Build() {
 void KcmpcReferenceGenerator::Build(const Eigen::Vector3d& com) {
   com_trajectory_ = GenerateComTrajectory(com, com_vel_knot_points_.samples,
                                           com_vel_knot_points_.times);
+  momentum_trajectory_ = GenerateMomentumTrajectory(com_vel_knot_points_.samples,
+                                          com_vel_knot_points_.times,
+                                          m_);
   q_trajectory_ = GenerateGeneralizedPosTrajectory(nominal_stand_, p_ScmBase_W_,
                                                    com_trajectory_, 4);
   v_trajectory_ = GenerateGeneralizedVelTrajectory(com_trajectory_,
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
index 659a4f3c1e..2473b84271 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
@@ -57,6 +57,8 @@ class KcmpcReferenceGenerator{
   drake::trajectories::PiecewisePolynomial<double> contact_sequence_;
   drake::trajectories::PiecewisePolynomial<double> grf_traj_;
   drake::trajectories::PiecewisePolynomial<double> contact_traj_;
+  drake::trajectories::PiecewisePolynomial<double> momentum_trajectory_;
+
 
  private:
   const drake::multibody::MultibodyPlant<double>& plant_;
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 2c985ab901..00561ea018 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -597,6 +597,15 @@ void KinematicCentroidalMPC::SetForceGuess(
   }
 }
 
+void KinematicCentroidalMPC::SetMomentumGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory) {
+  DRAKE_DEMAND(momentum_trajectory.rows() == 6);
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(mom_vars_[knot_point],
+                           momentum_trajectory.value(dt_ * knot_point));
+  }
+}
+
 void KinematicCentroidalMPC::SetModeSequence(
     const std::vector<std::vector<bool>>& contact_sequence) {
   contact_sequence_ = contact_sequence;
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 2206e70b54..9ee80c3077 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -260,7 +260,10 @@ class KinematicCentroidalMPC {
   void SetForceGuess(
       const drake::trajectories::PiecewisePolynomial<double>& force_trajectory);
 
-  void CreateVisualizationCallback(std::string model_file, double alpha,
+  void SetMomentumGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory);
+
+    void CreateVisualizationCallback(std::string model_file, double alpha,
                                    std::string weld_frame_to_world = "");
 
   /*!
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
index ad6bf7bc7e..2271de548b 100644
--- a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
@@ -1,3 +1,4 @@
+#include <iostream>
 #include "reference_generation_utils.h"
 
 #include "multibody/multibody_utils.h"
@@ -19,6 +20,21 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(
       time_points, samples);
 }
 
+drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(const std::vector<Eigen::Vector3d> &vel_ewrt_w,
+                                                                            const std::vector<double> &time_points,
+                                                                            double m) {
+  DRAKE_DEMAND(vel_ewrt_w.size() == (time_points.size() - 1));
+  auto n_points = time_points.size();
+  std::vector<drake::MatrixX<double>> samples(n_points);
+  for (int i = 0; i < n_points; i++) {
+    samples[i] = drake::Vector6d();
+    samples[i] << drake::Vector3<double>::Zero(3), vel_ewrt_w[i] * m;
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(
+      time_points, samples);
+}
+
+
 drake::trajectories::PiecewisePolynomial<double>
 GenerateGeneralizedPosTrajectory(
     const Eigen::VectorXd& nominal_stand, const Eigen::Vector3d& p_ScmBase_W,
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
index 46193269e5..81163d3c21 100644
--- a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
+++ b/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
@@ -16,6 +16,10 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eig
                                                                        const std::vector<Eigen::Vector3d>& vel_ewrt_w,
                                                                        const std::vector<double>& time_points);
 
+drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(const std::vector<Eigen::Vector3d> &vel_ewrt_w,
+                                                                            const std::vector<double> &time_points,
+                                                                            double m);
+
 /*!
  * @brief Constructs a trajectory for the generalized positions of a constant joint state and floating base position from com trajectory
  * @param nominal_stand [nq] generalized state

From 1731e6fb498a38e625db1893b142ef560313df46 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 15 Nov 2022 17:10:48 -0500
Subject: [PATCH 45/76] Fixed bug in lifting function

---
 .../cassie_kcmpc_trajopt.cc                   |  2 +-
 .../cassie_kinematic_centroidal_mpc.cc        | 30 +++++++++++++++++++
 .../cassie_kinematic_centroidal_mpc.h         | 12 ++++++++
 .../simple_models/planar_slip_lifter.cc       | 14 +++++----
 .../simple_models/planar_slip_lifter.h        |  2 +-
 .../kinematic_centroidal_mpc.h                | 10 +++----
 6 files changed, 57 insertions(+), 13 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 52a03e40e4..c6ecf11935 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -101,7 +101,7 @@ int DoMain(int argc, char* argv[]) {
   for(int i = 0; i < 25; i++){
     complexity_schedule.push_back(Complexity::KINEMATIC_CENTROIDAL);
   }
-//  mpc.SetComplexitySchedule(complexity_schedule);
+  mpc.SetComplexitySchedule(complexity_schedule);
 
   KcmpcReferenceGenerator generator(plant,
                                     reference_state.head(plant.num_positions()),
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index fcddc01f23..66315cf8db 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -184,3 +184,33 @@ drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_conta
                                                                                             int slip_foot_index) {
   return slip_contact_vel_vars_[knot_point_index].segment(2 * slip_foot_index, 2);
 }
+
+void CassieKinematicCentroidalMPC::SetComPositionGuess(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],
+                           slip_index_.unaryExpr(com_trajectory.value(dt_ * knot_point)));
+  }
+  KinematicCentroidalMPC::SetComPositionGuess(com_trajectory);
+}
+
+void CassieKinematicCentroidalMPC::SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double> &q_traj,
+                                                      const drake::trajectories::PiecewisePolynomial<double> &v_traj) {
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    dairlib::multibody::SetPositionsIfNew<double>(plant_, q_traj.value(dt_*knot_point),contexts_[knot_point].get());
+    dairlib::multibody::SetVelocitiesIfNew<double>(plant_, v_traj.value(dt_*knot_point),contexts_[knot_point].get());
+    for(int contact = 0; contact < slip_contact_points_.size(); contact++){
+      prog_->SetInitialGuess(slip_contact_pos_vars(knot_point, contact),
+                             slip_index_.unaryExpr(slip_contact_points_[contact].EvalFull(*contexts_[knot_point])));
+      prog_->SetInitialGuess(slip_contact_vel_vars(knot_point, contact),
+                             slip_index_.unaryExpr(slip_contact_points_[contact].EvalFullTimeDerivative(*contexts_[knot_point])));
+    }
+  }
+  KinematicCentroidalMPC::SetRobotStateGuess(q_traj, v_traj);
+}
+void CassieKinematicCentroidalMPC::SetMomentumGuess(const drake::trajectories::PiecewisePolynomial<double> &momentum_trajectory) {
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(slip_vel_vars_[knot_point],
+                           slip_index_.unaryExpr(momentum_trajectory.value(dt_ * knot_point))/m_);
+  }
+  KinematicCentroidalMPC::SetMomentumGuess(momentum_trajectory);
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index cb46d80780..3ae350ee3f 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -85,6 +85,16 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
 
   drake::solvers::VectorXDecisionVariable slip_contact_vel_vars(int knot_point_index, int slip_foot_index);
 
+  void SetComPositionGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& com_trajectory) override;
+
+  void SetRobotStateGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& q_traj,
+      const drake::trajectories::PiecewisePolynomial<double>& v_traj) override;
+
+  void SetMomentumGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory) override;
+
  private:
   void MapModeSequence();
 
@@ -127,5 +137,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
                                                                             {{true, true, false, false},{true, false}},
                                                                             {{false, false, true, true},{false, true}},
                                                                             {{false, false, false, false},{false, false}}};
+
+  const Eigen::Vector2i slip_index_{0,2};
 };
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index 2c2774344a..5ce17ee39e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -71,26 +71,28 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Ve
                                                                const drake::VectorX<double> &slip_feet_positions) const {
   DRAKE_DEMAND(slip_feet_positions.size() == 2*slip_contact_points_.size());
   //Add com position constraint
-  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(com_position, com_position, com_vars_);
+  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), com_vars_);
   //Add feet position constraint
   std::vector<drake::solvers::Binding<drake::solvers::Constraint>> foot_constraints;
   for(int i = 0; i < slip_contact_points_.size(); i++){
     const drake::Vector3<double> slip_spatial_foot_pos =  {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]};
     foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), drake::VectorX<double>::Zero(3), plant_.world_frame(),
                                                          std::nullopt,
-                                                         slip_spatial_foot_pos,
-                                                         slip_spatial_foot_pos));
+                                                         slip_spatial_foot_pos - com_position,
+                                                         slip_spatial_foot_pos - com_position));
   }
   //Set initial guess for com
-  ik_.get_mutable_prog()->SetInitialGuess(com_vars_,com_position);
+  ik_.get_mutable_prog()->SetInitialGuess(com_vars_,Eigen::VectorXd::Zero(3));
   //Solve
   const auto result = drake::solvers::Solve(ik_.prog());
   const auto q_sol = result.GetSolution(ik_.q());
   //Normalize quaternion
   drake::VectorX<double> q_sol_normd(n_q_);
   q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q_ - 4);
+
+  q_sol_normd.segment(4,3) = q_sol_normd.segment(4,3) + com_position;
   //Set initial guess for next time
-  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
+//  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
   //Remove added constraints
   ik_.get_mutable_prog()->RemoveConstraint(com_constraint);
   for(const auto& constraint : foot_constraints){
@@ -189,7 +191,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
     const auto dir = (com_pos - average_pos).normalized();
     // Distribute grf magnitude across all of the complex contact points
     for(const auto& complex_index : complex_feet_list){
-      rv.segment(3 * complex_index, complex_index) = dir * slip_grf_mag/complex_feet_list.size();
+      rv.segment(3 * complex_index, 3) = dir * slip_grf_mag/complex_feet_list.size();
     }
   }
   return rv;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
index c113f3c2a1..88785fa879 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -78,6 +78,6 @@ class PlanarSlipLifter {
   drake::solvers::VectorXDecisionVariable com_vars_;
 
   const int kSLIP_DIM = 2;
-  const Eigen::VectorXi slip_index_ = {0,2};
+  const Eigen::Vector2i slip_index_ = {0,2};
 };
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 9ee80c3077..74b7ae531a 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -245,22 +245,22 @@ class KinematicCentroidalMPC {
       const drake::trajectories::PiecewisePolynomial<double>& state_trajectory);
 
   // TODO remove once drake has trajectory stacking
-  void SetRobotStateGuess(
+  virtual void SetRobotStateGuess(
       const drake::trajectories::PiecewisePolynomial<double>& q_traj,
       const drake::trajectories::PiecewisePolynomial<double>& v_traj);
 
   void SetComPositionGuess(const drake::Vector3<double>& state);
 
-  void SetComPositionGuess(
+  virtual void SetComPositionGuess(
       const drake::trajectories::PiecewisePolynomial<double>& com_trajectory);
 
-  void SetContactGuess(const drake::trajectories::PiecewisePolynomial<double>&
+  virtual void SetContactGuess(const drake::trajectories::PiecewisePolynomial<double>&
                            contact_trajectory);
 
-  void SetForceGuess(
+  virtual void SetForceGuess(
       const drake::trajectories::PiecewisePolynomial<double>& force_trajectory);
 
-  void SetMomentumGuess(
+  virtual void SetMomentumGuess(
       const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory);
 
     void CreateVisualizationCallback(std::string model_file, double alpha,

From da2d982e80657591978a62b6e63036e16e9eab82 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 15 Nov 2022 18:40:25 -0500
Subject: [PATCH 46/76] Sort of works, gonna switch slip foot to just the toe

---
 .../kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc | 12 ++++++------
 .../cassie_kinematic_centroidal_mpc.cc               | 12 +++++++++++-
 .../cassie_kinematic_centroidal_mpc.h                |  4 +++-
 .../simple_models/planar_slip_constraints.cc         |  2 +-
 .../kinematic_centroidal_mpc.cc                      |  9 ++++++++-
 .../kinematic_centroidal_mpc.h                       |  2 ++
 6 files changed, 31 insertions(+), 10 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index c6ecf11935..2e2438edb4 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -93,13 +93,13 @@ int DoMain(int argc, char* argv[]) {
   // Create MPC and set gains
   CassieKinematicCentroidalMPC mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 2000, 0.6, traj_params.stance_width);
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 8000, 0.8, traj_params.stance_width);
   mpc.SetGains(gains);
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::PLANAR_SLIP);
   for(int i = 0; i < 25; i++){
-    complexity_schedule.push_back(Complexity::KINEMATIC_CENTROIDAL);
+    complexity_schedule[i] = Complexity::KINEMATIC_CENTROIDAL;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
 
@@ -167,9 +167,9 @@ int DoMain(int argc, char* argv[]) {
     mpc.Build(options);
   }
 
-  //  double alpha = .2;
-  //  mpc.CreateVisualizationCallback(
-  //      "examples/Cassie/urdf/cassie_fixed_springs.urdf", alpha);
+    double alpha = .2;
+    mpc.CreateVisualizationCallback(
+        "examples/Cassie/urdf/cassie_fixed_springs.urdf", alpha);
 
   std::cout << "Solving optimization\n\n";
   const auto pp_xtraj = mpc.Solve();
@@ -204,7 +204,7 @@ int DoMain(int argc, char* argv[]) {
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(1.0);
+    simulator.set_target_realtime_rate(0.2);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
   }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 66315cf8db..3768aa46bb 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -98,7 +98,7 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
       if (!is_first_knot(knot_point) and !is_last_knot(knot_point) and
           (!slip_contact_sequence_[knot_point - 1][contact_index] or
               !slip_contact_sequence_[knot_point + 1][contact_index])) {
-        lb = swing_foot_minimum_height_;
+        lb = swing_foot_minimum_height_+.1; //TODO figure out why this needs to be higher
       }
       lb +=slip_ground_offset_;
       prog_->AddBoundingBoxConstraint(
@@ -214,3 +214,13 @@ void CassieKinematicCentroidalMPC::SetMomentumGuess(const drake::trajectories::P
   }
   KinematicCentroidalMPC::SetMomentumGuess(momentum_trajectory);
 }
+drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_point) {
+  std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, 0)[1])<<" "<< result_->GetSolution(slip_contact_pos_vars(knot_point, 1)[1])<<std::endl;
+  drake::VectorX<double> slip_state(2 + 2 + 4 * slip_contact_points_.size());
+  slip_state<<result_->GetSolution(slip_com_vars_[knot_point]),
+      result_->GetSolution(slip_vel_vars_[knot_point]),
+      result_->GetSolution(slip_contact_pos_vars_[knot_point]),
+      result_->GetSolution(slip_contact_vel_vars_[knot_point]);
+
+ return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 3ae350ee3f..56d84ee501 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -108,6 +108,8 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
 
   void AddSlipDynamics(int knot_point)override;
 
+  drake::VectorX<double> LiftSlipSolution(int knot_point)override;
+
   /*!
    * @brief Adds loop closure constraints to the mpc
    */
@@ -122,7 +124,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   double r0_;
   double m_;
 
-  const double slip_ground_offset_ = 0.0562;
+  const double slip_ground_offset_ = 0;
 
   std::vector<drake::solvers::VectorXDecisionVariable> slip_com_vars_;
   std::vector<drake::solvers::VectorXDecisionVariable> slip_vel_vars_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index f226591930..350b47f64e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -147,7 +147,7 @@ void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
   drake::Vector<T, 4> xdot1 = CalcTimeDerivativesWithForce(com1, vel1,contact_pos1);
 
   // Predict state and return error
-  const auto x1Predict = x0 + 0.5 * dt_ * (x0 + x1);
+  const auto x1Predict = x0 + 0.5 * dt_ * (xdot0 + xdot1);
   *y = x1 - x1Predict;
 }
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 00561ea018..3f48c9de46 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -355,7 +355,14 @@ KinematicCentroidalMPC::Solve() {
   std::vector<drake::MatrixX<double>> states;
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     time_points.emplace_back(dt_ * knot_point);
-    states.emplace_back(result_->GetSolution(state_vars(knot_point)));
+    switch (complexity_schedule_[knot_point]) {
+      case KINEMATIC_CENTROIDAL:
+        states.emplace_back(result_->GetSolution(state_vars(knot_point)));
+        break;
+      case PLANAR_SLIP:
+        states.emplace_back(LiftSlipSolution(knot_point));
+        break;
+    }
   }
   return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(
       time_points, states);
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 74b7ae531a..4d722a6217 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -370,6 +370,8 @@ class KinematicCentroidalMPC {
 
   virtual void AddSlipDynamics(int knot_point){ DRAKE_DEMAND(false);};
 
+  virtual drake::VectorX<double> LiftSlipSolution(int knot_point){ DRAKE_DEMAND(false);};
+
   /*!
    * @brief Add costs from internally stored variables
    */

From a4f2b9523fd9e5ac7ddced8965b6cb9a87beda14 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 16 Nov 2022 19:55:10 -0500
Subject: [PATCH 47/76] Planar slip struggling with fixed timing

---
 .../cassie_kcmpc_trajopt.cc                   |  8 +++----
 .../cassie_kinematic_centroidal_mpc.cc        | 21 +++++++++---------
 .../kinematic_centroidal_mpc_gains.yaml       |  2 +-
 .../simple_models/planar_slip_constraints.cc  |  2 +-
 .../simple_models/planar_slip_lifter.cc       |  7 +++---
 .../simple_models/planar_slip_lifter.h        |  2 +-
 .../simple_models/planar_slip_lifting_test.cc | 22 +++++++++----------
 multibody/kinematic/world_point_evaluator.h   |  1 +
 .../kinematic_centroidal_mpc.cc               |  5 +++++
 9 files changed, 37 insertions(+), 33 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 2e2438edb4..7f58131311 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -93,13 +93,13 @@ int DoMain(int argc, char* argv[]) {
   // Create MPC and set gains
   CassieKinematicCentroidalMPC mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 8000, 0.8, traj_params.stance_width);
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 6000, traj_params.com_height, traj_params.stance_width);
   mpc.SetGains(gains);
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
-  std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::PLANAR_SLIP);
-  for(int i = 0; i < 25; i++){
-    complexity_schedule[i] = Complexity::KINEMATIC_CENTROIDAL;
+  std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
+  for(int i = 15; i < 25 ; i++){
+    complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 3768aa46bb..6bd6d4334e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -47,19 +47,19 @@ std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCent
   auto right_toe_pair = dairlib::RightToeFront(plant);
   std::vector<int> active_inds{0, 1, 2};
 
-  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, {0,0,0}, left_toe_pair.second,
+  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, left_toe_pair.first, left_toe_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  left_slip_eval.set_frictional();
-  left_slip_eval.set_mu(mu);
+  left_toe_eval.set_frictional();
+  left_toe_eval.set_mu(mu);
 
-  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, {0,0,0}, right_toe_pair.second,
+  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, right_toe_pair.first, right_toe_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  right_slip_eval.set_frictional();
-  right_slip_eval.set_mu(mu);
+  right_toe_eval.set_frictional();
+  right_toe_eval.set_mu(mu);
 
-  return {left_slip_eval, right_slip_eval};
+  return {left_toe_eval, right_toe_eval};
 }
 
 void CassieKinematicCentroidalMPC::AddLoopClosure() {
@@ -98,7 +98,7 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
       if (!is_first_knot(knot_point) and !is_last_knot(knot_point) and
           (!slip_contact_sequence_[knot_point - 1][contact_index] or
               !slip_contact_sequence_[knot_point + 1][contact_index])) {
-        lb = swing_foot_minimum_height_+.1; //TODO figure out why this needs to be higher
+        lb = swing_foot_minimum_height_;
       }
       lb +=slip_ground_offset_;
       prog_->AddBoundingBoxConstraint(
@@ -215,7 +215,6 @@ void CassieKinematicCentroidalMPC::SetMomentumGuess(const drake::trajectories::P
   KinematicCentroidalMPC::SetMomentumGuess(momentum_trajectory);
 }
 drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_point) {
-  std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, 0)[1])<<" "<< result_->GetSolution(slip_contact_pos_vars(knot_point, 1)[1])<<std::endl;
   drake::VectorX<double> slip_state(2 + 2 + 4 * slip_contact_points_.size());
   slip_state<<result_->GetSolution(slip_com_vars_[knot_point]),
       result_->GetSolution(slip_vel_vars_[knot_point]),
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
index af72e8e1c2..fdf924c7c8 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
@@ -31,6 +31,6 @@ contact_pos: [0.1, 16, 0.001]
 contact_vel: [0.02, 0.02, 0.02]
 contact_force: [0.0001, 0.0001, 0.0001]
 lin_momentum: [0.00001, 0.00001, 0.00001]
-ang_momentum: [0.0000001, 0.0000001, 0.0000001]
+ang_momentum: [0.0000000, 0.0000000, 0.0000000]
 
 swing_foot_minimum_height: 0.015
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index 350b47f64e..e295af6187 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -98,7 +98,7 @@ void PlanarSlipLiftingConstraint::EvaluateConstraint(const Eigen::Ref<const drak
                                                      drake::VectorX<double> *y) const {
   const auto& slip_state = x.head(slip_dim_);
   const auto& complex_state = x.tail(complex_dim_);
-  lifting_function_->Lift(slip_state, y);
+  *y = lifting_function_->Lift(slip_state);
   *y = *y - complex_state;
 }
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index 5ce17ee39e..daf49c946b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -64,7 +64,6 @@ PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double
           (ik_.q())(positions_map.at("ankle_joint_left"))+
           (ik_.q())(positions_map.at("toe_left")) ==
           -0.862002);
-
 }
 
 drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Vector3<double> &com_position,
@@ -76,7 +75,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Ve
   std::vector<drake::solvers::Binding<drake::solvers::Constraint>> foot_constraints;
   for(int i = 0; i < slip_contact_points_.size(); i++){
     const drake::Vector3<double> slip_spatial_foot_pos =  {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]};
-    foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), drake::VectorX<double>::Zero(3), plant_.world_frame(),
+    foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), slip_contact_points_[i].get_pt_A(), plant_.world_frame(),
                                                          std::nullopt,
                                                          slip_spatial_foot_pos - com_position,
                                                          slip_spatial_foot_pos - com_position));
@@ -174,7 +173,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
   // Loop through the slip feet
   for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
     // Calculate the slip grf
-    double r = (slip_feet_pos - slip_index_.unaryExpr(com_pos)).norm();
+    double r = (slip_feet_pos.segment(simple_index * 2, 2) - slip_index_.unaryExpr(com_pos)).norm();
     double slip_grf_mag = k_ * (r - r0_);
 
     // Find the average location for all of the complex contact points that make up the SLIP foot
@@ -182,7 +181,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
 
     const auto& complex_feet_list = simple_foot_index_to_complex_foot_index_.at(simple_index);
     for(const auto& complex_index : complex_feet_list){
-      average_pos = average_pos + complex_contact_point_pos.segment(3 * complex_index, complex_index);
+      average_pos = average_pos + complex_contact_point_pos.segment(3 * complex_index, 3);
     }
     average_pos = average_pos/complex_feet_list.size();
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
index 88785fa879..54d56b2b9e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -78,6 +78,6 @@ class PlanarSlipLifter {
   drake::solvers::VectorXDecisionVariable com_vars_;
 
   const int kSLIP_DIM = 2;
-  const Eigen::Vector2i slip_index_ = {0,2};
+  const Eigen::Vector2i slip_index_{0,2};
 };
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index 0ecccb3871..6baff37d23 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -67,19 +67,19 @@ int main(int argc, char* argv[]) {
       plant, right_heel_pair.first, right_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
 
-  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, {0,0,0}, left_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-
-  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, {0,0,0}, right_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+//  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+//      plant, {0,0,0}, left_heel_pair.second,
+//      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+//
+//  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+//      plant, {0,0,0}, right_heel_pair.second,
+//      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
 
   dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
-  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
+//  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
   PlanarSlipLifter lifter(plant,
                           context.get(),
-                          {left_slip_eval, right_slip_eval},
+                          {left_toe_eval, right_toe_eval},
                           {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
                           {{0, {0, 1}}, {1, {2, 3}}},
                           reference_state.head(plant.num_positions()),
@@ -89,7 +89,7 @@ int main(int argc, char* argv[]) {
 
   Eigen::Vector2d slip_com = {0.2,0.7};
   Eigen::Vector2d slip_vel = {0.1,0.3};
-  Eigen::Vector4d slip_feet = {0.2,0.0,0.2,0.05};
+  Eigen::Vector4d slip_feet = {0.0,0.0,0.0,0.00};
   Eigen::Vector4d slip_foot_vel = {0.11,0.12,0.15,0.18};
 
   Eigen::VectorXd slip_state(2+2+4+4);
@@ -109,7 +109,7 @@ int main(int argc, char* argv[]) {
   std::cout << "Solve time 2:" << elapsed.count() << std::endl;
 
   auto context_2 = plant.CreateDefaultContext();
-  auto constraint = PlanarSlipReductionConstraint<double>(plant, context_2.get(), {left_slip_eval, right_slip_eval}, complex_state.size(), 0);
+  auto constraint = PlanarSlipReductionConstraint<double>(plant, context_2.get(), {left_toe_eval, right_toe_eval}, complex_state.size(), 0);
 
   drake::VectorX<double> error(slip_state.size());
   drake::VectorX<double> input(slip_state.size() + complex_state.size());
diff --git a/multibody/kinematic/world_point_evaluator.h b/multibody/kinematic/world_point_evaluator.h
index 3daab08ac2..0421b363ff 100644
--- a/multibody/kinematic/world_point_evaluator.h
+++ b/multibody/kinematic/world_point_evaluator.h
@@ -94,6 +94,7 @@ class WorldPointEvaluator : public KinematicEvaluator<T> {
   void set_frictional() { is_frictional_ = true; };
 
   const drake::multibody::Frame<T> & get_frame() const {return frame_A_;};
+  const Eigen::Vector3d&  get_pt_A() const {return pt_A_;};
 
  private:
   const Eigen::Vector3d pt_A_;
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 3f48c9de46..024024397c 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -245,6 +245,11 @@ void KinematicCentroidalMPC::Build(
         AddKinematicsIntegrator(knot_point);
         if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
           AddSlipReductionConstraint(knot_point + 1);
+          //TODO why do I need these constraints here
+          AddContactConstraints(knot_point + 1);
+          AddCentroidalKinematicConsistency(knot_point + 1);
+          AddFrictionConeConstraints(knot_point + 1);
+          AddFlightContactForceConstraints(knot_point + 1);
         }
         break;
       case PLANAR_SLIP:

From d69b61f6425a4ad4f2e423c91775bf3b0b87a25a Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 1 Dec 2022 16:21:01 -0500
Subject: [PATCH 48/76] Switched to non planar slip

---
 .../cassie_kcmpc_trajopt.cc                   |   2 +-
 .../cassie_kinematic_centroidal_mpc.cc        |  75 ++++++---
 .../cassie_kinematic_centroidal_mpc.h         |  29 +++-
 .../simple_models/BUILD.bazel                 |   6 +-
 .../simple_models/planar_slip_constraints.cc  | 144 ++++++++++--------
 .../simple_models/planar_slip_constraints.h   |  46 ++++--
 .../simple_models/planar_slip_lifter.cc       |  58 ++++---
 .../simple_models/planar_slip_lifter.h        |   9 +-
 .../simple_models/planar_slip_lifting_test.cc |  28 ++--
 .../simple_models/planar_slip_reducer.cc      |  75 +++++++++
 .../simple_models/planar_slip_reducer.h       |  45 ++++++
 .../kinematic_centroidal_mpc.cc               |   2 +-
 12 files changed, 357 insertions(+), 162 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 7f58131311..561779a4a0 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -98,7 +98,7 @@ int DoMain(int argc, char* argv[]) {
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
-  for(int i = 15; i < 25 ; i++){
+  for(int i = 15; i <35 ; i++){
     complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 6bd6d4334e..319ab05d3b 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -78,18 +78,21 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
 }
 
 void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
+  Eigen::Vector2d force_bounds = {50.0, 50.0};
+  prog_->AddBoundingBoxConstraint(-force_bounds, force_bounds, slip_force_vars_[knot_point]);
   for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
     if(slip_contact_sequence_[knot_point][contact_index]){
       // Foot isn't moving
       prog_->AddBoundingBoxConstraint(
-          Eigen::VectorXd::Zero(2), Eigen::VectorXd::Zero(2),
+          Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
           slip_contact_vel_vars(knot_point, contact_index));
       // Foot is on the ground
       prog_->AddBoundingBoxConstraint(
-          slip_ground_offset_, slip_ground_offset_, slip_contact_pos_vars(knot_point, contact_index)[1]);
+          slip_ground_offset_, slip_ground_offset_, slip_contact_pos_vars(knot_point, contact_index)[2]);
 
-      prog_->AddConstraint((slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() <= r0_);
+      prog_->AddConstraint(slip_force_vars_[knot_point][contact_index] + k_ * ( r0_ - (slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() ) >= 0);
     }else{
+      prog_->AddBoundingBoxConstraint(0, 0, slip_force_vars_[knot_point][contact_index]);
       // Feet are above the ground
       double lb = 0;
       // Check if at least one of the time points before or after is also in
@@ -102,7 +105,7 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
       }
       lb +=slip_ground_offset_;
       prog_->AddBoundingBoxConstraint(
-          lb, 10, slip_contact_pos_vars(knot_point, contact_index)[1]);
+          lb, 10, slip_contact_pos_vars(knot_point, contact_index)[2]);
 
     }
   }
@@ -120,7 +123,10 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double term
                                                             terminal_gain,
                                                             slip_contact_points_.size(),
                                                             knot_point));
-  prog_->AddCost(lifting_cost, {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point]});
+//  prog_->AddCost(lifting_cost,
+//                 {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],
+//                  slip_contact_vel_vars_[knot_point],
+//                  slip_force_vars_[knot_point]});
 }
 
 void CassieKinematicCentroidalMPC::SetModeSequence(const std::vector<std::vector<bool>> &contact_sequence) {
@@ -138,13 +144,26 @@ void CassieKinematicCentroidalMPC::MapModeSequence() {
   }
 }
 void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
-  auto reduction_constraint =
-      std::make_shared<PlanarSlipReductionConstraint<double>>(
-          plant_, contexts_[knot_point].get(), slip_contact_points_,6 + 3 + 3 * 3 * n_contact_points_ + n_q_ + n_v_,knot_point);
-  prog_->AddConstraint(reduction_constraint,
-                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],
-                        com_pos_vars(knot_point), momentum_vars(knot_point), contact_pos_[knot_point], contact_vel_[knot_point], contact_force_[knot_point],
-                        state_vars(knot_point)});
+  prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point));
+  prog_->AddConstraint(slip_vel_vars_[knot_point] == momentum_vars(knot_point).tail(3));
+  prog_->AddConstraint(slip_contact_pos_vars(knot_point,0) == contact_pos_vars(knot_point,0));
+  prog_->AddConstraint(slip_contact_pos_vars(knot_point,1) == contact_pos_vars(knot_point,2));
+  prog_->AddConstraint(slip_contact_vel_vars(knot_point,0) == contact_vel_vars(knot_point,0));
+  prog_->AddConstraint(slip_contact_vel_vars(knot_point,1) == contact_vel_vars(knot_point,2));
+  auto grf_constraint =
+    std::make_shared<SlipGrfReductionConstrain>(
+        plant_, reducers[knot_point], 2, 4,knot_point);
+  prog_->AddConstraint(grf_constraint,
+                       {com_pos_vars(knot_point), slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
+
+
+//  auto reduction_constraint =
+//      std::make_shared<PlanarSlipReductionConstraint>(
+//          plant_, reducers[knot_point], 2, 4,knot_point);
+//  prog_->AddConstraint(reduction_constraint,
+//                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],slip_force_vars_[knot_point],
+//                        com_pos_vars(knot_point), momentum_vars(knot_point), contact_pos_[knot_point], contact_vel_[knot_point], contact_force_[knot_point],
+//                        state_vars(knot_point)});
 }
 
 void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
@@ -152,7 +171,7 @@ void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
       std::make_shared<PlanarSlipLiftingConstraint>(
           plant_, lifters_[knot_point], 2, n_contact_points_,knot_point);
   prog_->AddConstraint(lifting_constraint,
-                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],
+                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],slip_force_vars_[knot_point],
                         com_pos_vars(knot_point), momentum_vars(knot_point), contact_pos_[knot_point], contact_vel_[knot_point], contact_force_[knot_point],
                         state_vars(knot_point)});
 }
@@ -164,8 +183,8 @@ void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
             r0_, k_, m_, 2, slip_contact_sequence_[knot_point], dt_, knot_point);
 
     prog_->AddConstraint(slip_com_dynamics,
-                         {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],
-                          slip_com_vars_[knot_point+1], slip_vel_vars_[knot_point+1], slip_contact_pos_vars_[knot_point+1]});
+                         {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], slip_force_vars_[knot_point],
+                          slip_com_vars_[knot_point+1], slip_vel_vars_[knot_point+1], slip_contact_pos_vars_[knot_point+1],slip_force_vars_[knot_point+1]});
     prog_->AddConstraint(
         slip_contact_pos_vars_[knot_point + 1] ==
             slip_contact_pos_vars_[knot_point] +
@@ -178,17 +197,17 @@ void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
 
 drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_pos_vars(int knot_point_index,
                                                                                             int slip_foot_index) {
-  return slip_contact_pos_vars_[knot_point_index].segment(2 * slip_foot_index, 2);
+  return slip_contact_pos_vars_[knot_point_index].segment(3 * slip_foot_index, 3);
 }
 drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_vel_vars(int knot_point_index,
                                                                                             int slip_foot_index) {
-  return slip_contact_vel_vars_[knot_point_index].segment(2 * slip_foot_index, 2);
+  return slip_contact_vel_vars_[knot_point_index].segment(3 * slip_foot_index, 3);
 }
 
 void CassieKinematicCentroidalMPC::SetComPositionGuess(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],
-                           slip_index_.unaryExpr(com_trajectory.value(dt_ * knot_point)));
+                           com_trajectory.value(dt_ * knot_point));
   }
   KinematicCentroidalMPC::SetComPositionGuess(com_trajectory);
 }
@@ -200,26 +219,36 @@ void CassieKinematicCentroidalMPC::SetRobotStateGuess(const drake::trajectories:
     dairlib::multibody::SetVelocitiesIfNew<double>(plant_, v_traj.value(dt_*knot_point),contexts_[knot_point].get());
     for(int contact = 0; contact < slip_contact_points_.size(); contact++){
       prog_->SetInitialGuess(slip_contact_pos_vars(knot_point, contact),
-                             slip_index_.unaryExpr(slip_contact_points_[contact].EvalFull(*contexts_[knot_point])));
+                             slip_contact_points_[contact].EvalFull(*contexts_[knot_point]));
       prog_->SetInitialGuess(slip_contact_vel_vars(knot_point, contact),
-                             slip_index_.unaryExpr(slip_contact_points_[contact].EvalFullTimeDerivative(*contexts_[knot_point])));
+                             slip_contact_points_[contact].EvalFullTimeDerivative(*contexts_[knot_point]));
     }
   }
   KinematicCentroidalMPC::SetRobotStateGuess(q_traj, v_traj);
 }
+
 void CassieKinematicCentroidalMPC::SetMomentumGuess(const drake::trajectories::PiecewisePolynomial<double> &momentum_trajectory) {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     prog_->SetInitialGuess(slip_vel_vars_[knot_point],
-                           slip_index_.unaryExpr(momentum_trajectory.value(dt_ * knot_point))/m_);
+                           drake::VectorX<double>(momentum_trajectory.value(dt_ * knot_point)).tail(3)/m_);
   }
   KinematicCentroidalMPC::SetMomentumGuess(momentum_trajectory);
 }
+
 drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_point) {
-  drake::VectorX<double> slip_state(2 + 2 + 4 * slip_contact_points_.size());
+  drake::VectorX<double> slip_state(3 + 3 + 6 * slip_contact_points_.size() + slip_contact_points_.size());
   slip_state<<result_->GetSolution(slip_com_vars_[knot_point]),
       result_->GetSolution(slip_vel_vars_[knot_point]),
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
-      result_->GetSolution(slip_contact_vel_vars_[knot_point]);
+      result_->GetSolution(slip_contact_vel_vars_[knot_point]),
+      result_->GetSolution(slip_force_vars_[knot_point]);
 
  return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
 }
+
+void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
+  for(const auto& force_vars : slip_force_vars_){
+    prog_->SetInitialGuess(force_vars, drake::VectorX<double>::Zero(force_vars.size()));
+  }
+  KinematicCentroidalMPC::SetForceGuess(force_trajectory);
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 56d84ee501..a97acb8317 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -4,6 +4,7 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h"
 
 /*!
  * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop closure, joint limits, and cassie contact points
@@ -31,13 +32,15 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
     slip_contact_points_ = CreateSlipContactPoints(plant,mu);
     for (int knot = 0; knot < n_knot_points; knot++) {
       slip_com_vars_.push_back(prog_->NewContinuousVariables(
-          2, "slip_com_" + std::to_string(knot)));
+          3, "slip_com_" + std::to_string(knot)));
       slip_vel_vars_.push_back(prog_->NewContinuousVariables(
-          2, "slip_vel_" + std::to_string(knot)));
+          3, "slip_vel_" + std::to_string(knot)));
       slip_contact_pos_vars_.push_back(prog_->NewContinuousVariables(
-          2*2, "slip_contact_pos_" + std::to_string(knot)));
+          2*3, "slip_contact_pos_" + std::to_string(knot)));
       slip_contact_vel_vars_.push_back(prog_->NewContinuousVariables(
-          2*2, "slip_contact_vel_" + std::to_string(knot)));
+          2*3, "slip_contact_vel_" + std::to_string(knot)));
+      slip_force_vars_.push_back(prog_->NewContinuousVariables(
+          2, "slip_force_" + std::to_string(knot)));
       lifters_.emplace_back(new PlanarSlipLifter(plant,
                                                  contexts_[knot].get(),
                                                  slip_contact_points_,
@@ -45,8 +48,15 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
                                                  {{0, {0, 1}}, {1, {2, 3}}},
                                                  nominal_stand,
                                                  k,
-                                                 r0,
-                                                 {stance_width / 2, -stance_width / 2}));
+                                                 r0));
+      reducers.emplace_back(new PlanarSlipReducer(plant,
+                                                 contexts_[knot].get(),
+                                                 slip_contact_points_,
+                                                 CreateContactPoints(plant, mu),
+                                                 {{0, {0, 1}}, {1, {2, 3}}},
+                                                 k,
+                                                 r0));
+
     }
     std::vector<bool> stance_mode(2);
     std::fill(stance_mode.begin(), stance_mode.end(), true);
@@ -95,6 +105,9 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   void SetMomentumGuess(
       const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory) override;
 
+  void SetForceGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& force_trajectory) override;
+
  private:
   void MapModeSequence();
 
@@ -130,8 +143,10 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   std::vector<drake::solvers::VectorXDecisionVariable> slip_vel_vars_;
   std::vector<drake::solvers::VectorXDecisionVariable> slip_contact_pos_vars_;
   std::vector<drake::solvers::VectorXDecisionVariable> slip_contact_vel_vars_;
+  std::vector<drake::solvers::VectorXDecisionVariable> slip_force_vars_;
 
   std::vector<std::shared_ptr<PlanarSlipLifter>> lifters_;
+  std::vector<std::shared_ptr<PlanarSlipReducer>> reducers;
 
   std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
 
@@ -139,7 +154,5 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
                                                                             {{true, true, false, false},{true, false}},
                                                                             {{false, false, true, true},{false, true}},
                                                                             {{false, false, false, false},{false, false}}};
-
-  const Eigen::Vector2i slip_index_{0,2};
 };
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
index 0547e75e96..6a62209535 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
@@ -4,9 +4,11 @@ package(default_visibility = ["//visibility:public"])
 cc_library(
     name = "planar_slip",
     srcs = ["planar_slip_constraints.cc",
-            "planar_slip_lifter.cc"],
+            "planar_slip_lifter.cc",
+            "planar_slip_reducer.cc"],
     hdrs = ["planar_slip_constraints.h",
-            "planar_slip_lifter.h"],
+            "planar_slip_lifter.h",
+            "planar_slip_reducer.h"],
     deps = [
         "//solvers:constraints",
         "//multibody/kinematic:kinematic",
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index e295af6187..c2e4da9870 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -3,24 +3,22 @@
 #include "planar_slip_constraints.h"
 #include "multibody/multibody_utils.h"
 
-template<typename T>
-PlanarSlipReductionConstraint<T>::PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<T>& plant,
-                                                                drake::systems::Context<T> *context,
-                                                                const std::vector<dairlib::multibody::WorldPointEvaluator<
-                                                                    T>> &slip_feet,
-                                                                int complex_state_size,
-                                                                int knot_index):dairlib::solvers::NonlinearConstraint<T>(
-                                                                    2+2+2*2*slip_feet.size(), 2+2+2*2*slip_feet.size() + complex_state_size,
-                                                                    Eigen::VectorXd::Zero(2+2+2*2*slip_feet.size()),
-                                                                    Eigen::VectorXd::Zero(2+2+2*2*slip_feet.size()),
-                                                                    "PlanarSlipReductionConstraint[" +
-                                                                        std::to_string(knot_index) + "]"),
-                                                                        m_(plant.CalcTotalMass(*context)),
-                                                                        context_(context),
-                                                                        plant_(plant),
-                                                                        nx_(plant.num_positions()+plant.num_velocities()),
-                                                                        slip_feet_(slip_feet)
-                                                                    {}
+PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<double> &plant,
+                                                                std::shared_ptr<PlanarSlipReducer> reducing_function,
+                                                                int n_slip_feet,
+                                                                int n_complex_feet,
+                                                                int knot_index)
+    :dairlib::solvers::NonlinearConstraint<double>(
+    3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet,
+    3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions()
+        + plant.num_velocities(),
+    Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
+    Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
+    "PlanarSlipReductionConstraint[" +
+        std::to_string(knot_index) + "]"),
+     reducing_function_(reducing_function),
+     slip_dim_(2 + 2 + 2 * 2 * n_slip_feet + n_slip_feet),
+     complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()) {}
 
 
 /// Input is of the form:
@@ -28,6 +26,7 @@ PlanarSlipReductionConstraint<T>::PlanarSlipReductionConstraint(const drake::mul
 ///     slip_velocity
 ///     slip_contact_pos
 ///     slip_contact_vel
+///     slip_force
 ///     complex_com
 ///     complex_ang_momentum
 ///     complex_lin_momentum
@@ -36,28 +35,41 @@ PlanarSlipReductionConstraint<T>::PlanarSlipReductionConstraint(const drake::mul
 ///     complex_contact_force
 ///     complex_gen_pos
 ///     complex_gen_vel
-template<typename T>
-void PlanarSlipReductionConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
-                                                          drake::VectorX<T> *y) const {
-  const auto& slip_com = x.head(kSLIP_DIM);
-  const auto& slip_vel = x.segment(kSLIP_DIM, kSLIP_DIM);
-  const auto& slip_contact_pos = x.segment(kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_feet_.size());
-  const auto& slip_contact_vel = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size(), kSLIP_DIM * slip_feet_.size());
-  const auto& complex_com = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size() + kSLIP_DIM * slip_feet_.size(), 3);
-  const auto& complex_ang_momentum = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size() + kSLIP_DIM * slip_feet_.size() + 3, 3);
-  const auto& complex_lin_momentum = x.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_feet_.size() + kSLIP_DIM * slip_feet_.size() + 3 + 3, 3);
-  const auto& complex_gen_state = x.tail(nx_);
-
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew<T>(plant_, complex_gen_state,  context_);
-
-  *y = drake::VectorX<T>::Zero(2+2+2*kSLIP_DIM*slip_feet_.size());
-  y->head(kSLIP_DIM) = slip_com - slip_index_.unaryExpr(complex_com);
-
-  y->segment(kSLIP_DIM, kSLIP_DIM) = slip_vel * m_ - slip_index_.unaryExpr(complex_lin_momentum);
-  for(int i = 0; i<slip_feet_.size(); i++){
-    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_pos.segment(kSLIP_DIM * i, kSLIP_DIM) - slip_index_.unaryExpr(slip_feet_[i].EvalFull(*context_));
-    y->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * slip_feet_.size()+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_vel.segment(kSLIP_DIM * i, kSLIP_DIM) - slip_index_.unaryExpr(slip_feet_[i].EvalFullTimeDerivative(*context_));
-  }
+void PlanarSlipReductionConstraint::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>> &x,
+                                                          drake::VectorX<double> *y) const {
+  const auto& slip_state = x.head(slip_dim_);
+  const auto& complex_state = x.tail(complex_dim_);
+  *y = reducing_function_->Reduce(complex_state) - slip_state;
+}
+
+
+SlipGrfReductionConstrain::SlipGrfReductionConstrain(const drake::multibody::MultibodyPlant<double> &plant,
+                                                     std::shared_ptr<PlanarSlipReducer> reducing_function,
+                                                     int n_slip_feet,
+                                                     int n_complex_feet,
+                                                     int knot_index):dairlib::solvers::NonlinearConstraint<double>(
+    n_slip_feet,
+    3 + 3 * n_slip_feet + 3 * n_complex_feet + n_slip_feet,
+    Eigen::VectorXd::Zero(n_slip_feet),
+    Eigen::VectorXd::Zero(n_slip_feet),
+    "SlipGrfReductionConstraint[" +
+        std::to_string(knot_index) + "]"),
+                                                                     reducing_function_(reducing_function),
+                                                                     n_slip_feet_(n_slip_feet),
+                                                                     n_complex_feet_(n_complex_feet) {}
+/// Input is of the form:
+///     complex_com
+///     slip_contact_pos
+///     complex_grf
+///     slip_contact_force
+void SlipGrfReductionConstrain::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>> &x,
+                                                   drake::VectorX<double> *y) const {
+  const auto& complex_com = x.head(3);
+  const auto& slip_contact_pos = x.segment(3,3*n_slip_feet_);
+  const auto& complex_grf = x.segment(3 + 3*n_slip_feet_,3*n_complex_feet_);
+  const auto& slip_contact_force = x.segment(3 + 3*n_slip_feet_ + 3*n_complex_feet_,n_slip_feet_);
+  *y = reducing_function_->ReduceGrf(complex_com, slip_contact_pos, complex_grf) - slip_contact_force;
+
 }
 
 PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double> &plant,
@@ -67,7 +79,7 @@ PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody:
                                                          int knot_index)
     : dairlib::solvers::NonlinearConstraint<double>(
     6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities(),
-    2 + 2 + 2 * 2 * n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions()
+    3 + 3 + 3 * 2 * n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + n_slip_feet
         + plant.num_velocities(),
     Eigen::VectorXd::Zero(
         6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()),
@@ -76,7 +88,7 @@ PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody:
     "PlanarSlipLiftingConstraint[" +
         std::to_string(knot_index) + "]"),
       lifting_function_(std::move(lifting_function)),
-      slip_dim_(2 + 2 + 2 * 2 * n_slip_feet),
+      slip_dim_(2 + 2 + 2 * 2 * n_slip_feet + n_slip_feet),
       complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()){
 
 }
@@ -86,6 +98,7 @@ PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody:
 ///     slip_velocity
 ///     slip_contact_pos
 ///     slip_contact_vel
+///     slip_force
 ///     complex_com
 ///     complex_ang_momentum
 ///     complex_lin_momentum
@@ -98,8 +111,7 @@ void PlanarSlipLiftingConstraint::EvaluateConstraint(const Eigen::Ref<const drak
                                                      drake::VectorX<double> *y) const {
   const auto& slip_state = x.head(slip_dim_);
   const auto& complex_state = x.tail(complex_dim_);
-  *y = lifting_function_->Lift(slip_state);
-  *y = *y - complex_state;
+  *y = lifting_function_->Lift(slip_state) - complex_state;
 }
 
 template<typename T>
@@ -110,9 +122,9 @@ PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
                                                               std::vector<bool> contact_mask,
                                                               double dt,
                                                               int knot_index):dairlib::solvers::NonlinearConstraint<T>(
-    2 + 2, 2 * (2 + 2 + 2 * n_feet),
-    Eigen::VectorXd::Zero(2 + 2),
-    Eigen::VectorXd::Zero(2 + 2),
+    3 + 3, 2 * (3 + 3 + 3 * n_feet + n_feet),
+    Eigen::VectorXd::Zero(6),
+    Eigen::VectorXd::Zero(6),
     "PlanarSlipDynamicsConstraint[" +
         std::to_string(knot_index) + "]"),
                                                                               r0_(r0),
@@ -127,24 +139,28 @@ PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
 ///   - com0, center of mass at time k
 ///   - vel0, center of mass velocity at time k
 ///   - contact_pos0, active contact positions at time k
+///   - force0, slip force in parallel with spring at time k
 ///   - com1, center of mass at time k+1
 ///   - vel1, center of mass velocity at time k+1
 ///   - contact_pos1, active contact positions at time k+1
+///   - force1, slip force in parallel with spring at time k+1
 template<typename T>
 void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
                                                          drake::VectorX<T> *y) const {
-  const auto& com0 = x.head(2);
-  const auto& vel0 = x.segment(2, 2);
-  const auto& contact_pos0 = x.segment(2 + 2, n_feet_ * 2);
-  const auto& com1 = x.segment(2 + 2 + n_feet_ * 2, 2);
-  const auto& vel1 = x.segment(2 + 2 + n_feet_ * 2 + 2, 2);
-  const auto& contact_pos1 = x.segment(2 + 2 + n_feet_ * 2 + 2 + 2, n_feet_ * 2);
+  const auto& com0 = x.head(3);
+  const auto& vel0 = x.segment(3, 3);
+  const auto& contact_pos0 = x.segment(3 + 3, n_feet_ * 3);
+  const auto& force0 = x.segment(3 + 3 + n_feet_ * 3, n_feet_);
+  const auto& com1 = x.segment(3 + 3 + n_feet_ * 3 + n_feet_, 3);
+  const auto& vel1 = x.segment(3 + 3 + n_feet_ * 3 + 3 + n_feet_, 3);
+  const auto& contact_pos1 = x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_, n_feet_ * 3);
+  const auto& force1 = x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_ + n_feet_ * 3, n_feet_);
 
-  const auto& x0 = x.head(4);
-  const auto& x1 = x.segment(2 + 2 + n_feet_ * 2, 4);
+  const auto& x0 = x.head(6);
+  const auto& x1 = x.segment(3 + 3 + n_feet_ * 3 + n_feet_, 6);
 
-  drake::Vector<T, 4> xdot0 = CalcTimeDerivativesWithForce(com0, vel0,contact_pos0);
-  drake::Vector<T, 4> xdot1 = CalcTimeDerivativesWithForce(com1, vel1,contact_pos1);
+  drake::Vector<T, 6> xdot0 = CalcTimeDerivativesWithForce(com0, vel0,contact_pos0, force0);
+  drake::Vector<T, 6> xdot1 = CalcTimeDerivativesWithForce(com1, vel1,contact_pos1, force1);
 
   // Predict state and return error
   const auto x1Predict = x0 + 0.5 * dt_ * (xdot0 + xdot1);
@@ -154,18 +170,19 @@ void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
 template<typename T>
 drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(const drake::VectorX<T> &com_position,
                                                                                 const drake::VectorX<T> &com_vel,
-                                                                                const drake::VectorX<T> &contact_loc) const {
-  drake::Vector2<T> ddcom = {0, -9.81};
+                                                                                const drake::VectorX<T> &contact_loc,
+                                                                                const drake::VectorX<T> &slip_force) const {
+  drake::Vector3<T> ddcom = {0, 0, -9.81};
   for(int foot = 0; foot < n_feet_; foot ++){
     if(contact_mask_[foot]){
-      drake::Vector2<T> com_rt_foot = com_position - contact_loc.segment(2 * foot, 2);
+      drake::Vector3<T> com_rt_foot = com_position - contact_loc.segment(3 * foot, 3);
       const auto r = com_rt_foot.norm();
       const auto unit_vec = com_rt_foot/r;
-      const auto F = k_ * (r0_ - r);
+      const auto F = k_ * (r0_ - r) + slip_force[foot];
       ddcom = ddcom + F * unit_vec / m_;
     }
   }
-  drake::Vector4<T> derivative;
+  drake::Vector6<T> derivative;
   derivative << com_vel,ddcom;
   return derivative;
 }
@@ -180,7 +197,7 @@ QuadraticLiftedCost::QuadraticLiftedCost(std::shared_ptr<PlanarSlipLifter> lifti
                                             double terminal_gain,
                                             int n_slip_feet,
                                             int knot_point):dairlib::solvers::NonlinearCost<double>(
-    2 + 2 + 2 * 2 * n_slip_feet, "LiftedCost[" +
+    3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet, "LiftedCost[" +
         std::to_string(knot_point) + "]"),
                                                             lifting_function_(std::move(lifting_function)),
                                                             com_cost_(std::move(com_cost)),
@@ -211,5 +228,4 @@ Eigen::Matrix<double, -1, 1, 0> QuadraticLiftedCost::CalcCost(const QuadraticLif
   return terminal_gain_ * error.transpose() * cost.Q * error;
 }
 
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipReductionConstraint);
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipDynamicsConstraint);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
index 6a41b128a3..06e4d78d77 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
@@ -9,27 +9,40 @@
 #include "multibody/kinematic/kinematic_evaluator_set.h"
 #include "multibody/kinematic/world_point_evaluator.h"
 #include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h"
 
-template <typename T>
-class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstraint<T> {
+class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstraint<double> {
+ public:
+  PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<double> &plant,
+                                std::shared_ptr<PlanarSlipReducer> reducing_function,
+                                int n_slip_feet,
+                                int n_complex_feet,
+                                int knot_index);
+
+ private:
+  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
+                          drake::VectorX<double>* y) const override;
 
+  std::shared_ptr<PlanarSlipReducer> reducing_function_;
+  const int slip_dim_;
+  const int complex_dim_;
+};
+
+class SlipGrfReductionConstrain : public dairlib::solvers::NonlinearConstraint<double> {
  public:
-  PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<T>& plant,
-                                drake::systems::Context<T>* context,
-                                const std::vector<dairlib::multibody::WorldPointEvaluator<T>>& slip_feet,
-                                int complex_state_size, int knot_index);
+  SlipGrfReductionConstrain(const drake::multibody::MultibodyPlant<double> &plant,
+                                std::shared_ptr<PlanarSlipReducer> reducing_function,
+                                int n_slip_feet,
+                                int n_complex_feet,
+                                int knot_index);
 
  private:
-  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
-                          drake::VectorX<T>* y) const override;
+  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
+                          drake::VectorX<double>* y) const override;
 
-  const T m_;
-  drake::systems::Context<T>* context_;
-  const drake::multibody::MultibodyPlant<T>& plant_;
-  int nx_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<T>> slip_feet_;
-  const int kSLIP_DIM = 2;
-  const Eigen::Vector2i slip_index_{0,2};
+  std::shared_ptr<PlanarSlipReducer> reducing_function_;
+  const int n_slip_feet_;
+  const int n_complex_feet_;
 };
 
 class PlanarSlipLiftingConstraint : public dairlib::solvers::NonlinearConstraint<double> {
@@ -61,7 +74,8 @@ class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstrain
   drake::VectorX<T> CalcTimeDerivativesWithForce(
       const drake::VectorX<T>& com_position,
       const drake::VectorX<T>& com_vel,
-      const drake::VectorX<T>& contact_loc) const;
+      const drake::VectorX<T>& contact_loc,
+      const drake::VectorX<T> &slip_force) const;
 
   const double r0_;
   const double k_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index daf49c946b..19faf76e09 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -10,15 +10,13 @@ PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double
                                       const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
                                       const drake::VectorX<double> &nominal_stand,
                                       double k,
-                                      double r0,
-                                      const std::vector<double> &stance_widths):
+                                      double r0):
     plant_(plant),
     context_(context),
     ik_(plant, context),
     m_(plant.CalcTotalMass(*context)),
     k_(k),
     r0_(r0),
-    stance_widths_(stance_widths),
     slip_contact_points_(slip_contact_points),
     complex_contact_points_(complex_contact_points),
     simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
@@ -51,34 +49,30 @@ PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double
           (ik_.q())(positions_map.at("ankle_joint_right")) ==
           M_PI * 13 / 180.0);
 
-  // Keep the foot flat (breaks if orientation changes)
-  ik_.get_mutable_prog()->AddLinearConstraint(
-      (ik_.q())(positions_map.at("hip_pitch_right")) +
-          (ik_.q())(positions_map.at("knee_right")) +
-          (ik_.q())(positions_map.at("ankle_joint_right"))+
-          (ik_.q())(positions_map.at("toe_right")) ==
-          -0.862002);
-  ik_.get_mutable_prog()->AddLinearConstraint(
-      (ik_.q())(positions_map.at("hip_pitch_left")) +
-          (ik_.q())(positions_map.at("knee_left")) +
-          (ik_.q())(positions_map.at("ankle_joint_left"))+
-          (ik_.q())(positions_map.at("toe_left")) ==
-          -0.862002);
 }
 
 drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Vector3<double> &com_position,
                                                                const drake::VectorX<double> &slip_feet_positions) const {
-  DRAKE_DEMAND(slip_feet_positions.size() == 2*slip_contact_points_.size());
+  DRAKE_DEMAND(slip_feet_positions.size() == 3*slip_contact_points_.size());
   //Add com position constraint
   const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), com_vars_);
   //Add feet position constraint
   std::vector<drake::solvers::Binding<drake::solvers::Constraint>> foot_constraints;
   for(int i = 0; i < slip_contact_points_.size(); i++){
-    const drake::Vector3<double> slip_spatial_foot_pos =  {slip_feet_positions[2*i], stance_widths_[i], slip_feet_positions[2*i+1]};
+    const auto& slip_spatial_foot_pos = slip_feet_positions.segment(3 * i, 3);
+    const drake::Vector3<double> slip_foot_rt_com = slip_spatial_foot_pos - com_position;
     foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), slip_contact_points_[i].get_pt_A(), plant_.world_frame(),
                                                          std::nullopt,
-                                                         slip_spatial_foot_pos - com_position,
-                                                         slip_spatial_foot_pos - com_position));
+                                                         slip_foot_rt_com,
+                                                         slip_foot_rt_com));
+    for(const auto complex_index : simple_foot_index_to_complex_foot_index_.at(i)){
+      const drake::Vector3<double> lb{-100, -100, slip_foot_rt_com[2]};
+      const drake::Vector3<double> ub{100, 100, slip_foot_rt_com[2]};
+      foot_constraints.push_back(ik_.AddPositionConstraint(complex_contact_points_[complex_index].get_frame(), complex_contact_points_[complex_index].get_pt_A(), plant_.world_frame(),
+                                                           std::nullopt,
+                                                           lb,
+                                                           ub));
+    }
   }
   //Set initial guess for com
   ik_.get_mutable_prog()->SetInitialGuess(com_vars_,Eigen::VectorXd::Zero(3));
@@ -97,14 +91,13 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Ve
   for(const auto& constraint : foot_constraints){
     ik_.get_mutable_prog()->RemoveConstraint(constraint);
   }
-  // TODO figure out what to do about toe
   return q_sol_normd;
 }
 drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::VectorX<double>& generalized_pos,
                                                                const drake::Vector3<double>& linear_momentum,
                                                                const drake::Vector3<double>& com_pos,
                                                                const drake::VectorX<double>& slip_feet_velocities)const {
-  DRAKE_DEMAND(slip_feet_velocities.size() == 2*slip_contact_points_.size());
+  DRAKE_DEMAND(slip_feet_velocities.size() == 3*slip_contact_points_.size());
   // Preallocate linear constraint
   drake::MatrixX<double> A(3 + 3 * slip_contact_points_.size() ,n_v_); // 3 rows for linear momentum, 3 rows for each slip foot
   drake::VectorX<double> b(3 + 3 * slip_contact_points_.size());
@@ -117,7 +110,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
   slip_contact_points_[0].EvalFull(*context_ );
 
   for(int i = 0; i < slip_contact_points_.size(); i++){
-    b.segment(3 * i, 3) = Eigen::Vector3d({slip_feet_velocities[2 * i] , 0 , slip_feet_velocities[2 * i + 1]});
+    b.segment(3 * i, 3) =slip_feet_velocities.segment(3 * i, 3);
     slip_contact_points_[i].EvalFullJacobian(*context_ );
     A.middleRows(3 * i, 3) = slip_contact_points_[i].EvalFullJacobian(*context_ );
   }
@@ -168,13 +161,14 @@ drake::VectorX<double> PlanarSlipLifter::LiftContactVel(const drake::VectorX<dou
 
 drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &com_pos,
                                                const drake::VectorX<double> &slip_feet_pos,
+                                               const drake::VectorX<double> &slip_force,
                                                const drake::VectorX<double> &complex_contact_point_pos)const {
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
   // Loop through the slip feet
   for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
     // Calculate the slip grf
-    double r = (slip_feet_pos.segment(simple_index * 2, 2) - slip_index_.unaryExpr(com_pos)).norm();
-    double slip_grf_mag = k_ * (r - r0_);
+    double r = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).norm();
+    double slip_grf_mag = slip_force[simple_index] + k_ * (r0_ - r);
 
     // Find the average location for all of the complex contact points that make up the SLIP foot
     drake::Vector3<double> average_pos = drake::VectorX<double>::Zero(3);
@@ -201,6 +195,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
 ///     slip_velocity
 ///     slip_contact_pos
 ///     slip_contact_vel
+///     slip_force
 /// Output is of the form:
 ///     complex_com
 ///     complex_ang_momentum
@@ -216,19 +211,20 @@ void PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip
   const auto& slip_vel = slip_state.segment(kSLIP_DIM, kSLIP_DIM);
   const auto& slip_contact_pos = slip_state.segment(kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_contact_points_.size());
   const auto& slip_contact_vel = slip_state.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size(), kSLIP_DIM * slip_contact_points_.size());
+  const auto& slip_force = slip_state.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size() + kSLIP_DIM * slip_contact_points_.size(), slip_contact_points_.size());
 
-  const drake::Vector3<double> com = {slip_com[0], 0, slip_com[1]};
-  const drake::Vector3<double> lin_mom = {m_ * slip_vel[0], 0, m_ * slip_vel[1]};
-  const auto& generalized_pos = LiftGeneralizedPosition(com, slip_contact_pos);
-  const auto& generalized_vel = LiftGeneralizedVelocity(generalized_pos, lin_mom, com, slip_contact_vel);
+  const drake::Vector3<double> lin_mom = slip_vel * m_;
+  const auto& generalized_pos = LiftGeneralizedPosition(slip_com, slip_contact_pos);
+  const auto& generalized_vel = LiftGeneralizedVelocity(generalized_pos, lin_mom, slip_com, slip_contact_vel);
 
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
   dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel, context_);
   const auto& complex_contact_pos = LiftContactPos(generalized_pos);
 
-  (*complex_state) << com, plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com)
-      .get_coeffs(), complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel), LiftGrf(com,
+  (*complex_state) << slip_com, plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, slip_com)
+      .get_coeffs(), complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel), LiftGrf(slip_com,
                                                                                                     slip_contact_pos,
+                                                                                                    slip_force,
                                                                                                     complex_contact_pos),
       generalized_pos, generalized_vel;
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
index 54d56b2b9e..7ec1da8d5a 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -19,8 +19,7 @@ class PlanarSlipLifter {
                    const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
                    const drake::VectorX<double>& nominal_stand,
                    double k,
-                   double r0,
-                   const std::vector<double>& stance_widths);
+                   double r0);
 
   void Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
             drake::VectorX<double> *complex_state) const;
@@ -59,7 +58,8 @@ class PlanarSlipLifter {
 
   drake::VectorX<double> LiftGrf(const drake::VectorX<double>& com_pos,
                             const drake::VectorX<double>& slip_feet_pos,
-                            const drake::VectorX<double>& complex_contact_point_pos) const;
+                                 const drake::VectorX<double> &slip_force,
+                                 const drake::VectorX<double>& complex_contact_point_pos) const;
 
 
   const drake::multibody::MultibodyPlant<double>& plant_;
@@ -77,7 +77,6 @@ class PlanarSlipLifter {
 
   drake::solvers::VectorXDecisionVariable com_vars_;
 
-  const int kSLIP_DIM = 2;
-  const Eigen::Vector2i slip_index_{0,2};
+  const int kSLIP_DIM = 3;
 };
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index 6baff37d23..cd36c6f3be 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -16,6 +16,7 @@
 #include "examples/Cassie/cassie_utils.h"
 #include "multibody/visualization_utils.h"
 #include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h"
+#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h"
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
@@ -84,16 +85,23 @@ int main(int argc, char* argv[]) {
                           {{0, {0, 1}}, {1, {2, 3}}},
                           reference_state.head(plant.num_positions()),
                           2000,
-                          0.5,
-                          {0.1, -0.1});
+                          0.5);
+  PlanarSlipReducer reducer(plant,
+                          context.get(),
+                          {left_toe_eval, right_toe_eval},
+                          {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
+                          {{0, {0, 1}}, {1, {2, 3}}},
+                          2000,
+                          0.5);
 
-  Eigen::Vector2d slip_com = {0.2,0.7};
-  Eigen::Vector2d slip_vel = {0.1,0.3};
-  Eigen::Vector4d slip_feet = {0.0,0.0,0.0,0.00};
-  Eigen::Vector4d slip_foot_vel = {0.11,0.12,0.15,0.18};
+  Eigen::Vector3d slip_com = {0.2,0,0.7};
+  Eigen::Vector3d slip_vel = {0.1,0,0.1};
+  Eigen::VectorXd slip_feet(6); slip_feet  << 0.0, 0.2, 0.0,0.0,-0.2, 0.0;
+  Eigen::VectorXd slip_foot_vel(6); slip_foot_vel  << 0.11,0.12,0.0,0.15,0.18,0.0;
+  Eigen::Vector2d slip_force = {0.5,0.5};
 
-  Eigen::VectorXd slip_state(2+2+4+4);
-  slip_state << slip_com,slip_vel,slip_feet,slip_foot_vel;
+  Eigen::VectorXd slip_state(3+3+6+6+2);
+  slip_state << slip_com,slip_vel,slip_feet,slip_foot_vel,slip_force;
   Eigen::VectorXd complex_state(6 + 3 + 3 * 3 * 4 + plant.num_positions() + plant.num_velocities());
 
   auto start = std::chrono::high_resolution_clock::now();
@@ -107,9 +115,7 @@ int main(int argc, char* argv[]) {
   finish = std::chrono::high_resolution_clock::now();
   elapsed = finish - start;
   std::cout << "Solve time 2:" << elapsed.count() << std::endl;
-
-  auto context_2 = plant.CreateDefaultContext();
-  auto constraint = PlanarSlipReductionConstraint<double>(plant, context_2.get(), {left_toe_eval, right_toe_eval}, complex_state.size(), 0);
+  auto constraint = PlanarSlipReductionConstraint(plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
 
   drake::VectorX<double> error(slip_state.size());
   drake::VectorX<double> input(slip_state.size() + complex_state.size());
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
new file mode 100644
index 0000000000..b59e8fc24d
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
@@ -0,0 +1,75 @@
+#include <iostream>
+#include "planar_slip_reducer.h"
+#include "multibody/multibody_utils.h"
+
+PlanarSlipReducer::PlanarSlipReducer(const drake::multibody::MultibodyPlant<double> &plant,
+                                     drake::systems::Context<double> *context,
+                                     const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
+                                     const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
+                                     const std::map<int, std::vector<int>> &simple_foot_index_to_complex_foot_index,
+                                     double k,
+                                     double r0):plant_(plant),
+                                                context_(context),
+                                                k_(k),
+                                                r0_(r0),
+                                                m_(plant.CalcTotalMass(*context)),
+                                                slip_contact_points_(slip_contact_points),
+                                                complex_contact_points_(complex_contact_points),
+                                                simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
+                                                n_q_(plant.num_positions()),
+                                                n_v_(plant.num_velocities()) {}
+/// Input is of the form:
+///     complex_com
+///     complex_ang_momentum
+///     complex_lin_momentum
+///     complex_contact_pos
+///     complex_contact_vel
+///     complex_contact_force
+///     complex_gen_pos
+///     complex_gen_vel
+/// Output is of the form:
+///     slip_com
+///     slip_velocity
+///     slip_contact_pos
+///     slip_contact_vel
+///     slip_force
+void PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state,
+                               drake::VectorX<double> *slip_state) const {
+  const auto& complex_com = complex_state.segment(0, 3);
+  const auto& complex_ang_momentum = complex_state.segment(3, 3);
+  const auto& complex_lin_momentum = complex_state.segment( 3 + 3, 3);
+  const auto& complex_grf = complex_state.segment( 3 + 3 + 3 + 2 * 3 * complex_contact_points_.size(), 3 * complex_contact_points_.size());
+  const auto& complex_gen_state = complex_state.tail(n_q_+n_v_);
+
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant_, complex_gen_state, context_);
+  slip_state->head(kSLIP_DIM) = complex_com;
+  slip_state->segment(kSLIP_DIM, kSLIP_DIM) = complex_lin_momentum/m_;
+  for(int i = 0; i<slip_contact_points_.size(); i++){
+    slip_state->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_points_[i].EvalFull(*context_);
+    slip_state->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * slip_contact_points_.size()+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_points_[i].EvalFullTimeDerivative(*context_);
+  }
+  slip_state->tail(slip_contact_points_.size()) = ReduceGrf(complex_com, slip_state->segment(kSLIP_DIM+kSLIP_DIM, slip_contact_points_.size()+kSLIP_DIM), complex_grf);
+}
+
+drake::VectorX<double> PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state) const {
+  drake::VectorX<double> slip_state(kSLIP_DIM + kSLIP_DIM + 2 * kSLIP_DIM * slip_contact_points_.size() + slip_contact_points_.size());
+  Reduce(complex_state, &slip_state);
+  return slip_state;
+}
+
+drake::VectorX<double> PlanarSlipReducer::ReduceGrf(const Eigen::Ref<const drake::VectorX<double>> &complex_com,
+                                                    const Eigen::Ref<const drake::VectorX<double>> &slip_contact_pos,
+                                                    const Eigen::Ref<const drake::VectorX<double>> &complex_grf)const {
+  Eigen::VectorXd slip_force(slip_contact_points_.size());
+  for(int i = 0; i<slip_contact_points_.size(); i++){
+    const double r = (complex_com - slip_contact_pos.segment(3 * i, 3)).norm();
+    const double spring_force = k_ * (r0_ - r);
+    auto complex_feet_it = simple_foot_index_to_complex_foot_index_.find(i);
+    Eigen::Vector3d complex_force = Eigen::Vector3d::Zero(3);
+    for(const auto complex_index : complex_feet_it->second){
+      complex_force = complex_force + complex_grf.segment(3 * complex_index, 3);
+    }
+    slip_force.coeffRef(i) = complex_force.norm() - spring_force;
+  }
+  return slip_force;
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
new file mode 100644
index 0000000000..2f9c719b12
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
@@ -0,0 +1,45 @@
+#pragma once
+#include <drake/multibody/plant/multibody_plant.h>
+#include "drake/common/drake_copyable.h"
+#include "drake/common/symbolic.h"
+#include "drake/solvers/constraint.h"
+#include "drake/systems/trajectory_optimization/multiple_shooting.h"
+#include "solvers/nonlinear_constraint.h"
+#include "multibody/kinematic/kinematic_evaluator_set.h"
+#include "multibody/kinematic/world_point_evaluator.h"
+#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
+
+class PlanarSlipReducer {
+ public:
+  PlanarSlipReducer(const drake::multibody::MultibodyPlant<double>& plant,
+                   drake::systems::Context<double>* context,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& slip_contact_points,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& complex_contact_points,
+                   const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
+                   double k,
+                   double r0);
+
+  void Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state,
+            drake::VectorX<double> *slip_state) const;
+
+  drake::VectorX<double> Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state) const;
+
+  drake::VectorX<double> ReduceGrf(const Eigen::Ref<const drake::VectorX<double>> &complex_com,
+                                   const Eigen::Ref<const drake::VectorX<double>> &slip_contact_pos,
+                                   const Eigen::Ref<const drake::VectorX<double>> &complex_grf) const;
+ private:
+  const drake::multibody::MultibodyPlant<double>& plant_;
+  mutable drake::systems::Context<double>* context_;
+  const double k_;
+  const double r0_;
+  const double m_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> complex_contact_points_;
+  const std::map<int, std::vector<int>> simple_foot_index_to_complex_foot_index_;
+  const int n_q_;
+  const int n_v_;
+
+  const int kSLIP_DIM = 3;
+};
+
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 024024397c..d24adc7841 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -245,7 +245,7 @@ void KinematicCentroidalMPC::Build(
         AddKinematicsIntegrator(knot_point);
         if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
           AddSlipReductionConstraint(knot_point + 1);
-          //TODO why do I need these constraints here
+          //TODO why do I need these constraints here, do I need these constraints below?
           AddContactConstraints(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
           AddFrictionConeConstraints(knot_point + 1);

From 4cabeac7d108ebad82a85bd8edf27c464888c1eb Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 6 Dec 2022 13:47:10 -0500
Subject: [PATCH 49/76] Slip is standing finally

---
 .../cassie_kcmpc_trajopt.cc                   |  6 +-
 .../cassie_kinematic_centroidal_mpc.cc        | 85 ++++++++++++-------
 .../trajectory_parameters.yaml                |  2 +-
 .../kinematic_centroidal_mpc.cc               |  1 +
 .../kinematic_centroidal_mpc.h                |  1 +
 5 files changed, 60 insertions(+), 35 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 561779a4a0..8df8e0735f 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -74,7 +74,7 @@ int DoMain(int argc, char* argv[]) {
   // Create reference
   // TODO(yangwill): move this into the reference generator
   // Specify knot points
-  std::vector<Gait> gait_samples = {stand, walk, stand};
+  std::vector<Gait> gait_samples = {stand, stand, stand};
   DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size());
   std::vector<double> durations = std::vector<double>(gait_samples.size());
   for (int i = 0; i < gait_samples.size(); ++i) {
@@ -98,7 +98,7 @@ int DoMain(int argc, char* argv[]) {
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
-  for(int i = 15; i <35 ; i++){
+  for(int i = 1; i <traj_params.n_knot_points ; i++){
     complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
@@ -149,7 +149,7 @@ int DoMain(int argc, char* argv[]) {
     options.SetOption(id, "dual_inf_tol", traj_params.tol);
     options.SetOption(id, "constr_viol_tol", traj_params.tol);
     options.SetOption(id, "compl_inf_tol", traj_params.tol);
-    options.SetOption(id, "max_iter", 200);
+    options.SetOption(id, "max_iter", 1000);
     options.SetOption(id, "nlp_lower_bound_inf", -1e6);
     options.SetOption(id, "nlp_upper_bound_inf", 1e6);
     options.SetOption(id, "print_timing_statistics", "yes");
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 319ab05d3b..4a312a1b16 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -78,9 +78,12 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
 }
 
 void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
-  Eigen::Vector2d force_bounds = {50.0, 50.0};
-  prog_->AddBoundingBoxConstraint(-force_bounds, force_bounds, slip_force_vars_[knot_point]);
+
+  Eigen::Vector2d force_bounds = {500.0, 500.0};
+  prog_->AddBoundingBoxConstraint(force_bounds, force_bounds, slip_force_vars_[knot_point]);
+
   for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
+    prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 3);
     if(slip_contact_sequence_[knot_point][contact_index]){
       // Foot isn't moving
       prog_->AddBoundingBoxConstraint(
@@ -92,7 +95,10 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
 
       prog_->AddConstraint(slip_force_vars_[knot_point][contact_index] + k_ * ( r0_ - (slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() ) >= 0);
     }else{
-      prog_->AddBoundingBoxConstraint(0, 0, slip_force_vars_[knot_point][contact_index]);
+      if(!is_last_knot(knot_point) && slip_contact_sequence_[knot_point + 1][contact_index]){
+//        prog_->AddConstraint( r0_ - (slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() == 0);
+      }
+//      prog_->AddBoundingBoxConstraint(0, 0, slip_force_vars_[knot_point][contact_index]);
       // Feet are above the ground
       double lb = 0;
       // Check if at least one of the time points before or after is also in
@@ -104,29 +110,48 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
         lb = swing_foot_minimum_height_;
       }
       lb +=slip_ground_offset_;
-      prog_->AddBoundingBoxConstraint(
-          lb, 10, slip_contact_pos_vars(knot_point, contact_index)[2]);
-
+//      prog_->AddBoundingBoxConstraint(
+//          lb, 0.5, slip_contact_pos_vars(knot_point, contact_index)[2]);
     }
   }
 }
 
 void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double terminal_gain) {
   const double t = dt_ * knot_point;
-  std::shared_ptr<QuadraticLiftedCost> lifting_cost(new QuadraticLiftedCost(lifters_[knot_point],
-                                                            {Q_com_, com_ref_traj_->value(t)},
-                                                            {Q_mom_, mom_ref_traj_->value(t)},
-                                                            {Q_contact_, contact_ref_traj_->value(t)},
-                                                            {Q_force_, force_ref_traj_->value(t)},
-                                                            {Q_q_,q_ref_traj_->value(t)},
-                                                            {Q_v_,v_ref_traj_->value(t)},
-                                                            terminal_gain,
-                                                            slip_contact_points_.size(),
-                                                            knot_point));
-//  prog_->AddCost(lifting_cost,
-//                 {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],
-//                  slip_contact_vel_vars_[knot_point],
-//                  slip_force_vars_[knot_point]});
+  if (com_ref_traj_) {
+    prog_->AddQuadraticErrorCost(terminal_gain * Q_com_,
+                                 com_ref_traj_->value(t),
+                                 slip_com_vars_[knot_point]);
+  }
+  // TODO add Velocity cost
+  if (contact_ref_traj_) {
+    const Eigen::MatrixXd Q_contact_pos =gains_.contact_pos.asDiagonal();
+    prog_->AddQuadraticErrorCost(
+        terminal_gain * Q_contact_pos,
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(0,3),
+        slip_contact_pos_vars(knot_point,0));
+    prog_->AddQuadraticErrorCost(
+        terminal_gain * Q_contact_pos,
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(6,3),
+        slip_contact_vel_vars(knot_point,1));
+
+    const Eigen::MatrixXd Q_contact_vel =gains_.contact_vel.asDiagonal();
+
+    prog_->AddQuadraticErrorCost(
+        terminal_gain * Q_contact_pos,
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(12,3),
+        slip_contact_pos_vars(knot_point,0));
+    prog_->AddQuadraticErrorCost(
+        terminal_gain * Q_contact_pos,
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(18,3),
+        slip_contact_vel_vars(knot_point,1));
+  }
+  if (force_ref_traj_) {
+    const Eigen::MatrixXd Q_contact_pos =Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2]).asDiagonal();
+    prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_pos,
+                                 Eigen::Vector2d::Zero(2),
+                                 slip_force_vars_[knot_point]);
+  }
 }
 
 void CassieKinematicCentroidalMPC::SetModeSequence(const std::vector<std::vector<bool>> &contact_sequence) {
@@ -153,17 +178,11 @@ void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
   auto grf_constraint =
     std::make_shared<SlipGrfReductionConstrain>(
         plant_, reducers[knot_point], 2, 4,knot_point);
-  prog_->AddConstraint(grf_constraint,
-                       {com_pos_vars(knot_point), slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
+  //TODO why is this constraint bad
+//  prog_->AddConstraint(grf_constraint,
+//                       {com_pos_vars(knot_point), slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
 
 
-//  auto reduction_constraint =
-//      std::make_shared<PlanarSlipReductionConstraint>(
-//          plant_, reducers[knot_point], 2, 4,knot_point);
-//  prog_->AddConstraint(reduction_constraint,
-//                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],slip_force_vars_[knot_point],
-//                        com_pos_vars(knot_point), momentum_vars(knot_point), contact_pos_[knot_point], contact_vel_[knot_point], contact_force_[knot_point],
-//                        state_vars(knot_point)});
 }
 
 void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
@@ -242,8 +261,12 @@ drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_p
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-
- return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
+  std::cout<<"Knot point "<< knot_point<<std::endl;
+  for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++) {
+//    std::cout<<(result_->GetSolution(slip_com_vars_[knot_point]) - result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))).squaredNorm()<<std::endl;
+  std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))<<std::endl;
+  }
+    return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
 }
 
 void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
diff --git a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml b/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
index 02bfc91688..d2681a7361 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
@@ -3,5 +3,5 @@ duration_scaling:
   [1.0, 3.0, 1.0]
 com_height: 0.8
 stance_width: 0.25
-vel: 0.5
+vel: 0.0
 tol: 1e-2
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index d24adc7841..6464367667 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -638,6 +638,7 @@ void KinematicCentroidalMPC::AddInitialStateConstraint(
   prog_->AddBoundingBoxConstraint(state, state, state_vars((0)));
 }
 void KinematicCentroidalMPC::SetGains(const KinematicCentroidalGains& gains) {
+  gains_ = gains;
   std::map<std::string, int> positions_map =
       dairlib::multibody::MakeNameToPositionsMap(plant_);
   std::map<std::string, int> velocities_map =
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 4d722a6217..4048dbb77b 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -447,4 +447,5 @@ class KinematicCentroidalMPC {
   dairlib::LcmTrajectory lcm_trajectory_;
 
   std::vector<Complexity> complexity_schedule_;
+  KinematicCentroidalGains gains_;
 };

From eb04535b83156333c0e1e0eefd7356e9a384a6c5 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 7 Dec 2022 11:58:33 -0500
Subject: [PATCH 50/76] Robot is able to stand

---
 .../cassie_kinematic_centroidal_mpc.cc        | 23 ++++++++-----------
 .../kinematic_centroidal_mpc.cc               |  1 -
 2 files changed, 9 insertions(+), 15 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 4a312a1b16..55a8b84cc7 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -79,8 +79,8 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
 
 void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
 
-  Eigen::Vector2d force_bounds = {500.0, 500.0};
-  prog_->AddBoundingBoxConstraint(force_bounds, force_bounds, slip_force_vars_[knot_point]);
+  Eigen::Vector2d force_bounds = {800.0, 800.0};
+  prog_->AddBoundingBoxConstraint(-force_bounds, force_bounds, slip_force_vars_[knot_point]);
 
   for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
     prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 3);
@@ -133,16 +133,15 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double term
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_contact_pos,
         Eigen::VectorXd(contact_ref_traj_->value(t)).segment(6,3),
-        slip_contact_vel_vars(knot_point,1));
+        slip_contact_pos_vars(knot_point,1));
 
     const Eigen::MatrixXd Q_contact_vel =gains_.contact_vel.asDiagonal();
-
     prog_->AddQuadraticErrorCost(
-        terminal_gain * Q_contact_pos,
+        terminal_gain * Q_contact_vel,
         Eigen::VectorXd(contact_ref_traj_->value(t)).segment(12,3),
-        slip_contact_pos_vars(knot_point,0));
+        slip_contact_vel_vars(knot_point,0));
     prog_->AddQuadraticErrorCost(
-        terminal_gain * Q_contact_pos,
+        terminal_gain * Q_contact_vel,
         Eigen::VectorXd(contact_ref_traj_->value(t)).segment(18,3),
         slip_contact_vel_vars(knot_point,1));
   }
@@ -178,7 +177,7 @@ void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
   auto grf_constraint =
     std::make_shared<SlipGrfReductionConstrain>(
         plant_, reducers[knot_point], 2, 4,knot_point);
-  //TODO why is this constraint bad
+  //TODO why is this constraint bad, probably due to it not thinking about the contact mode
 //  prog_->AddConstraint(grf_constraint,
 //                       {com_pos_vars(knot_point), slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
 
@@ -261,17 +260,13 @@ drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_p
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-  std::cout<<"Knot point "<< knot_point<<std::endl;
-  for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++) {
-//    std::cout<<(result_->GetSolution(slip_com_vars_[knot_point]) - result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))).squaredNorm()<<std::endl;
-  std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))<<std::endl;
-  }
     return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
 }
 
 void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
   for(const auto& force_vars : slip_force_vars_){
-    prog_->SetInitialGuess(force_vars, drake::VectorX<double>::Zero(force_vars.size()));
+    //TODO find better initial guess
+    prog_->SetInitialGuess(force_vars, 500 * drake::VectorX<double>::Ones(force_vars.size()));
   }
   KinematicCentroidalMPC::SetForceGuess(force_trajectory);
 }
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 6464367667..da8b84fe5a 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -245,7 +245,6 @@ void KinematicCentroidalMPC::Build(
         AddKinematicsIntegrator(knot_point);
         if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
           AddSlipReductionConstraint(knot_point + 1);
-          //TODO why do I need these constraints here, do I need these constraints below?
           AddContactConstraints(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
           AddFrictionConeConstraints(knot_point + 1);

From 8c51d747ab155c248fd214621d58a6998b097df5 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 8 Dec 2022 13:22:13 -0500
Subject: [PATCH 51/76] Making some progress on walking

---
 .../cassie_kcmpc_trajopt.cc                   |  5 +--
 .../cassie_kinematic_centroidal_mpc.cc        | 31 +++++++++++++------
 .../kinematic_centroidal_mpc.cc               |  4 +--
 3 files changed, 26 insertions(+), 14 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 8df8e0735f..62b785bda2 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -1,5 +1,5 @@
 #include <iostream>
-
+#include<unistd.h>               // for linux 
 #include <drake/common/yaml/yaml_io.h>
 #include <drake/geometry/drake_visualizer.h>
 #include <drake/geometry/meshcat_visualizer.h>
@@ -74,7 +74,7 @@ int DoMain(int argc, char* argv[]) {
   // Create reference
   // TODO(yangwill): move this into the reference generator
   // Specify knot points
-  std::vector<Gait> gait_samples = {stand, stand, stand};
+  std::vector<Gait> gait_samples = {stand, walk, stand};
   DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size());
   std::vector<double> durations = std::vector<double>(gait_samples.size());
   for (int i = 0; i < gait_samples.size(); ++i) {
@@ -207,6 +207,7 @@ int DoMain(int argc, char* argv[]) {
     simulator.set_target_realtime_rate(0.2);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
+    sleep(2);
   }
 }
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 55a8b84cc7..8b81e83a95 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -83,7 +83,9 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
   prog_->AddBoundingBoxConstraint(-force_bounds, force_bounds, slip_force_vars_[knot_point]);
 
   for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
-    prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 3);
+
+    prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 2);
+
     if(slip_contact_sequence_[knot_point][contact_index]){
       // Foot isn't moving
       prog_->AddBoundingBoxConstraint(
@@ -93,12 +95,9 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
       prog_->AddBoundingBoxConstraint(
           slip_ground_offset_, slip_ground_offset_, slip_contact_pos_vars(knot_point, contact_index)[2]);
 
-      prog_->AddConstraint(slip_force_vars_[knot_point][contact_index] + k_ * ( r0_ - (slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() ) >= 0);
+//      prog_->AddConstraint(slip_force_vars_[knot_point][contact_index] + k_ * ( r0_ - (slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() ) >= 0);
     }else{
-      if(!is_last_knot(knot_point) && slip_contact_sequence_[knot_point + 1][contact_index]){
-//        prog_->AddConstraint( r0_ - (slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() == 0);
-      }
-//      prog_->AddBoundingBoxConstraint(0, 0, slip_force_vars_[knot_point][contact_index]);
+      prog_->AddBoundingBoxConstraint(0, 0, slip_force_vars_[knot_point][contact_index]);
       // Feet are above the ground
       double lb = 0;
       // Check if at least one of the time points before or after is also in
@@ -110,8 +109,8 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
         lb = swing_foot_minimum_height_;
       }
       lb +=slip_ground_offset_;
-//      prog_->AddBoundingBoxConstraint(
-//          lb, 0.5, slip_contact_pos_vars(knot_point, contact_index)[2]);
+      prog_->AddBoundingBoxConstraint(
+          lb, 0.5, slip_contact_pos_vars(knot_point, contact_index)[2]);
     }
   }
 }
@@ -123,7 +122,7 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double term
                                  com_ref_traj_->value(t),
                                  slip_com_vars_[knot_point]);
   }
-  // TODO add Velocity cost
+  // TODO add com Velocity cost
   if (contact_ref_traj_) {
     const Eigen::MatrixXd Q_contact_pos =gains_.contact_pos.asDiagonal();
     prog_->AddQuadraticErrorCost(
@@ -260,7 +259,19 @@ drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_p
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-    return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
+  for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++) {
+//    Eigen::Vector3d foot_rt_com = (result_->GetSolution(slip_com_vars_[knot_point]) -
+//        result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index)));
+//      std::cout<<"Knot point "<< knot_point << "Contact " << contact_index <<std::endl;
+//////    std::cout<<result_->GetSolution(slip_com_vars_[knot_point])<<std::endl;
+//////    std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))<<std::endl;
+////    Eigen::Vector3d r_bounds = {2, 2, 2};
+////
+////    std::cout<< ((r_bounds[0] >= foot_rt_com[0]) && (r_bounds[1] >= foot_rt_com[1]) && (r_bounds[2] >= foot_rt_com[2])) << std::endl;
+////    std::cout<< ((foot_rt_com[0] >= -r_bounds[0]) && (foot_rt_com[1] >= -r_bounds[1]) && (foot_rt_com[2] >= -r_bounds[2])) << std::endl;
+//    std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))<<std::endl;
+  }
+      return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
 }
 
 void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index da8b84fe5a..6f77d247a5 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -247,7 +247,7 @@ void KinematicCentroidalMPC::Build(
           AddSlipReductionConstraint(knot_point + 1);
           AddContactConstraints(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
-          AddFrictionConeConstraints(knot_point + 1);
+//          AddFrictionConeConstraints(knot_point + 1);
           AddFlightContactForceConstraints(knot_point + 1);
         }
         break;
@@ -281,7 +281,7 @@ void KinematicCentroidalMPC::AddConstantMomentumReference(
 
 void KinematicCentroidalMPC::AddCosts() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    const double terminal_gain = is_last_knot(knot_point) ? 100 : 1;
+    const double terminal_gain = is_last_knot(knot_point) ? 1 : 1;
     const double collocation_gain =
         (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
     double t = dt_ * knot_point;

From 40b71714f39b08b243d74d9cce1d496615baeea6 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 8 Dec 2022 17:24:15 -0500
Subject: [PATCH 52/76] Walking is working somewhat well

---
 .../kinematic_centroidal_mpc/BUILD.bazel      |  1 +
 .../cassie_kcmpc_trajopt.cc                   | 12 +++---
 .../cassie_kinematic_centroidal_mpc.cc        | 39 +++++++++----------
 .../cassie_kinematic_centroidal_mpc.h         |  3 ++
 .../gaits/BUILD.bazel                         |  2 +
 .../kinematic_centroidal_mpc/gaits/run.yaml   | 14 +++++++
 .../simple_models/planar_slip_constraints.cc  |  8 +++-
 .../simple_models/planar_slip_constraints.h   |  3 +-
 .../trajectory_parameters.yaml                |  2 +-
 .../kinematic_centroidal_mpc.cc               |  2 +-
 10 files changed, 56 insertions(+), 30 deletions(-)
 create mode 100644 examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml

diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
index ad92c2d094..6c195bd040 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
@@ -28,6 +28,7 @@ cc_binary(
             "//examples/Cassie/kinematic_centroidal_mpc/gaits:stand.yaml",
             "//examples/Cassie/kinematic_centroidal_mpc/gaits:walk.yaml",
             "//examples/Cassie/kinematic_centroidal_mpc/gaits:step.yaml",
+            "//examples/Cassie/kinematic_centroidal_mpc/gaits:run.yaml",
             "//examples/Cassie/kinematic_centroidal_mpc:trajectory_parameters.yaml"],
     deps = [
         ":centroidal_mpc",
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 62b785bda2..1e3a6beca0 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -70,11 +70,13 @@ int DoMain(int argc, char* argv[]) {
       "examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml");
   auto step = drake::yaml::LoadYamlFile<Gait>(
       "examples/Cassie/kinematic_centroidal_mpc/gaits/step.yaml");
-
+  auto run = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml");
+  walk.period = 1.6;
   // Create reference
   // TODO(yangwill): move this into the reference generator
   // Specify knot points
-  std::vector<Gait> gait_samples = {stand, walk, stand};
+  std::vector<Gait> gait_samples = {stand, walk, walk};
   DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size());
   std::vector<double> durations = std::vector<double>(gait_samples.size());
   for (int i = 0; i < gait_samples.size(); ++i) {
@@ -93,12 +95,12 @@ int DoMain(int argc, char* argv[]) {
   // Create MPC and set gains
   CassieKinematicCentroidalMPC mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 6000, traj_params.com_height, traj_params.stance_width);
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 5000, 10,0.9, traj_params.stance_width);
   mpc.SetGains(gains);
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
-  for(int i = 1; i <traj_params.n_knot_points ; i++){
+  for(int i = 5; i <traj_params.n_knot_points ; i++){
     complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
@@ -204,7 +206,7 @@ int DoMain(int argc, char* argv[]) {
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(0.2);
+    simulator.set_target_realtime_rate(1);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
     sleep(2);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 8b81e83a95..345d604c7e 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -78,10 +78,6 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
 }
 
 void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
-
-  Eigen::Vector2d force_bounds = {800.0, 800.0};
-  prog_->AddBoundingBoxConstraint(-force_bounds, force_bounds, slip_force_vars_[knot_point]);
-
   for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
 
     prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 2);
@@ -94,8 +90,6 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
       // Foot is on the ground
       prog_->AddBoundingBoxConstraint(
           slip_ground_offset_, slip_ground_offset_, slip_contact_pos_vars(knot_point, contact_index)[2]);
-
-//      prog_->AddConstraint(slip_force_vars_[knot_point][contact_index] + k_ * ( r0_ - (slip_com_vars_[knot_point]-slip_contact_pos_vars(knot_point,contact_index)).norm() ) >= 0);
     }else{
       prog_->AddBoundingBoxConstraint(0, 0, slip_force_vars_[knot_point][contact_index]);
       // Feet are above the ground
@@ -122,7 +116,22 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double term
                                  com_ref_traj_->value(t),
                                  slip_com_vars_[knot_point]);
   }
-  // TODO add com Velocity cost
+
+  // Project linear momentum
+  if (mom_ref_traj_){
+    const Eigen::MatrixXd Q_vel =gains_.lin_momentum.asDiagonal() * m_;
+    prog_->AddQuadraticErrorCost(terminal_gain * Q_vel,
+                                 Eigen::VectorXd(mom_ref_traj_->value(t)).tail(3),
+                                 slip_vel_vars_[knot_point]);
+  }
+  // Project com velocity
+  if(v_ref_traj_){
+    const Eigen::MatrixXd Q_vel =Q_v_.block(3,3,3,3);
+    prog_->AddQuadraticErrorCost(terminal_gain * Q_vel,
+                                 Eigen::VectorXd(v_ref_traj_->value(t)).segment(4,3),
+                                 slip_vel_vars_[knot_point]);
+  }
+
   if (contact_ref_traj_) {
     const Eigen::MatrixXd Q_contact_pos =gains_.contact_pos.asDiagonal();
     prog_->AddQuadraticErrorCost(
@@ -197,7 +206,7 @@ void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
   if(!is_last_knot(knot_point)) {
     auto slip_com_dynamics =
         std::make_shared<PlanarSlipDynamicsConstraint<double>>(
-            r0_, k_, m_, 2, slip_contact_sequence_[knot_point], dt_, knot_point);
+            r0_, k_, b_,m_, 2, slip_contact_sequence_[knot_point], dt_, knot_point);
 
     prog_->AddConstraint(slip_com_dynamics,
                          {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], slip_force_vars_[knot_point],
@@ -259,25 +268,13 @@ drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_p
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-  for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++) {
-//    Eigen::Vector3d foot_rt_com = (result_->GetSolution(slip_com_vars_[knot_point]) -
-//        result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index)));
-//      std::cout<<"Knot point "<< knot_point << "Contact " << contact_index <<std::endl;
-//////    std::cout<<result_->GetSolution(slip_com_vars_[knot_point])<<std::endl;
-//////    std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))<<std::endl;
-////    Eigen::Vector3d r_bounds = {2, 2, 2};
-////
-////    std::cout<< ((r_bounds[0] >= foot_rt_com[0]) && (r_bounds[1] >= foot_rt_com[1]) && (r_bounds[2] >= foot_rt_com[2])) << std::endl;
-////    std::cout<< ((foot_rt_com[0] >= -r_bounds[0]) && (foot_rt_com[1] >= -r_bounds[1]) && (foot_rt_com[2] >= -r_bounds[2])) << std::endl;
-//    std::cout<<result_->GetSolution(slip_contact_pos_vars(knot_point, contact_index))<<std::endl;
-  }
       return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
 }
 
 void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
   for(const auto& force_vars : slip_force_vars_){
     //TODO find better initial guess
-    prog_->SetInitialGuess(force_vars, 500 * drake::VectorX<double>::Ones(force_vars.size()));
+    prog_->SetInitialGuess(force_vars, 0 * drake::VectorX<double>::Ones(force_vars.size()));
   }
   KinematicCentroidalMPC::SetForceGuess(force_trajectory);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index a97acb8317..e4dc199dae 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -17,6 +17,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
                                double mu,
                                const drake::VectorX<double>& nominal_stand,
                                double k = 1000,
+                               double b = 20,
                                double r0 = 0.5,
                                double stance_width = 0.2) :
       KinematicCentroidalMPC(plant, n_knot_points, dt, CreateContactPoints(plant, mu)),
@@ -25,6 +26,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
       loop_closure_evaluators(Plant()),
       slip_contact_sequence_(n_knot_points),
       k_(k),
+      b_(b),
       r0_(r0){
     AddPlantJointLimits(dairlib::JointNames());
     AddLoopClosure();
@@ -135,6 +137,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   std::vector<std::vector<bool>> slip_contact_sequence_;
   double k_;
   double r0_;
+  double b_;
   double m_;
 
   const double slip_ground_offset_ = 0;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel b/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel
index 891835672d..c5a8d79ca4 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel
@@ -5,3 +5,5 @@ exports_files(["stand.yaml"])
 exports_files(["walk.yaml"])
 
 exports_files(["step.yaml"])
+
+exports_files(["run.yaml"])
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml b/examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml
new file mode 100644
index 0000000000..f676a9c7e7
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml
@@ -0,0 +1,14 @@
+gait_pattern:
+  - start_phase: 0.0
+    end_phase: 0.45
+    contact_status: [true, true, false, false]
+  - start_phase: 0.45
+    end_phase: 0.5
+    contact_status: [false, false, false, false]
+  - start_phase: 0.5
+    end_phase: 0.95
+    contact_status: [false, false, true, true]
+  - start_phase: 0.95
+    end_phase: 1.0
+    contact_status: [false, false, false, false]
+period: 1.0
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index c2e4da9870..17d53504b6 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -117,6 +117,7 @@ void PlanarSlipLiftingConstraint::EvaluateConstraint(const Eigen::Ref<const drak
 template<typename T>
 PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
                                                               double k,
+                                                              double b,
                                                               T m,
                                                               int n_feet,
                                                               std::vector<bool> contact_mask,
@@ -129,6 +130,7 @@ PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
         std::to_string(knot_index) + "]"),
                                                                               r0_(r0),
                                                                               k_(k),
+                                                                              b_(b),
                                                                               m_(m),
                                                                               n_feet_(n_feet),
                                                                               contact_mask_(std::move(contact_mask)),
@@ -178,7 +180,11 @@ drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
       drake::Vector3<T> com_rt_foot = com_position - contact_loc.segment(3 * foot, 3);
       const auto r = com_rt_foot.norm();
       const auto unit_vec = com_rt_foot/r;
-      const auto F = k_ * (r0_ - r) + slip_force[foot];
+      const auto dr = com_vel.dot(unit_vec);
+      auto F = k_ * (r0_ - r) + slip_force[foot] - b_ * dr;
+      if(F < 0){
+        F = 0;
+      }
       ddcom = ddcom + F * unit_vec / m_;
     }
   }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
index 06e4d78d77..c96c863368 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
@@ -65,7 +65,7 @@ template <typename T>
 class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
 
  public:
-  PlanarSlipDynamicsConstraint(double r0, double k, T m, int n_feet, std::vector<bool>  contact_mask, double dt, int knot_index);
+  PlanarSlipDynamicsConstraint(double r0, double k, double b, T m, int n_feet, std::vector<bool>  contact_mask, double dt, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
@@ -79,6 +79,7 @@ class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstrain
 
   const double r0_;
   const double k_;
+  const double b_;
   const T m_;
   const int n_feet_;
   const std::vector<bool> contact_mask_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml b/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
index d2681a7361..02bfc91688 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
+++ b/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
@@ -3,5 +3,5 @@ duration_scaling:
   [1.0, 3.0, 1.0]
 com_height: 0.8
 stance_width: 0.25
-vel: 0.0
+vel: 0.5
 tol: 1e-2
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 6f77d247a5..18e1f4ef5e 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -281,7 +281,7 @@ void KinematicCentroidalMPC::AddConstantMomentumReference(
 
 void KinematicCentroidalMPC::AddCosts() {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    const double terminal_gain = is_last_knot(knot_point) ? 1 : 1;
+    const double terminal_gain = is_last_knot(knot_point) ? 50 : 1;
     const double collocation_gain =
         (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
     double t = dt_ * knot_point;

From 39b9e54824f6f5d0452c43115fed7e091d7e2f9e Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 9 Dec 2022 11:42:01 -0500
Subject: [PATCH 53/76] Properly setting r0

---
 .../Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc  | 5 +++--
 .../kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc     | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 1e3a6beca0..95f788de88 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -95,12 +95,13 @@ int DoMain(int argc, char* argv[]) {
   // Create MPC and set gains
   CassieKinematicCentroidalMPC mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 5000, 10,0.9, traj_params.stance_width);
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 5000, 10,
+      sqrt(pow(traj_params.com_height,2) + pow(traj_params.stance_width,2)), traj_params.stance_width);
   mpc.SetGains(gains);
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
-  for(int i = 5; i <traj_params.n_knot_points ; i++){
+  for(int i = 1; i <traj_params.n_knot_points ; i++){
     complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 18e1f4ef5e..8aa72517e3 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -247,7 +247,7 @@ void KinematicCentroidalMPC::Build(
           AddSlipReductionConstraint(knot_point + 1);
           AddContactConstraints(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
-//          AddFrictionConeConstraints(knot_point + 1);
+          AddFrictionConeConstraints(knot_point + 1);
           AddFlightContactForceConstraints(knot_point + 1);
         }
         break;

From e7173815adec8333b8708dd69f68ec4f43331733 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 9 Dec 2022 12:00:39 -0500
Subject: [PATCH 54/76] Fixed lifting function toe velocity

---
 .../simple_models/planar_slip_lifter.cc       | 28 +++++++++++--------
 1 file changed, 17 insertions(+), 11 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index 19faf76e09..54c2ee0fca 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -99,23 +99,31 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
                                                                const drake::VectorX<double>& slip_feet_velocities)const {
   DRAKE_DEMAND(slip_feet_velocities.size() == 3*slip_contact_points_.size());
   // Preallocate linear constraint
-  drake::MatrixX<double> A(3 + 3 * slip_contact_points_.size() ,n_v_); // 3 rows for linear momentum, 3 rows for each slip foot
-  drake::VectorX<double> b(3 + 3 * slip_contact_points_.size());
+  drake::MatrixX<double> A(3 + 2 * 3 * slip_contact_points_.size() ,n_v_); // 3 rows for linear momentum, 3 rows for each slip foot, 3 rows for each slip foot
+  drake::VectorX<double> b(3 + 2 * 3 * slip_contact_points_.size());
 
+  // order of constraints are: slip foot velocity, linear momentum, zero toe rotation
+  // Zero toe rotation accomplished by constraining per foot complex contact velocities to be equal
+
+  // set b for linear momentum
   b.segment(3 * slip_contact_points_.size(), 3) = drake::VectorX<double>::Zero(3);
-  b.tail(3) = linear_momentum;
+  // Set b for zero toe rotation
+  b.tail(3 * slip_contact_points_.size()) = drake::VectorX<double>::Zero(3 * slip_contact_points_.size());
 
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
-
-  slip_contact_points_[0].EvalFull(*context_ );
-
   for(int i = 0; i < slip_contact_points_.size(); i++){
+    // Set A and b for slip foot velocity constraint
     b.segment(3 * i, 3) =slip_feet_velocities.segment(3 * i, 3);
-    slip_contact_points_[i].EvalFullJacobian(*context_ );
-    A.middleRows(3 * i, 3) = slip_contact_points_[i].EvalFullJacobian(*context_ );
+    A.middleRows(3 * i, 3) = slip_contact_points_[i].EvalFullJacobian(*context_);
+    // Set A for zero toe rotation
+    const auto contact_it = simple_foot_index_to_complex_foot_index_.find(i);
+    A.middleRows(3 * slip_contact_points_.size() + 3 + 3 * i, 3) =
+        complex_contact_points_[contact_it->second[0]].EvalFullJacobian(*context_)
+            - complex_contact_points_[contact_it->second[1]].EvalFullJacobian(*context_);
   }
 
   // Finite difference to calculate momentum jacobian
+  // TODO replace this with analytical gradient
   drake::VectorX<double> x_val = drake::VectorX<double>::Zero(n_v_);
   drake::VectorX<double> yi(6);
   dairlib::multibody::SetVelocitiesIfNew<double>(plant_,x_val,context_);
@@ -127,9 +135,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
     x_val(i) -= eps;
     A.col(i).tail(3) = (plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos).translational() - y0) / eps;
   }
-
-  // TODO figure out what to do about toe velocity
-
+  
   // solve
   drake::VectorX<double> rv(n_v_);
   // Set base angular velocity to zero

From ac577d9c07bb2741e0c760d3286b8c31301ecf6f Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 9 Dec 2022 12:38:52 -0500
Subject: [PATCH 55/76] Added damping, but realized com velocity is broken

---
 .../cassie_kinematic_centroidal_mpc.h             |  1 +
 .../simple_models/planar_slip_constraints.cc      |  3 ++-
 .../simple_models/planar_slip_lifter.cc           | 15 ++++++++++-----
 .../simple_models/planar_slip_lifter.h            |  9 ++++++---
 .../simple_models/planar_slip_lifting_test.cc     |  5 ++++-
 .../simple_models/planar_slip_reducer.cc          | 15 ++++++++++++---
 .../simple_models/planar_slip_reducer.h           |  3 +++
 7 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index e4dc199dae..94d4c4bca6 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -50,6 +50,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
                                                  {{0, {0, 1}}, {1, {2, 3}}},
                                                  nominal_stand,
                                                  k,
+                                                 b,
                                                  r0));
       reducers.emplace_back(new PlanarSlipReducer(plant,
                                                  contexts_[knot].get(),
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index 17d53504b6..8d5cd79732 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -68,7 +68,8 @@ void SlipGrfReductionConstrain::EvaluateConstraint(const Eigen::Ref<const drake:
   const auto& slip_contact_pos = x.segment(3,3*n_slip_feet_);
   const auto& complex_grf = x.segment(3 + 3*n_slip_feet_,3*n_complex_feet_);
   const auto& slip_contact_force = x.segment(3 + 3*n_slip_feet_ + 3*n_complex_feet_,n_slip_feet_);
-  *y = reducing_function_->ReduceGrf(complex_com, slip_contact_pos, complex_grf) - slip_contact_force;
+  //TODO eventually fix
+//  *y = reducing_function_->ReduceGrf(complex_com, <#initializer#>, slip_contact_pos, complex_grf) - slip_contact_force;
 
 }
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index 54c2ee0fca..eb7072bb1a 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -10,12 +10,14 @@ PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double
                                       const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
                                       const drake::VectorX<double> &nominal_stand,
                                       double k,
+                                      double b,
                                       double r0):
     plant_(plant),
     context_(context),
     ik_(plant, context),
     m_(plant.CalcTotalMass(*context)),
     k_(k),
+    b_(b),
     r0_(r0),
     slip_contact_points_(slip_contact_points),
     complex_contact_points_(complex_contact_points),
@@ -135,7 +137,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
     x_val(i) -= eps;
     A.col(i).tail(3) = (plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos).translational() - y0) / eps;
   }
-  
+
   // solve
   drake::VectorX<double> rv(n_v_);
   // Set base angular velocity to zero
@@ -166,15 +168,17 @@ drake::VectorX<double> PlanarSlipLifter::LiftContactVel(const drake::VectorX<dou
 }
 
 drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &com_pos,
-                                               const drake::VectorX<double> &slip_feet_pos,
-                                               const drake::VectorX<double> &slip_force,
-                                               const drake::VectorX<double> &complex_contact_point_pos)const {
+                                                 const drake::VectorX<double> &com_vel,
+                                                 const drake::VectorX<double> &slip_feet_pos,
+                                                 const drake::VectorX<double> &slip_force,
+                                                 const drake::VectorX<double> &complex_contact_point_pos) const {
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
   // Loop through the slip feet
   for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
     // Calculate the slip grf
     double r = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).norm();
-    double slip_grf_mag = slip_force[simple_index] + k_ * (r0_ - r);
+    double dr = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).normalized().dot(com_vel);
+    double slip_grf_mag = slip_force[simple_index] + k_ * (r0_ - r) - b_ * dr;
 
     // Find the average location for all of the complex contact points that make up the SLIP foot
     drake::Vector3<double> average_pos = drake::VectorX<double>::Zero(3);
@@ -229,6 +233,7 @@ void PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip
 
   (*complex_state) << slip_com, plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, slip_com)
       .get_coeffs(), complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel), LiftGrf(slip_com,
+                                                                                                    slip_vel,
                                                                                                     slip_contact_pos,
                                                                                                     slip_force,
                                                                                                     complex_contact_pos),
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
index 7ec1da8d5a..ffb0f355b6 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -19,6 +19,7 @@ class PlanarSlipLifter {
                    const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
                    const drake::VectorX<double>& nominal_stand,
                    double k,
+                   double b,
                    double r0);
 
   void Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
@@ -56,10 +57,11 @@ class PlanarSlipLifter {
 
   drake::VectorX<double> LiftContactVel(const drake::VectorX<double>& generalized_pos, const drake::VectorX<double>& generalized_vel) const;
 
-  drake::VectorX<double> LiftGrf(const drake::VectorX<double>& com_pos,
-                            const drake::VectorX<double>& slip_feet_pos,
+  drake::VectorX<double> LiftGrf(const drake::VectorX<double> &com_pos,
+                                 const drake::VectorX<double> &com_vel,
+                                 const drake::VectorX<double> &slip_feet_pos,
                                  const drake::VectorX<double> &slip_force,
-                                 const drake::VectorX<double>& complex_contact_point_pos) const;
+                                 const drake::VectorX<double> &complex_contact_point_pos) const;
 
 
   const drake::multibody::MultibodyPlant<double>& plant_;
@@ -67,6 +69,7 @@ class PlanarSlipLifter {
   mutable drake::multibody::InverseKinematics ik_;
   const double m_;
   const double k_;
+  const double b_;
   const double r0_;
   const std::vector<double> stance_widths_;
   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index cd36c6f3be..63bfac2a47 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -85,6 +85,7 @@ int main(int argc, char* argv[]) {
                           {{0, {0, 1}}, {1, {2, 3}}},
                           reference_state.head(plant.num_positions()),
                           2000,
+                          0,
                           0.5);
   PlanarSlipReducer reducer(plant,
                           context.get(),
@@ -92,6 +93,7 @@ int main(int argc, char* argv[]) {
                           {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
                           {{0, {0, 1}}, {1, {2, 3}}},
                           2000,
+                          0,
                           0.5);
 
   Eigen::Vector3d slip_com = {0.2,0,0.7};
@@ -121,7 +123,8 @@ int main(int argc, char* argv[]) {
   drake::VectorX<double> input(slip_state.size() + complex_state.size());
   input << slip_state,complex_state;
   constraint.DoEval(input, &error);
-  std::cout<<"Max Error in inverse test: "<< error.cwiseAbs().maxCoeff() << std::endl;
+//  std::cout<<"Max Error in inverse test: "<< error.cwiseAbs().maxCoeff() << std::endl;
+  std::cout<<error<<std::endl;
 
   if(true){
     // Build temporary diagram for visualization
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
index b59e8fc24d..a012946718 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
@@ -8,9 +8,11 @@ PlanarSlipReducer::PlanarSlipReducer(const drake::multibody::MultibodyPlant<doub
                                      const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
                                      const std::map<int, std::vector<int>> &simple_foot_index_to_complex_foot_index,
                                      double k,
+                                     double b,
                                      double r0):plant_(plant),
                                                 context_(context),
                                                 k_(k),
+                                                b_(b),
                                                 r0_(r0),
                                                 m_(plant.CalcTotalMass(*context)),
                                                 slip_contact_points_(slip_contact_points),
@@ -48,7 +50,12 @@ void PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::VectorX<double>> &c
     slip_state->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_points_[i].EvalFull(*context_);
     slip_state->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * slip_contact_points_.size()+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_points_[i].EvalFullTimeDerivative(*context_);
   }
-  slip_state->tail(slip_contact_points_.size()) = ReduceGrf(complex_com, slip_state->segment(kSLIP_DIM+kSLIP_DIM, slip_contact_points_.size()+kSLIP_DIM), complex_grf);
+  slip_state->tail(slip_contact_points_.size()) = ReduceGrf(complex_com,
+                                                            complex_lin_momentum/m_,
+                                                            slip_state->segment(kSLIP_DIM + kSLIP_DIM,
+                                                                                slip_contact_points_.size()
+                                                                                    + kSLIP_DIM),
+                                                            complex_grf);
 }
 
 drake::VectorX<double> PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state) const {
@@ -58,12 +65,14 @@ drake::VectorX<double> PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::V
 }
 
 drake::VectorX<double> PlanarSlipReducer::ReduceGrf(const Eigen::Ref<const drake::VectorX<double>> &complex_com,
+                                                    const Eigen::Ref<const drake::VectorX<double>> &com_vel,
                                                     const Eigen::Ref<const drake::VectorX<double>> &slip_contact_pos,
-                                                    const Eigen::Ref<const drake::VectorX<double>> &complex_grf)const {
+                                                    const Eigen::Ref<const drake::VectorX<double>> &complex_grf) const {
   Eigen::VectorXd slip_force(slip_contact_points_.size());
   for(int i = 0; i<slip_contact_points_.size(); i++){
     const double r = (complex_com - slip_contact_pos.segment(3 * i, 3)).norm();
-    const double spring_force = k_ * (r0_ - r);
+    const double dr =  (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized().dot(com_vel);
+    const double spring_force = k_ * (r0_ - r) - b_ * dr;
     auto complex_feet_it = simple_foot_index_to_complex_foot_index_.find(i);
     Eigen::Vector3d complex_force = Eigen::Vector3d::Zero(3);
     for(const auto complex_index : complex_feet_it->second){
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
index 2f9c719b12..90e18e84cb 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
@@ -18,6 +18,7 @@ class PlanarSlipReducer {
                    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& complex_contact_points,
                    const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
                    double k,
+                   double b,
                    double r0);
 
   void Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state,
@@ -26,12 +27,14 @@ class PlanarSlipReducer {
   drake::VectorX<double> Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state) const;
 
   drake::VectorX<double> ReduceGrf(const Eigen::Ref<const drake::VectorX<double>> &complex_com,
+                                   const Eigen::Ref<const drake::VectorX<double>> &com_vel,
                                    const Eigen::Ref<const drake::VectorX<double>> &slip_contact_pos,
                                    const Eigen::Ref<const drake::VectorX<double>> &complex_grf) const;
  private:
   const drake::multibody::MultibodyPlant<double>& plant_;
   mutable drake::systems::Context<double>* context_;
   const double k_;
+  const double b_;
   const double r0_;
   const double m_;
   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;

From 8b7bd24f0d8da848c06883a0399353718fd92604 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 9 Dec 2022 12:50:03 -0500
Subject: [PATCH 56/76] Fixed reduction function

---
 .../simple_models/planar_slip_lifter.cc                   | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index eb7072bb1a..bf9bc08196 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -104,13 +104,11 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
   drake::MatrixX<double> A(3 + 2 * 3 * slip_contact_points_.size() ,n_v_); // 3 rows for linear momentum, 3 rows for each slip foot, 3 rows for each slip foot
   drake::VectorX<double> b(3 + 2 * 3 * slip_contact_points_.size());
 
-  // order of constraints are: slip foot velocity, linear momentum, zero toe rotation
+  // order of constraints are: slip foot velocity,  zero toe rotation, linear momentum,
   // Zero toe rotation accomplished by constraining per foot complex contact velocities to be equal
 
   // set b for linear momentum
-  b.segment(3 * slip_contact_points_.size(), 3) = drake::VectorX<double>::Zero(3);
-  // Set b for zero toe rotation
-  b.tail(3 * slip_contact_points_.size()) = drake::VectorX<double>::Zero(3 * slip_contact_points_.size());
+  b.tail(3) = linear_momentum;
 
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
   for(int i = 0; i < slip_contact_points_.size(); i++){
@@ -119,7 +117,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
     A.middleRows(3 * i, 3) = slip_contact_points_[i].EvalFullJacobian(*context_);
     // Set A for zero toe rotation
     const auto contact_it = simple_foot_index_to_complex_foot_index_.find(i);
-    A.middleRows(3 * slip_contact_points_.size() + 3 + 3 * i, 3) =
+    A.middleRows(3 * slip_contact_points_.size() + 3 * i, 3) =
         complex_contact_points_[contact_it->second[0]].EvalFullJacobian(*context_)
             - complex_contact_points_[contact_it->second[1]].EvalFullJacobian(*context_);
   }

From 9bd340729f57f56d6fc93e870b942a8eea1619c4 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 9 Dec 2022 13:05:16 -0500
Subject: [PATCH 57/76] Fixed grf reduction constraint slip velocity

---
 .../cassie_kinematic_centroidal_mpc.cc             |  2 +-
 .../simple_models/planar_slip_constraints.cc       | 14 +++++++-------
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 345d604c7e..7ecbe54a8c 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -187,7 +187,7 @@ void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
         plant_, reducers[knot_point], 2, 4,knot_point);
   //TODO why is this constraint bad, probably due to it not thinking about the contact mode
 //  prog_->AddConstraint(grf_constraint,
-//                       {com_pos_vars(knot_point), slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
+//                       {com_pos_vars(knot_point), slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
 
 
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index 8d5cd79732..c1ccaeb18c 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -49,7 +49,7 @@ SlipGrfReductionConstrain::SlipGrfReductionConstrain(const drake::multibody::Mul
                                                      int n_complex_feet,
                                                      int knot_index):dairlib::solvers::NonlinearConstraint<double>(
     n_slip_feet,
-    3 + 3 * n_slip_feet + 3 * n_complex_feet + n_slip_feet,
+    3 + 3 * n_slip_feet + 3 * n_complex_feet + n_slip_feet + 3,
     Eigen::VectorXd::Zero(n_slip_feet),
     Eigen::VectorXd::Zero(n_slip_feet),
     "SlipGrfReductionConstraint[" +
@@ -59,18 +59,18 @@ SlipGrfReductionConstrain::SlipGrfReductionConstrain(const drake::multibody::Mul
                                                                      n_complex_feet_(n_complex_feet) {}
 /// Input is of the form:
 ///     complex_com
+///     slip com velocity
 ///     slip_contact_pos
 ///     complex_grf
 ///     slip_contact_force
 void SlipGrfReductionConstrain::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>> &x,
                                                    drake::VectorX<double> *y) const {
   const auto& complex_com = x.head(3);
-  const auto& slip_contact_pos = x.segment(3,3*n_slip_feet_);
-  const auto& complex_grf = x.segment(3 + 3*n_slip_feet_,3*n_complex_feet_);
-  const auto& slip_contact_force = x.segment(3 + 3*n_slip_feet_ + 3*n_complex_feet_,n_slip_feet_);
-  //TODO eventually fix
-//  *y = reducing_function_->ReduceGrf(complex_com, <#initializer#>, slip_contact_pos, complex_grf) - slip_contact_force;
-
+  const auto& slip_vel = x.segment(3,3);
+  const auto& slip_contact_pos = x.segment(3 + 3,3*n_slip_feet_);
+  const auto& complex_grf = x.segment(3 + 3*n_slip_feet_ + 3,3*n_complex_feet_);
+  const auto& slip_contact_force = x.segment(3 + 3*n_slip_feet_ + 3*n_complex_feet_ + 3,n_slip_feet_);
+  *y = reducing_function_->ReduceGrf(complex_com, slip_vel, slip_contact_pos, complex_grf) - slip_contact_force;
 }
 
 PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double> &plant,

From 50ad6bc4639941355edce2c83a4c50b3579e5411 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 9 Dec 2022 16:11:50 -0500
Subject: [PATCH 58/76] Still strugging with grf constraint

---
 .../cassie_kinematic_centroidal_mpc.cc        | 25 ++++++++++++--
 .../cassie_kinematic_centroidal_mpc.h         | 23 +++----------
 .../simple_models/planar_slip_constraints.cc  |  2 +-
 .../simple_models/planar_slip_lifter.cc       | 24 +++++++-------
 .../simple_models/planar_slip_lifter.h        | 18 +++++-----
 .../simple_models/planar_slip_lifting_test.cc | 24 +++++++-------
 .../simple_models/planar_slip_reducer.cc      | 33 +++++++++++--------
 .../simple_models/planar_slip_reducer.h       | 20 ++++++-----
 .../kinematic_centroidal_mpc.h                |  2 +-
 9 files changed, 94 insertions(+), 77 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 7ecbe54a8c..d7008d6418 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -185,11 +185,9 @@ void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
   auto grf_constraint =
     std::make_shared<SlipGrfReductionConstrain>(
         plant_, reducers[knot_point], 2, 4,knot_point);
-  //TODO why is this constraint bad, probably due to it not thinking about the contact mode
+  //TODO why is this constraint bad
 //  prog_->AddConstraint(grf_constraint,
 //                       {com_pos_vars(knot_point), slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
-
-
 }
 
 void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
@@ -278,3 +276,24 @@ void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::Piec
   }
   KinematicCentroidalMPC::SetForceGuess(force_trajectory);
 }
+void CassieKinematicCentroidalMPC::Build(const drake::solvers::SolverOptions &solver_options) {
+  for (int knot = 0; knot < n_knot_points_; knot++) {
+    lifters_.emplace_back(new PlanarSlipLifter(plant_,
+                                               contexts_[knot].get(),
+                                               slip_contact_points_,
+                                               CreateContactPoints(plant_, 0),
+                                               {{0, {0, 1}}, {1, {2, 3}}},
+                                               nominal_stand_,
+                                               k_,
+                                               b_,
+                                               r0_, slip_contact_sequence_[knot]));
+    reducers.emplace_back(new PlanarSlipReducer(plant_,
+                                                contexts_[knot].get(),
+                                                slip_contact_points_,
+                                                CreateContactPoints(plant_, 0),
+                                                {{0, {0, 1}}, {1, {2, 3}}},
+                                                k_,
+                                                r0_, 0, slip_contact_sequence_[knot]));
+  }
+  KinematicCentroidalMPC::Build(solver_options);
+}
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index 94d4c4bca6..a24de5ed0a 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -26,8 +26,9 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
       loop_closure_evaluators(Plant()),
       slip_contact_sequence_(n_knot_points),
       k_(k),
+      r0_(r0),
       b_(b),
-      r0_(r0){
+      nominal_stand_(nominal_stand){
     AddPlantJointLimits(dairlib::JointNames());
     AddLoopClosure();
 
@@ -43,23 +44,6 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
           2*3, "slip_contact_vel_" + std::to_string(knot)));
       slip_force_vars_.push_back(prog_->NewContinuousVariables(
           2, "slip_force_" + std::to_string(knot)));
-      lifters_.emplace_back(new PlanarSlipLifter(plant,
-                                                 contexts_[knot].get(),
-                                                 slip_contact_points_,
-                                                 CreateContactPoints(plant, mu),
-                                                 {{0, {0, 1}}, {1, {2, 3}}},
-                                                 nominal_stand,
-                                                 k,
-                                                 b,
-                                                 r0));
-      reducers.emplace_back(new PlanarSlipReducer(plant,
-                                                 contexts_[knot].get(),
-                                                 slip_contact_points_,
-                                                 CreateContactPoints(plant, mu),
-                                                 {{0, {0, 1}}, {1, {2, 3}}},
-                                                 k,
-                                                 r0));
-
     }
     std::vector<bool> stance_mode(2);
     std::fill(stance_mode.begin(), stance_mode.end(), true);
@@ -111,6 +95,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   void SetForceGuess(
       const drake::trajectories::PiecewisePolynomial<double>& force_trajectory) override;
 
+  void Build(const drake::solvers::SolverOptions& solver_options) override;
  private:
   void MapModeSequence();
 
@@ -140,7 +125,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
   double r0_;
   double b_;
   double m_;
-
+  const drake::VectorX<double> nominal_stand_;
   const double slip_ground_offset_ = 0;
 
   std::vector<drake::solvers::VectorXDecisionVariable> slip_com_vars_;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index c1ccaeb18c..462e552417 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -17,7 +17,7 @@ PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(const drake::multib
     "PlanarSlipReductionConstraint[" +
         std::to_string(knot_index) + "]"),
      reducing_function_(reducing_function),
-     slip_dim_(2 + 2 + 2 * 2 * n_slip_feet + n_slip_feet),
+     slip_dim_(3 + 3 + 2 * 3 * n_slip_feet + n_slip_feet),
      complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()) {}
 
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index bf9bc08196..3ab40f5823 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -4,14 +4,16 @@
 #include "multibody/multibody_utils.h"
 
 PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double> &plant,
-                                      drake::systems::Context<double> *context,
-                                      const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
-                                      const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
-                                      const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
-                                      const drake::VectorX<double> &nominal_stand,
-                                      double k,
-                                      double b,
-                                      double r0):
+                                   drake::systems::Context<double> *context,
+                                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
+                                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
+                                   const std::map<int,
+                                                  std::vector<int>> &simple_foot_index_to_complex_foot_index,
+                                   const drake::VectorX<double> &nominal_stand,
+                                   double k,
+                                   double b,
+                                   double r0,
+                                   const std::vector<bool>& contact_mask) :
     plant_(plant),
     context_(context),
     ik_(plant, context),
@@ -23,7 +25,8 @@ PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double
     complex_contact_points_(complex_contact_points),
     simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
     n_q_(plant.num_positions()),
-    n_v_(plant.num_velocities()){
+    n_v_(plant.num_velocities()),
+    slip_contact_mask_(contact_mask){
   ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), nominal_stand);
   com_vars_ = ik_.get_mutable_prog()->NewContinuousVariables(3);
   auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(&plant, std::nullopt, plant.world_frame(), context);
@@ -176,8 +179,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
     // Calculate the slip grf
     double r = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).norm();
     double dr = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).normalized().dot(com_vel);
-    double slip_grf_mag = slip_force[simple_index] + k_ * (r0_ - r) - b_ * dr;
-
+    double slip_grf_mag = slip_contact_mask_[simple_index] ? slip_force[simple_index] + k_ * (r0_ - r) - b_ * dr : 0;
     // Find the average location for all of the complex contact points that make up the SLIP foot
     drake::Vector3<double> average_pos = drake::VectorX<double>::Zero(3);
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
index ffb0f355b6..8856911778 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
@@ -12,15 +12,17 @@
 
 class PlanarSlipLifter {
  public:
-  PlanarSlipLifter(const drake::multibody::MultibodyPlant<double>& plant,
-                   drake::systems::Context<double>* context,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& slip_contact_points,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& complex_contact_points,
-                   const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
-                   const drake::VectorX<double>& nominal_stand,
+  PlanarSlipLifter(const drake::multibody::MultibodyPlant<double> &plant,
+                   drake::systems::Context<double> *context,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
+                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
+                   const std::map<int,
+                                  std::vector<int>> &simple_foot_index_to_complex_foot_index,
+                   const drake::VectorX<double> &nominal_stand,
                    double k,
                    double b,
-                   double r0);
+                   double r0,
+                   const std::vector<bool>& contact_mask);
 
   void Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
             drake::VectorX<double> *complex_state) const;
@@ -77,7 +79,7 @@ class PlanarSlipLifter {
   const std::map<int, std::vector<int>> simple_foot_index_to_complex_foot_index_;
   const int n_q_;
   const int n_v_;
-
+  const std::vector<bool> slip_contact_mask_;
   drake::solvers::VectorXDecisionVariable com_vars_;
 
   const int kSLIP_DIM = 3;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index 63bfac2a47..581182e7ad 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -78,6 +78,8 @@ int main(int argc, char* argv[]) {
 
   dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
 //  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
+
+  std::vector<bool> contact_mask = {true, true};
   PlanarSlipLifter lifter(plant,
                           context.get(),
                           {left_toe_eval, right_toe_eval},
@@ -86,21 +88,21 @@ int main(int argc, char* argv[]) {
                           reference_state.head(plant.num_positions()),
                           2000,
                           0,
-                          0.5);
+                          0.5, contact_mask);
   PlanarSlipReducer reducer(plant,
-                          context.get(),
-                          {left_toe_eval, right_toe_eval},
-                          {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
-                          {{0, {0, 1}}, {1, {2, 3}}},
-                          2000,
-                          0,
-                          0.5);
+                            context.get(),
+                            {left_toe_eval, right_toe_eval},
+                            {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
+                            {{0, {0, 1}}, {1, {2, 3}}},
+                            2000,
+                            0,
+                            0.5, contact_mask);
 
   Eigen::Vector3d slip_com = {0.2,0,0.7};
   Eigen::Vector3d slip_vel = {0.1,0,0.1};
   Eigen::VectorXd slip_feet(6); slip_feet  << 0.0, 0.2, 0.0,0.0,-0.2, 0.0;
   Eigen::VectorXd slip_foot_vel(6); slip_foot_vel  << 0.11,0.12,0.0,0.15,0.18,0.0;
-  Eigen::Vector2d slip_force = {0.5,0.5};
+  Eigen::Vector2d slip_force = {0.0,0.0};
 
   Eigen::VectorXd slip_state(3+3+6+6+2);
   slip_state << slip_com,slip_vel,slip_feet,slip_foot_vel,slip_force;
@@ -123,8 +125,8 @@ int main(int argc, char* argv[]) {
   drake::VectorX<double> input(slip_state.size() + complex_state.size());
   input << slip_state,complex_state;
   constraint.DoEval(input, &error);
-//  std::cout<<"Max Error in inverse test: "<< error.cwiseAbs().maxCoeff() << std::endl;
-  std::cout<<error<<std::endl;
+  std::cout<<"Max Error in inverse test: "<< error.cwiseAbs().maxCoeff() << std::endl;
+//  std::cout<<error<<std::endl;
 
   if(true){
     // Build temporary diagram for visualization
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
index a012946718..02c984246f 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
@@ -6,20 +6,23 @@ PlanarSlipReducer::PlanarSlipReducer(const drake::multibody::MultibodyPlant<doub
                                      drake::systems::Context<double> *context,
                                      const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
                                      const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
-                                     const std::map<int, std::vector<int>> &simple_foot_index_to_complex_foot_index,
+                                     const std::map<int,
+                                                    std::vector<int>> &simple_foot_index_to_complex_foot_index,
                                      double k,
                                      double b,
-                                     double r0):plant_(plant),
-                                                context_(context),
-                                                k_(k),
-                                                b_(b),
-                                                r0_(r0),
-                                                m_(plant.CalcTotalMass(*context)),
-                                                slip_contact_points_(slip_contact_points),
-                                                complex_contact_points_(complex_contact_points),
-                                                simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
-                                                n_q_(plant.num_positions()),
-                                                n_v_(plant.num_velocities()) {}
+                                     double r0,
+                                     const std::vector<bool> &contact_mask) : plant_(plant),
+                                                                              context_(context),
+                                                                              k_(k),
+                                                                              b_(b),
+                                                                              r0_(r0),
+                                                                              m_(plant.CalcTotalMass(*context)),
+                                                                              slip_contact_points_(slip_contact_points),
+                                                                              complex_contact_points_(complex_contact_points),
+                                                                              simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
+                                                                              n_q_(plant.num_positions()),
+                                                                              n_v_(plant.num_velocities()),
+                                                                              slip_contact_mask_(contact_mask){}
 /// Input is of the form:
 ///     complex_com
 ///     complex_ang_momentum
@@ -71,14 +74,16 @@ drake::VectorX<double> PlanarSlipReducer::ReduceGrf(const Eigen::Ref<const drake
   Eigen::VectorXd slip_force(slip_contact_points_.size());
   for(int i = 0; i<slip_contact_points_.size(); i++){
     const double r = (complex_com - slip_contact_pos.segment(3 * i, 3)).norm();
-    const double dr =  (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized().dot(com_vel);
+    const Eigen::VectorXd unit_vec = (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized();
+    const double dr =  unit_vec.dot(com_vel);
     const double spring_force = k_ * (r0_ - r) - b_ * dr;
     auto complex_feet_it = simple_foot_index_to_complex_foot_index_.find(i);
     Eigen::Vector3d complex_force = Eigen::Vector3d::Zero(3);
     for(const auto complex_index : complex_feet_it->second){
       complex_force = complex_force + complex_grf.segment(3 * complex_index, 3);
     }
-    slip_force.coeffRef(i) = complex_force.norm() - spring_force;
+    const double mag = complex_force.dot(unit_vec) > 0 ? complex_force.norm() : -complex_force.norm();
+    slip_force.coeffRef(i) = slip_contact_mask_[i] ? mag - spring_force : 0;
   }
   return slip_force;
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
index 90e18e84cb..014c7060d3 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
@@ -12,14 +12,16 @@
 
 class PlanarSlipReducer {
  public:
-  PlanarSlipReducer(const drake::multibody::MultibodyPlant<double>& plant,
-                   drake::systems::Context<double>* context,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& slip_contact_points,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& complex_contact_points,
-                   const std::map<int, std::vector<int>>& simple_foot_index_to_complex_foot_index,
-                   double k,
-                   double b,
-                   double r0);
+  PlanarSlipReducer(const drake::multibody::MultibodyPlant<double> &plant,
+                    drake::systems::Context<double> *context,
+                    const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
+                    const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
+                    const std::map<int,
+                                   std::vector<int>> &simple_foot_index_to_complex_foot_index,
+                    double k,
+                    double b,
+                    double r0,
+                    const std::vector<bool> &contact_mask);
 
   void Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state,
             drake::VectorX<double> *slip_state) const;
@@ -42,7 +44,7 @@ class PlanarSlipReducer {
   const std::map<int, std::vector<int>> simple_foot_index_to_complex_foot_index_;
   const int n_q_;
   const int n_v_;
-
+  const std::vector<bool> slip_contact_mask_;
   const int kSLIP_DIM = 3;
 };
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index 4048dbb77b..dd29beb342 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -190,7 +190,7 @@ class KinematicCentroidalMPC {
    * @brief Adds standard constraints to optimization problem and sets options
    * @param solver_options
    */
-  void Build(const drake::solvers::SolverOptions& solver_options);
+  virtual void Build(const drake::solvers::SolverOptions& solver_options);
 
   /*!
    * @brief Solve the optimization problem and return a piecewise linear

From 73c7b10295176b47a70de5349dae77f9d1fe4ef7 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 9 Dec 2022 18:19:46 -0500
Subject: [PATCH 59/76] fixed bunch of reduction function bugs

---
 .../cassie_kinematic_centroidal_mpc.cc        | 26 ++++++++++++++-----
 .../simple_models/planar_slip_constraints.cc  |  4 +--
 .../simple_models/planar_slip_lifting_test.cc | 10 ++++++-
 .../simple_models/planar_slip_reducer.cc      |  4 ++-
 4 files changed, 34 insertions(+), 10 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index d7008d6418..01a24bec28 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -177,7 +177,7 @@ void CassieKinematicCentroidalMPC::MapModeSequence() {
 }
 void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
   prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point));
-  prog_->AddConstraint(slip_vel_vars_[knot_point] == momentum_vars(knot_point).tail(3));
+  prog_->AddConstraint(slip_vel_vars_[knot_point] * m_ == momentum_vars(knot_point).tail(3));
   prog_->AddConstraint(slip_contact_pos_vars(knot_point,0) == contact_pos_vars(knot_point,0));
   prog_->AddConstraint(slip_contact_pos_vars(knot_point,1) == contact_pos_vars(knot_point,2));
   prog_->AddConstraint(slip_contact_vel_vars(knot_point,0) == contact_vel_vars(knot_point,0));
@@ -185,9 +185,8 @@ void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
   auto grf_constraint =
     std::make_shared<SlipGrfReductionConstrain>(
         plant_, reducers[knot_point], 2, 4,knot_point);
-  //TODO why is this constraint bad
-//  prog_->AddConstraint(grf_constraint,
-//                       {com_pos_vars(knot_point), slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
+  prog_->AddConstraint(grf_constraint,
+                       {com_pos_vars(knot_point), slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
 }
 
 void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
@@ -266,7 +265,22 @@ drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_p
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-      return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
+  if(knot_point == 1){
+    auto grf_constraint =
+        std::make_shared<SlipGrfReductionConstrain>(
+            plant_, reducers[knot_point], 2, 4,knot_point);
+    drake::VectorX<double> grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3);
+    drake::VectorX<double> grf_error(2);
+    grf_input << result_->GetSolution(com_pos_vars(knot_point)),
+        result_->GetSolution(slip_vel_vars_[knot_point]),
+        result_->GetSolution(slip_contact_pos_vars_[knot_point]),
+        result_->GetSolution(contact_force_[knot_point]),
+        result_->GetSolution(slip_force_vars_[knot_point]);
+    grf_constraint->Eval(grf_input, &grf_error);
+    std::cout<<"Grf error"<<std::endl;
+    std::cout<<grf_error<<std::endl;
+  }
+  return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
 }
 
 void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
@@ -293,7 +307,7 @@ void CassieKinematicCentroidalMPC::Build(const drake::solvers::SolverOptions &so
                                                 CreateContactPoints(plant_, 0),
                                                 {{0, {0, 1}}, {1, {2, 3}}},
                                                 k_,
-                                                r0_, 0, slip_contact_sequence_[knot]));
+                                                b_, r0_, slip_contact_sequence_[knot]));
   }
   KinematicCentroidalMPC::Build(solver_options);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
index 462e552417..c224090e6c 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
@@ -68,8 +68,8 @@ void SlipGrfReductionConstrain::EvaluateConstraint(const Eigen::Ref<const drake:
   const auto& complex_com = x.head(3);
   const auto& slip_vel = x.segment(3,3);
   const auto& slip_contact_pos = x.segment(3 + 3,3*n_slip_feet_);
-  const auto& complex_grf = x.segment(3 + 3*n_slip_feet_ + 3,3*n_complex_feet_);
-  const auto& slip_contact_force = x.segment(3 + 3*n_slip_feet_ + 3*n_complex_feet_ + 3,n_slip_feet_);
+  const auto& complex_grf = x.segment(3 + 3 + 3*n_slip_feet_,3*n_complex_feet_);
+  const auto& slip_contact_force = x.segment(3 + 3 + 3*n_slip_feet_ + 3*n_complex_feet_ ,n_slip_feet_);
   *y = reducing_function_->ReduceGrf(complex_com, slip_vel, slip_contact_pos, complex_grf) - slip_contact_force;
 }
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
index 581182e7ad..c543132e23 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
@@ -102,7 +102,7 @@ int main(int argc, char* argv[]) {
   Eigen::Vector3d slip_vel = {0.1,0,0.1};
   Eigen::VectorXd slip_feet(6); slip_feet  << 0.0, 0.2, 0.0,0.0,-0.2, 0.0;
   Eigen::VectorXd slip_foot_vel(6); slip_foot_vel  << 0.11,0.12,0.0,0.15,0.18,0.0;
-  Eigen::Vector2d slip_force = {0.0,0.0};
+  Eigen::Vector2d slip_force = {0.5,0.3};
 
   Eigen::VectorXd slip_state(3+3+6+6+2);
   slip_state << slip_com,slip_vel,slip_feet,slip_foot_vel,slip_force;
@@ -128,6 +128,14 @@ int main(int argc, char* argv[]) {
   std::cout<<"Max Error in inverse test: "<< error.cwiseAbs().maxCoeff() << std::endl;
 //  std::cout<<error<<std::endl;
 
+  auto grf_constraint = SlipGrfReductionConstrain(plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
+  drake::VectorX<double> grf_error(2);
+  drake::VectorX<double> grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3);
+//  grf_input << slip_com, slip_vel, slip_feet, complex_state.segment(3 + 6 + 12 + 12,12),slip_force;
+
+  grf_constraint.DoEval(grf_input, &grf_error);
+  std::cout<<"Max Error in grf inverse test: "<< grf_error.cwiseAbs().maxCoeff() << std::endl;
+
   if(true){
     // Build temporary diagram for visualization
     drake::systems::DiagramBuilder<double> builder_ik;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
index 02c984246f..a604982816 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
@@ -57,7 +57,7 @@ void PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::VectorX<double>> &c
                                                             complex_lin_momentum/m_,
                                                             slip_state->segment(kSLIP_DIM + kSLIP_DIM,
                                                                                 slip_contact_points_.size()
-                                                                                    + kSLIP_DIM),
+                                                                                    * kSLIP_DIM),
                                                             complex_grf);
 }
 
@@ -71,6 +71,8 @@ drake::VectorX<double> PlanarSlipReducer::ReduceGrf(const Eigen::Ref<const drake
                                                     const Eigen::Ref<const drake::VectorX<double>> &com_vel,
                                                     const Eigen::Ref<const drake::VectorX<double>> &slip_contact_pos,
                                                     const Eigen::Ref<const drake::VectorX<double>> &complex_grf) const {
+  DRAKE_DEMAND(complex_com.size() == 3);
+  DRAKE_DEMAND(slip_contact_pos.size() == 3 * slip_contact_points_.size());
   Eigen::VectorXd slip_force(slip_contact_points_.size());
   for(int i = 0; i<slip_contact_points_.size(); i++){
     const double r = (complex_com - slip_contact_pos.segment(3 * i, 3)).norm();

From 8448c54ef83aeb4bff1ddafa93833933ea910257 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 13 Dec 2022 09:35:48 -0500
Subject: [PATCH 60/76] Cleaning up code

---
 .../kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc      | 2 +-
 .../cassie_kinematic_centroidal_mpc.cc                    | 8 ++++----
 .../cassie_kinematic_centroidal_mpc.h                     | 2 ++
 .../simple_models/planar_slip_lifter.cc                   | 3 +++
 .../kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc  | 3 ---
 5 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
index 95f788de88..b079e48832 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
@@ -101,7 +101,7 @@ int DoMain(int argc, char* argv[]) {
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
-  for(int i = 1; i <traj_params.n_knot_points ; i++){
+  for(int i = 3; i <traj_params.n_knot_points ; i++){
     complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
index 01a24bec28..16af32f046 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
@@ -80,7 +80,7 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
 void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
   for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
 
-    prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 2);
+    prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 1.5);
 
     if(slip_contact_sequence_[knot_point][contact_index]){
       // Foot isn't moving
@@ -205,9 +205,9 @@ void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
         std::make_shared<PlanarSlipDynamicsConstraint<double>>(
             r0_, k_, b_,m_, 2, slip_contact_sequence_[knot_point], dt_, knot_point);
 
-    prog_->AddConstraint(slip_com_dynamics,
+    slip_dynamics_binding_.push_back(prog_->AddConstraint(slip_com_dynamics,
                          {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], slip_force_vars_[knot_point],
-                          slip_com_vars_[knot_point+1], slip_vel_vars_[knot_point+1], slip_contact_pos_vars_[knot_point+1],slip_force_vars_[knot_point+1]});
+                          slip_com_vars_[knot_point+1], slip_vel_vars_[knot_point+1], slip_contact_pos_vars_[knot_point+1],slip_force_vars_[knot_point+1]}));
     prog_->AddConstraint(
         slip_contact_pos_vars_[knot_point + 1] ==
             slip_contact_pos_vars_[knot_point] +
@@ -265,7 +265,7 @@ drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_p
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-  if(knot_point == 1){
+  if(knot_point == 3){
     auto grf_constraint =
         std::make_shared<SlipGrfReductionConstrain>(
             plant_, reducers[knot_point], 2, 4,knot_point);
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
index a24de5ed0a..194191ebf1 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
@@ -143,5 +143,7 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
                                                                             {{true, true, false, false},{true, false}},
                                                                             {{false, false, true, true},{false, true}},
                                                                             {{false, false, false, false},{false, false}}};
+  std::vector<drake::solvers::Binding<drake::solvers::Constraint>>
+      slip_dynamics_binding_;
 };
 
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
index 3ab40f5823..ba9bf860ad 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
@@ -180,6 +180,9 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
     double r = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).norm();
     double dr = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).normalized().dot(com_vel);
     double slip_grf_mag = slip_contact_mask_[simple_index] ? slip_force[simple_index] + k_ * (r0_ - r) - b_ * dr : 0;
+    if(slip_grf_mag  < 0){
+      slip_grf_mag = 0;
+    }
     // Find the average location for all of the complex contact points that make up the SLIP foot
     drake::Vector3<double> average_pos = drake::VectorX<double>::Zero(3);
 
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 8aa72517e3..51c66d6c20 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -245,10 +245,7 @@ void KinematicCentroidalMPC::Build(
         AddKinematicsIntegrator(knot_point);
         if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
           AddSlipReductionConstraint(knot_point + 1);
-          AddContactConstraints(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
-          AddFrictionConeConstraints(knot_point + 1);
-          AddFlightContactForceConstraints(knot_point + 1);
         }
         break;
       case PLANAR_SLIP:

From 334fb227c700cc7f7a972df39b3ebdfe11b4882c Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 13 Dec 2022 10:00:40 -0500
Subject: [PATCH 61/76] Working on reducing diff to adaptive_mpc before merge

---
 .../visualize_configs/long_jump.yaml          |  4 ++--
 examples/Cassie/BUILD.bazel                   |  2 +-
 .../BUILD.bazel                               | 18 ++++++++---------
 .../README.md                                 |  0
 .../cassie_kcmpc_trajopt.cc                   | 20 +++++++++----------
 .../cassie_kinematic_centroidal_mpc.cc        |  2 +-
 .../cassie_kinematic_centroidal_mpc.h         |  4 ++--
 .../cassie_reference_utils.cc                 |  0
 .../cassie_reference_utils.h                  |  0
 .../contact_scheduler.cc                      |  2 +-
 .../contact_scheduler.h                       |  0
 .../gaits/BUILD.bazel                         |  0
 .../gaits/run.yaml                            |  0
 .../gaits/stand.yaml                          |  0
 .../gaits/step.yaml                           |  0
 .../gaits/walk.yaml                           |  2 +-
 .../kinematic_centroidal_mpc_gains.yaml       |  0
 .../kinematic_trajectory_generator.cc         |  0
 .../kinematic_trajectory_generator.h          |  0
 .../osc_centroidal_gains.yaml                 |  0
 .../osc_tracking_qp_settings.yaml             |  0
 .../simple_models/BUILD.bazel                 |  4 ++--
 .../simple_models/planar_slip_constraints.cc  |  0
 .../simple_models/planar_slip_constraints.h   |  4 ++--
 .../simple_models/planar_slip_lifter.cc       |  0
 .../simple_models/planar_slip_lifter.h        |  0
 .../simple_models/planar_slip_lifting_test.cc |  8 ++++----
 .../simple_models/planar_slip_reducer.cc      |  0
 .../simple_models/planar_slip_reducer.h       |  0
 .../trajectory_parameters.h                   |  0
 .../trajectory_parameters.yaml                |  0
 examples/Cassie/osc/high_level_command.h      |  2 +-
 .../Cassie/systems/cassie_out_to_radio.cc     | 13 ++++++------
 examples/Cassie/systems/cassie_out_to_radio.h | 20 +++++++++----------
 multibody/multipose_visualizer.cc             |  8 ++++++++
 multibody/multipose_visualizer.h              | 11 ++++++++++
 systems/framework/lcm_driven_loop.h           |  3 ++-
 37 files changed, 73 insertions(+), 54 deletions(-)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/BUILD.bazel (75%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/README.md (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/cassie_kcmpc_trajopt.cc (92%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/cassie_kinematic_centroidal_mpc.cc (99%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/cassie_kinematic_centroidal_mpc.h (97%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/cassie_reference_utils.cc (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/cassie_reference_utils.h (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/contact_scheduler.cc (97%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/contact_scheduler.h (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/gaits/BUILD.bazel (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/gaits/run.yaml (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/gaits/stand.yaml (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/gaits/step.yaml (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/gaits/walk.yaml (96%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/kinematic_centroidal_mpc_gains.yaml (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/kinematic_trajectory_generator.cc (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/kinematic_trajectory_generator.h (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/osc_centroidal_gains.yaml (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/osc_tracking_qp_settings.yaml (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/BUILD.bazel (83%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/planar_slip_constraints.cc (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/planar_slip_constraints.h (96%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/planar_slip_lifter.cc (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/planar_slip_lifter.h (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/planar_slip_lifting_test.cc (95%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/planar_slip_reducer.cc (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/simple_models/planar_slip_reducer.h (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/trajectory_parameters.h (100%)
 rename examples/Cassie/{kinematic_centroidal_mpc => kinematic_centroidal_planner}/trajectory_parameters.yaml (100%)

diff --git a/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml b/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml
index 624c7196b0..5f67cb36a8 100644
--- a/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml
+++ b/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml
@@ -1,8 +1,8 @@
 filename: examples/Cassie/saved_trajectories/kcmpc_solution
 spring_urdf: examples/Cassie/urdf/cassie_v2_shells.urdf
 fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs.urdf
-visualize_mode: 1
+visualize_mode: 2
 realtime_rate: 1.0
 num_poses: 10
 use_transparency: 1
-use_springs: 1
\ No newline at end of file
+use_springs: 1
diff --git a/examples/Cassie/BUILD.bazel b/examples/Cassie/BUILD.bazel
index 6fc3c8383f..0a465250a0 100644
--- a/examples/Cassie/BUILD.bazel
+++ b/examples/Cassie/BUILD.bazel
@@ -355,7 +355,7 @@ cc_binary(
         ":cassie_urdf",
         ":cassie_utils",
         "//examples/Cassie/osc_jump",
-        "//examples/Cassie/kinematic_centroidal_mpc:centroidal_mpc",
+        "//examples/Cassie/kinematic_centroidal_planner:centroidal_mpc",
         "//lcm:trajectory_saver",
         "//systems/trajectory_optimization:lcm_trajectory_systems",
         "//multibody:utils",
diff --git a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
similarity index 75%
rename from examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
rename to examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
index 6c195bd040..c1b62ec1aa 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
@@ -24,12 +24,12 @@ cc_library(
 cc_binary(
     name = "cassie_kcmpc_trajopt",
     srcs = ["cassie_kcmpc_trajopt.cc"],
-    data = ["//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_mpc_gains.yaml",
-            "//examples/Cassie/kinematic_centroidal_mpc/gaits:stand.yaml",
-            "//examples/Cassie/kinematic_centroidal_mpc/gaits:walk.yaml",
-            "//examples/Cassie/kinematic_centroidal_mpc/gaits:step.yaml",
-            "//examples/Cassie/kinematic_centroidal_mpc/gaits:run.yaml",
-            "//examples/Cassie/kinematic_centroidal_mpc:trajectory_parameters.yaml"],
+    data = ["//examples/Cassie/kinematic_centroidal_planner:kinematic_centroidal_mpc_gains.yaml",
+            "//examples/Cassie/kinematic_centroidal_planner/gaits:stand.yaml",
+            "//examples/Cassie/kinematic_centroidal_planner/gaits:walk.yaml",
+            "//examples/Cassie/kinematic_centroidal_planner/gaits:step.yaml",
+            "//examples/Cassie/kinematic_centroidal_planner/gaits:run.yaml",
+            "//examples/Cassie/kinematic_centroidal_planner:trajectory_parameters.yaml"],
     deps = [
         ":centroidal_mpc",
         "//examples/Cassie:cassie_utils",
@@ -38,8 +38,8 @@ cc_binary(
         "//multibody/kinematic",
         "//solvers:optimization_utils",
         "//systems/primitives",
-        "//examples/Cassie/kinematic_centroidal_mpc:cassie_kinematic_centroidal_mpc",
-        "//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_reference_generator",
+        "//examples/Cassie/kinematic_centroidal_planner:cassie_kinematic_centroidal_mpc",
+        "//examples/Cassie/kinematic_centroidal_planner:kinematic_centroidal_reference_generator",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -95,7 +95,7 @@ cc_library(
         "//common",
         "//systems/controllers/kinematic_centroidal_mpc",
         "//multibody:visualization_utils",
-        "//examples/Cassie/kinematic_centroidal_mpc/simple_models:planar_slip",
+        "//examples/Cassie/kinematic_centroidal_planner/simple_models:planar_slip",
         "@drake//:drake_shared_library",
     ],
 )
diff --git a/examples/Cassie/kinematic_centroidal_mpc/README.md b/examples/Cassie/kinematic_centroidal_planner/README.md
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/README.md
rename to examples/Cassie/kinematic_centroidal_planner/README.md
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
similarity index 92%
rename from examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
rename to examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index b079e48832..0c1a8d4652 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -15,9 +15,9 @@
 #include <gflags/gflags.h>
 
 #include "common/find_resource.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
+#include "examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h"
 #include "systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h"
 #include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h"
 #include "systems/primitives/subvector_pass_through.h"
@@ -32,7 +32,7 @@ DEFINE_string(channel_reference, "KCMPC_REF",
               "MPC are published");
 DEFINE_string(
     trajectory_parameters,
-    "examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml",
+    "examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml",
     "YAML file that contains trajectory parameters such as speed, tolerance, "
     "com_height");
 DEFINE_double(knot_points_to_publish, 10, "Number of knot points to publish");
@@ -43,7 +43,7 @@ int DoMain(int argc, char* argv[]) {
   auto traj_params = drake::yaml::LoadYamlFile<TrajectoryParameters>(
       FLAGS_trajectory_parameters);
   auto gains = drake::yaml::LoadYamlFile<KinematicCentroidalGains>(
-      "examples/Cassie/kinematic_centroidal_mpc/"
+      "examples/Cassie/kinematic_centroidal_planner/"
       "kinematic_centroidal_mpc_gains.yaml");
   // Create fix-spring Cassie MBP
   drake::systems::DiagramBuilder<double> builder;
@@ -65,13 +65,13 @@ int DoMain(int argc, char* argv[]) {
 
   // Create gaits
   auto stand = drake::yaml::LoadYamlFile<Gait>(
-      "examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml");
+      "examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml");
   auto walk = drake::yaml::LoadYamlFile<Gait>(
-      "examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml");
+      "examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml");
   auto step = drake::yaml::LoadYamlFile<Gait>(
-      "examples/Cassie/kinematic_centroidal_mpc/gaits/step.yaml");
+      "examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml");
   auto run = drake::yaml::LoadYamlFile<Gait>(
-      "examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml");
+      "examples/Cassie/kinematic_centroidal_planner/gaits/run.yaml");
   walk.period = 1.6;
   // Create reference
   // TODO(yangwill): move this into the reference generator
@@ -215,7 +215,5 @@ int DoMain(int argc, char* argv[]) {
 }
 
 int main(int argc, char* argv[]) {
-  //  DoMain(45, 5, 0.8, 0.3, 0.5, 1e-2);
-  //  DoMain(45, 0.8, 0.25, 0.0, 1e-2);
   DoMain(argc, argv);
 }
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.cc
similarity index 99%
rename from examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
rename to examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.cc
index 16af32f046..c9721f6fe1 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.cc
@@ -2,7 +2,7 @@
 #include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/multibody/parsing/parser.h"
 #include "common/find_resource.h"
 #include "examples/Cassie/cassie_utils.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h"
 
 std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalMPC::CreateContactPoints(const drake::multibody::MultibodyPlant<
     double> &plant,
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h
similarity index 97%
rename from examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
rename to examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h
index 194191ebf1..beb674f5eb 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h
@@ -3,8 +3,8 @@
 #include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "examples/Cassie/cassie_utils.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
 
 /*!
  * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop closure, joint limits, and cassie contact points
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.cc
rename to examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc
diff --git a/examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h b/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h
rename to examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h
diff --git a/examples/Cassie/kinematic_centroidal_mpc/contact_scheduler.cc b/examples/Cassie/kinematic_centroidal_planner/contact_scheduler.cc
similarity index 97%
rename from examples/Cassie/kinematic_centroidal_mpc/contact_scheduler.cc
rename to examples/Cassie/kinematic_centroidal_planner/contact_scheduler.cc
index cba5bf50ea..4ceb660994 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/contact_scheduler.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/contact_scheduler.cc
@@ -1,5 +1,5 @@
 #include <iostream>
-#include "examples/Cassie/kinematic_centroidal_mpc/contact_scheduler.h"
+#include "examples/Cassie/kinematic_centroidal_planner/contact_scheduler.h"
 
 using drake::systems::Context;
 using drake::systems::DiscreteUpdateEvent;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/contact_scheduler.h b/examples/Cassie/kinematic_centroidal_planner/contact_scheduler.h
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/contact_scheduler.h
rename to examples/Cassie/kinematic_centroidal_planner/contact_scheduler.h
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/gaits/BUILD.bazel
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/gaits/BUILD.bazel
rename to examples/Cassie/kinematic_centroidal_planner/gaits/BUILD.bazel
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/run.yaml
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/gaits/run.yaml
rename to examples/Cassie/kinematic_centroidal_planner/gaits/run.yaml
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/gaits/stand.yaml
rename to examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/step.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/gaits/step.yaml
rename to examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml
diff --git a/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
similarity index 96%
rename from examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml
rename to examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
index 6036fc59dd..2589a35887 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/gaits/walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
@@ -11,4 +11,4 @@ gait_pattern:
   - start_phase: 0.9
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 1.0
\ No newline at end of file
+period: 1.25
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/kinematic_centroidal_mpc_gains.yaml
rename to examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_trajectory_generator.cc b/examples/Cassie/kinematic_centroidal_planner/kinematic_trajectory_generator.cc
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/kinematic_trajectory_generator.cc
rename to examples/Cassie/kinematic_centroidal_planner/kinematic_trajectory_generator.cc
diff --git a/examples/Cassie/kinematic_centroidal_mpc/kinematic_trajectory_generator.h b/examples/Cassie/kinematic_centroidal_planner/kinematic_trajectory_generator.h
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/kinematic_trajectory_generator.h
rename to examples/Cassie/kinematic_centroidal_planner/kinematic_trajectory_generator.h
diff --git a/examples/Cassie/kinematic_centroidal_mpc/osc_centroidal_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/osc_centroidal_gains.yaml
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/osc_centroidal_gains.yaml
rename to examples/Cassie/kinematic_centroidal_planner/osc_centroidal_gains.yaml
diff --git a/examples/Cassie/kinematic_centroidal_mpc/osc_tracking_qp_settings.yaml b/examples/Cassie/kinematic_centroidal_planner/osc_tracking_qp_settings.yaml
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/osc_tracking_qp_settings.yaml
rename to examples/Cassie/kinematic_centroidal_planner/osc_tracking_qp_settings.yaml
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
similarity index 83%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
index 6a62209535..5c54f61f88 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
@@ -23,12 +23,12 @@ cc_binary(
     srcs = ["planar_slip_lifting_test.cc"],
     deps = [
         "//examples/Cassie:cassie_utils",
-        "//examples/Cassie/kinematic_centroidal_mpc/simple_models:planar_slip",
+        "planar_slip",
         "//common",
         "//multibody:visualization_utils",
         "//multibody/kinematic",
         "//systems/primitives",
-        "//examples/Cassie/kinematic_centroidal_mpc:kinematic_centroidal_reference_generator",
+        "//examples/Cassie/kinematic_centroidal_planner:kinematic_centroidal_reference_generator",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
similarity index 96%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
index c96c863368..f5c931ebe7 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
@@ -8,8 +8,8 @@
 #include "solvers/nonlinear_cost.h"
 #include "multibody/kinematic/kinematic_evaluator_set.h"
 #include "multibody/kinematic/world_point_evaluator.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
 
 class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstraint<double> {
  public:
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
similarity index 95%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
index c543132e23..d0b3e42d82 100644
--- a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
@@ -11,12 +11,12 @@
 #include <drake/common/yaml/yaml_io.h>
 #include "common/find_resource.h"
 #include "systems/primitives/subvector_pass_through.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/cassie_reference_utils.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "multibody/visualization_utils.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_constraints.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc
diff --git a/examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/simple_models/planar_slip_reducer.h
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h
diff --git a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.h b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.h
rename to examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
diff --git a/examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml
similarity index 100%
rename from examples/Cassie/kinematic_centroidal_mpc/trajectory_parameters.yaml
rename to examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml
diff --git a/examples/Cassie/osc/high_level_command.h b/examples/Cassie/osc/high_level_command.h
index 7103e08ad2..a42696e836 100644
--- a/examples/Cassie/osc/high_level_command.h
+++ b/examples/Cassie/osc/high_level_command.h
@@ -75,7 +75,7 @@ class HighLevelCommand : public drake::systems::LeafSystem<double> {
   const drake::systems::OutputPort<double>& get_yaw_output_port() const {
     return this->get_output_port(yaw_port_);
   }
-  const drake::systems::InputPort<double>& get_radio_port() const {
+  const drake::systems::InputPort<double>& get_input_port_radio() const {
     return this->get_input_port(radio_port_);
   }
   const drake::systems::OutputPort<double>& get_xy_output_port() const {
diff --git a/examples/Cassie/systems/cassie_out_to_radio.cc b/examples/Cassie/systems/cassie_out_to_radio.cc
index 694df24651..58d8eb1d9d 100644
--- a/examples/Cassie/systems/cassie_out_to_radio.cc
+++ b/examples/Cassie/systems/cassie_out_to_radio.cc
@@ -17,12 +17,13 @@ CassieOutToRadio::CassieOutToRadio() {
 }
 
 void CassieOutToRadio::CalcRadioOut(
-    const drake::systems::Context<double> &context,
-    dairlib::lcmt_radio_out *output) const {
-  const dairlib::lcmt_cassie_out *cassie_output = this->
-      EvalInputValue<dairlib::lcmt_cassie_out>(context, cassie_out_input_port_);
+    const drake::systems::Context<double>& context,
+    dairlib::lcmt_radio_out* output) const {
+  const dairlib::lcmt_cassie_out* cassie_output =
+      this->EvalInputValue<dairlib::lcmt_cassie_out>(context,
+                                                     cassie_out_input_port_);
   *output = cassie_output->pelvis.radio;
 }
 
-}
-}
\ No newline at end of file
+}  // namespace systems
+}  // namespace dairlib
diff --git a/examples/Cassie/systems/cassie_out_to_radio.h b/examples/Cassie/systems/cassie_out_to_radio.h
index ad1bde46cf..77277b392a 100644
--- a/examples/Cassie/systems/cassie_out_to_radio.h
+++ b/examples/Cassie/systems/cassie_out_to_radio.h
@@ -1,29 +1,29 @@
 #pragma once
+#include <memory>
+
 #include "dairlib/lcmt_cassie_out.hpp"
 #include "dairlib/lcmt_radio_out.hpp"
-#include <memory>
 
 #include "drake/systems/framework/leaf_system.h"
 
 namespace dairlib {
 namespace systems {
 
- /// Convenience system to separate out the radio lcm struct from the larger
- /// lcmt_cassie_out struct. THe output port of this system will pass through
- /// the pelvis.radio field of the lcmt_cassie_out message at the system's
- /// input port
+/// Convenience system to separate out the radio lcm struct from the larger
+/// lcmt_cassie_out struct. THe output port of this system will pass through
+/// the pelvis.radio field of the lcmt_cassie_out message at the system's
+/// input port
 class CassieOutToRadio : public drake::systems::LeafSystem<double> {
  public:
   CassieOutToRadio();
 
  protected:
-  void CalcRadioOut(
-      const drake::systems::Context<double> &context,
-      dairlib::lcmt_radio_out *output) const;
+  void CalcRadioOut(const drake::systems::Context<double>& context,
+                    dairlib::lcmt_radio_out* output) const;
 
  private:
   int cassie_out_input_port_;
   int radio_output_port_;
 };
-}
-}
\ No newline at end of file
+}  // namespace systems
+}  // namespace dairlib
diff --git a/multibody/multipose_visualizer.cc b/multibody/multipose_visualizer.cc
index 2406c3f337..7116b58f2d 100644
--- a/multibody/multipose_visualizer.cc
+++ b/multibody/multipose_visualizer.cc
@@ -2,6 +2,7 @@
 
 #include "drake/geometry/drake_visualizer.h"
 #include "drake/geometry/scene_graph.h"
+#include "drake/geometry/meshcat_visualizer_params.h"
 #include "drake/systems/framework/diagram_builder.h"
 #include "drake/systems/lcm/lcm_interface_system.h"
 
@@ -88,6 +89,12 @@ MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses,
     }
   }
 
+  drake::geometry::MeshcatVisualizerParams params;
+  params.publish_period = 1.0/60.0;
+  meshcat_ = std::make_shared<drake::geometry::Meshcat>();
+  meshcat_visualizer_ = &drake::geometry::MeshcatVisualizer<double>::AddToBuilder(
+      &builder, *scene_graph, meshcat_, std::move(params));
+
   DrakeVisualizer<double>::AddToBuilder(&builder, *scene_graph, lcm);
   diagram_ = builder.Build();
   diagram_context_ = diagram_->CreateDefaultContext();
@@ -106,6 +113,7 @@ void MultiposeVisualizer::DrawPoses(MatrixXd poses) {
 
   // Publish diagram
   diagram_->Publish(*diagram_context_);
+
 }
 
 }  // namespace multibody
diff --git a/multibody/multipose_visualizer.h b/multibody/multipose_visualizer.h
index 3d4f28b0d7..2723a1d535 100644
--- a/multibody/multipose_visualizer.h
+++ b/multibody/multipose_visualizer.h
@@ -4,6 +4,7 @@
 #include <vector>
 
 #include "drake/geometry/scene_graph.h"
+#include "drake/geometry/meshcat_visualizer.h"
 #include "drake/multibody/parsing/parser.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/systems/framework/diagram.h"
@@ -53,10 +54,20 @@ class MultiposeVisualizer {
   /// ignored.
   void DrawPoses(Eigen::MatrixXd poses);
 
+  int num_poses() const{
+    return num_poses_;
+  }
+
+  int num_positions(){
+    return plant_->num_positions();
+  }
+
  private:
   int num_poses_;
   drake::multibody::MultibodyPlant<double>* plant_;
   std::unique_ptr<drake::systems::Diagram<double>> diagram_;
+  std::shared_ptr<drake::geometry::Meshcat> meshcat_;
+  drake::geometry::MeshcatVisualizer<double>* meshcat_visualizer_;
   std::unique_ptr<drake::systems::Context<double>> diagram_context_;
   std::vector<drake::multibody::ModelInstanceIndex> model_indices_;
 };
diff --git a/systems/framework/lcm_driven_loop.h b/systems/framework/lcm_driven_loop.h
index 8026d1f5d3..8d29b53030 100644
--- a/systems/framework/lcm_driven_loop.h
+++ b/systems/framework/lcm_driven_loop.h
@@ -3,6 +3,7 @@
 #include <map>
 #include <string>
 #include <vector>
+#include <iostream>
 
 #include "dairlib/lcmt_controller_switch.hpp"
 #include "dairlib/lcmt_robot_output.hpp"
@@ -326,4 +327,4 @@ class LcmDrivenLoop {
 };
 
 }  // namespace systems
-}  // namespace dairlib
\ No newline at end of file
+}  // namespace dairlib

From 820ee8dd05f1d27edeba5f9fd5180f4e8f96d11c Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 13 Dec 2022 14:44:53 -0500
Subject: [PATCH 62/76] Cleaning up code for merge, code runs, but ipopt
 crashes

---
 examples/Cassie/BUILD.bazel                   |  53 +-
 .../centroidal_mpc_tracking_controller.cc     |  34 +-
 .../kinematic_centroidal_planner/BUILD.bazel  |  34 +-
 .../cassie_kcmpc_trajopt.cc                   | 131 +--
 ... => cassie_kinematic_centroidal_solver.cc} |  97 +-
 ...h => cassie_kinematic_centroidal_solver.h} |  33 +-
 .../contact_scheduler.cc                      |  39 +-
 .../gaits/BUILD.bazel                         |  15 +-
 .../gaits/jump.yaml                           |  11 +
 .../gaits/left_step.yaml                      |   8 +
 .../gaits/right_step.yaml                     |   8 +
 .../gaits/stand.yaml                          |   2 +-
 .../gaits/step.yaml                           |   8 -
 .../gaits/walk.yaml                           |   2 +-
 .../kinematic_centroidal_mpc_gains.yaml       |   4 +-
 .../motions/BUILD.bazel                       |  12 +
 .../motions/motion_jump.yaml                  |  11 +
 .../motions/motion_left_step.yaml             |  11 +
 .../motions/motion_right_step.yaml            |  11 +
 .../motions/motion_test.yaml                  |  11 +
 .../motions/motion_walk.yaml                  |  11 +
 .../osc_centroidal_gains.yaml                 |   2 +-
 .../simple_models/BUILD.bazel                 |   2 +-
 .../trajectory_parameters.h                   |  25 +-
 .../trajectory_parameters.yaml                |   7 -
 examples/Cassie/run_osc_walking_controller.cc |   2 +-
 .../Cassie/run_osc_walking_controller_alip.cc |   2 +-
 lcmtypes/lcmt_timestamped_saved_traj.lcm      |  10 +
 solvers/BUILD.bazel                           |   1 +
 .../kinematic_centroidal_mpc/BUILD.bazel      |  42 +-
 .../kinematic_centroidal_mpc/README.md        |  26 +
 .../kinematic_centroidal_mpc.cc               | 909 +++++-------------
 .../kinematic_centroidal_mpc.h                | 509 ++--------
 .../kinematic_centroidal_visualizer.cc        |  64 ++
 .../kinematic_centroidal_visualizer.h         |  40 +
 systems/trajectory_optimization/BUILD.bazel   |   1 +
 .../kinematic_centroidal_planner/BUILD.bazel  |  52 +
 .../kinematic_centroidal_planner}/gait.cc     |   3 +-
 .../kinematic_centroidal_planner}/gait.h      |  35 +-
 .../kcmpc_reference_generator.cc              |  43 +-
 .../kcmpc_reference_generator.h               |  48 +-
 .../kinematic_centroidal_constraints.cc       |   2 +-
 .../kinematic_centroidal_constraints.h        |   0
 .../kinematic_centroidal_gains.h              |  11 +-
 .../kinematic_centroidal_solver.cc            | 729 ++++++++++++++
 .../kinematic_centroidal_solver.h             | 494 ++++++++++
 .../reference_generation_utils.cc             |  42 +-
 .../reference_generation_utils.h              |  13 +-
 .../lcm_trajectory_systems.cc                 |  48 +-
 .../lcm_trajectory_systems.h                  |   3 +
 50 files changed, 2224 insertions(+), 1487 deletions(-)
 rename examples/Cassie/kinematic_centroidal_planner/{cassie_kinematic_centroidal_mpc.cc => cassie_kinematic_centroidal_solver.cc} (74%)
 rename examples/Cassie/kinematic_centroidal_planner/{cassie_kinematic_centroidal_mpc.h => cassie_kinematic_centroidal_solver.h} (83%)
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/gaits/jump.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/gaits/left_step.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/gaits/right_step.yaml
 delete mode 100644 examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
 delete mode 100644 examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml
 create mode 100644 lcmtypes/lcmt_timestamped_saved_traj.lcm
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/README.md
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.cc
 create mode 100644 systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.h
 create mode 100644 systems/trajectory_optimization/kinematic_centroidal_planner/BUILD.bazel
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/gait.cc (92%)
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/gait.h (64%)
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/kcmpc_reference_generator.cc (58%)
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/kcmpc_reference_generator.h (66%)
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/kinematic_centroidal_constraints.cc (99%)
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/kinematic_centroidal_constraints.h (100%)
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/kinematic_centroidal_gains.h (86%)
 create mode 100644 systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
 create mode 100644 systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/reference_generation_utils.cc (74%)
 rename systems/{controllers/kinematic_centroidal_mpc => trajectory_optimization/kinematic_centroidal_planner}/reference_generation_utils.h (93%)

diff --git a/examples/Cassie/BUILD.bazel b/examples/Cassie/BUILD.bazel
index 0a465250a0..551f555869 100644
--- a/examples/Cassie/BUILD.bazel
+++ b/examples/Cassie/BUILD.bazel
@@ -351,11 +351,12 @@ cc_binary(
 cc_binary(
     name = "centroidal_mpc_tracking_controller",
     srcs = ["centroidal_mpc_tracking_controller.cc"],
+    data = ["//examples/Cassie/kinematic_centroidal_planner:gains"],
     deps = [
         ":cassie_urdf",
         ":cassie_utils",
         "//examples/Cassie/osc_jump",
-        "//examples/Cassie/kinematic_centroidal_planner:centroidal_mpc",
+        "//examples/Cassie/kinematic_centroidal_planner:centroidal_tracking_controller",
         "//lcm:trajectory_saver",
         "//systems/trajectory_optimization:lcm_trajectory_systems",
         "//multibody:utils",
@@ -369,6 +370,56 @@ cc_binary(
     ],
 )
 
+cc_binary(
+    name = "centroidal_mpc_reference",
+    srcs = ["centroidal_mpc_reference.cc"],
+    data = ["//examples/Cassie/kinematic_centroidal_planner:kinematic_centroidal_mpc_gains.yaml",
+            "//examples/Cassie/kinematic_centroidal_planner/gaits:gaits",
+            "//examples/Cassie/kinematic_centroidal_planner/motions:motions"],
+    deps = [
+        ":cassie_urdf",
+        ":cassie_utils",
+        "//examples/Cassie/osc:osc",
+        "//examples/Cassie/systems:cassie_out_to_radio",
+        "//examples/Cassie/kinematic_centroidal_planner:centroidal_tracking_controller",
+        "//lcm:trajectory_saver",
+        "//systems/trajectory_optimization:lcm_trajectory_systems",
+        "//systems/trajectory_optimization/kinematic_centroidal_planner:kinematic_centroidal_planner",
+        "//systems/controllers/kinematic_centroidal_mpc:kinematic_centroidal_mpc",
+        "//multibody:utils",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/primitives:trajectory_passthrough",
+        "//systems/framework:lcm_driven_loop",
+        "//systems/primitives",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+cc_binary(
+    name = "run_mpc_visualization",
+    srcs = ["run_mpc_visualization.cc"],
+    deps = [
+        ":cassie_urdf",
+        ":cassie_utils",
+        "//examples/Cassie/osc:osc",
+        "//examples/Cassie/systems:cassie_out_to_radio",
+        "//examples/Cassie/kinematic_centroidal_planner:centroidal_tracking_controller",
+        "//lcm:trajectory_saver",
+        "//systems/controllers/kinematic_centroidal_mpc:kinematic_centroidal_visualizer",
+        "//multibody:utils",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/primitives:trajectory_passthrough",
+        "//systems/trajectory_optimization:lcm_trajectory_systems",
+        "//systems/framework:lcm_driven_loop",
+        "//systems/primitives",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
 cc_binary(
     name = "run_osc_walking_controller",
     srcs = ["run_osc_walking_controller.cc"],
diff --git a/examples/Cassie/centroidal_mpc_tracking_controller.cc b/examples/Cassie/centroidal_mpc_tracking_controller.cc
index 5d52374be2..ff34e5633e 100644
--- a/examples/Cassie/centroidal_mpc_tracking_controller.cc
+++ b/examples/Cassie/centroidal_mpc_tracking_controller.cc
@@ -3,14 +3,14 @@
 #include <drake/systems/framework/diagram_builder.h>
 #include <drake/systems/lcm/lcm_publisher_system.h>
 #include <gflags/gflags.h>
+#include <dairlib/lcmt_timestamped_saved_traj.hpp>
 
 #include "dairlib/lcmt_robot_input.hpp"
 #include "dairlib/lcmt_robot_output.hpp"
 #include "examples/Cassie/cassie_utils.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/contact_scheduler.h"
-#include "examples/Cassie/kinematic_centroidal_mpc/kinematic_trajectory_generator.h"
+#include "examples/Cassie/kinematic_centroidal_planner/contact_scheduler.h"
+#include "examples/Cassie/kinematic_centroidal_planner/kinematic_trajectory_generator.h"
 #include "examples/Cassie/osc_jump/osc_jumping_gains.h"
-#include "lcm/dircon_saved_trajectory.h"
 #include "lcm/lcm_trajectory.h"
 #include "multibody/kinematic/fixed_joint_evaluator.h"
 #include "systems/controllers/controller_failure_aggregator.h"
@@ -63,7 +63,7 @@ DEFINE_string(
     "CASSIE_STATE_DISPATCHER for use on hardware with the state estimator");
 DEFINE_string(channel_u, "CASSIE_INPUT",
               "The name of the channel where control efforts are published");
-DEFINE_string(channel_reference, "KCMPC_REF",
+DEFINE_string(channel_reference, "KCMPC_OUTPUT",
               "The name of the channel where the reference trajectories from "
               "MPC are published");
 DEFINE_string(folder_path, "examples/Cassie/saved_trajectories/",
@@ -72,11 +72,11 @@ DEFINE_string(traj_name, "kcmpc_solution",
               "File to load saved trajectories from");
 DEFINE_string(
     gains_filename,
-    "examples/Cassie/kinematic_centroidal_mpc/osc_centroidal_gains.yaml",
+    "examples/Cassie/kinematic_centroidal_planner/osc_centroidal_gains.yaml",
     "Filepath containing gains");
 DEFINE_string(
     osqp_settings,
-    "examples/Cassie/kinematic_centroidal_mpc/osc_tracking_qp_settings.yaml",
+    "examples/Cassie/kinematic_centroidal_planner/osc_tracking_qp_settings.yaml",
     "Filepath containing qp settings");
 
 int DoMain(int argc, char* argv[]) {
@@ -135,14 +135,12 @@ int DoMain(int argc, char* argv[]) {
   const int RIGHT_STANCE = 2;
   const int DOUBLE_STANCE = 3;
   contact_state_to_fsm_map[0] = 0;
-  //  contact_state_to_fsm_map[12] = 1; // left stance
-  //  contact_state_to_fsm_map[3] = 2; // right stance
   contact_state_to_fsm_map[12] = 2;  // left stance
   contact_state_to_fsm_map[3] = 1;   // right stance
   contact_state_to_fsm_map[15] = 3;  // double stance
 
   auto trajectory_subscriber =
-      builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_saved_traj>(
+      builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_timestamped_saved_traj>(
           FLAGS_channel_reference, &lcm));
   auto state_reference_receiver =
       builder.AddSystem<LcmTrajectoryReceiver>("state_traj");
@@ -486,24 +484,8 @@ int DoMain(int argc, char* argv[]) {
   // Run lcm-driven simulation
   systems::LcmDrivenLoop<dairlib::lcmt_robot_output> loop(
       &lcm, std::move(owned_diagram), state_receiver, FLAGS_channel_x, true);
-
-  /// Fixing the reference trajectories for now.
-  /// TODO(yangwill): set up the subscriber and publisher for the reference
-  /// trajectories
-  const LcmTrajectory& lcm_traj =
-      LcmTrajectory(FindResourceOrThrow(output_traj_path));
-  auto& state_reference_receiver_context =
-      loop.get_diagram()->GetMutableSubsystemContext(
-          *state_reference_receiver, &loop.get_diagram_mutable_context());
-  auto& contact_force_reference_receiver_context =
-      loop.get_diagram()->GetMutableSubsystemContext(
-          *contact_force_reference_receiver,
-          &loop.get_diagram_mutable_context());
-  state_reference_receiver->get_input_port().FixValue(
-      &state_reference_receiver_context, lcm_traj.GenerateLcmObject());
-  contact_force_reference_receiver->get_input_port().FixValue(
-      &contact_force_reference_receiver_context, lcm_traj.GenerateLcmObject());
   DrawAndSaveDiagramGraph(*loop.get_diagram());
+
   loop.Simulate();
 
   return 0;
diff --git a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
index c1b62ec1aa..4c15a2b400 100644
--- a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
@@ -1,10 +1,15 @@
 package(default_visibility = ["//visibility:public"])
 
-exports_files(["kinematic_centroidal_mpc_gains.yaml"])
-exports_files(["trajectory_parameters.yaml"])
+filegroup(
+    name = "gains",
+    data = ["kinematic_centroidal_mpc_gains.yaml",
+           "osc_centroidal_gains.yaml",
+           "osc_tracking_qp_settings.yaml"],
+    visibility = ["//visibility:public"],
+)
 
 cc_library(
-    name = "centroidal_mpc",
+    name = "centroidal_tracking_controller",
     srcs = [],
     deps = [
         ":contact_scheduler",
@@ -24,14 +29,11 @@ cc_library(
 cc_binary(
     name = "cassie_kcmpc_trajopt",
     srcs = ["cassie_kcmpc_trajopt.cc"],
-    data = ["//examples/Cassie/kinematic_centroidal_planner:kinematic_centroidal_mpc_gains.yaml",
-            "//examples/Cassie/kinematic_centroidal_planner/gaits:stand.yaml",
-            "//examples/Cassie/kinematic_centroidal_planner/gaits:walk.yaml",
-            "//examples/Cassie/kinematic_centroidal_planner/gaits:step.yaml",
-            "//examples/Cassie/kinematic_centroidal_planner/gaits:run.yaml",
-            "//examples/Cassie/kinematic_centroidal_planner:trajectory_parameters.yaml"],
+    data = [":kinematic_centroidal_mpc_gains.yaml",
+            "//examples/Cassie/kinematic_centroidal_planner/gaits:gaits",
+            "//examples/Cassie/kinematic_centroidal_planner/motions:motions"],
     deps = [
-        ":centroidal_mpc",
+        ":centroidal_tracking_controller",
         "//examples/Cassie:cassie_utils",
         "//common",
         "//multibody:visualization_utils",
@@ -39,7 +41,7 @@ cc_binary(
         "//solvers:optimization_utils",
         "//systems/primitives",
         "//examples/Cassie/kinematic_centroidal_planner:cassie_kinematic_centroidal_mpc",
-        "//examples/Cassie/kinematic_centroidal_planner:kinematic_centroidal_reference_generator",
+        "//examples/Cassie/kinematic_centroidal_planner:cassie_reference_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -74,13 +76,13 @@ cc_library(
 )
 
 cc_library(
-    name = "kinematic_centroidal_reference_generator",
+    name = "cassie_reference_utils",
     srcs = ["cassie_reference_utils.cc"],
     hdrs = ["cassie_reference_utils.h"],
     deps = [
         "//examples/Cassie:cassie_utils",
         "//common",
-        "//systems/controllers/kinematic_centroidal_mpc:kinematic_centroidal_reference_generator",
+        "//systems/trajectory_optimization/kinematic_centroidal_planner:kinematic_centroidal_planner",
         "//multibody:visualization_utils",
         "@drake//:drake_shared_library",
     ],
@@ -88,12 +90,12 @@ cc_library(
 
 cc_library(
     name = "cassie_kinematic_centroidal_mpc",
-    srcs = ["cassie_kinematic_centroidal_mpc.cc"],
-    hdrs = ["cassie_kinematic_centroidal_mpc.h"],
+    srcs = ["cassie_kinematic_centroidal_solver.cc"],
+    hdrs = ["cassie_kinematic_centroidal_solver.h"],
     deps = [
         "//examples/Cassie:cassie_utils",
         "//common",
-        "//systems/controllers/kinematic_centroidal_mpc",
+        "//systems/trajectory_optimization/kinematic_centroidal_planner",
         "//multibody:visualization_utils",
         "//examples/Cassie/kinematic_centroidal_planner/simple_models:planar_slip",
         "@drake//:drake_shared_library",
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index 0c1a8d4652..dea369c1d4 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -1,5 +1,5 @@
 #include <iostream>
-#include<unistd.h>               // for linux 
+#include<unistd.h>
 #include <drake/common/yaml/yaml_io.h>
 #include <drake/geometry/drake_visualizer.h>
 #include <drake/geometry/meshcat_visualizer.h>
@@ -15,12 +15,14 @@
 #include <gflags/gflags.h>
 
 #include "common/find_resource.h"
-#include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h"
+#include "dairlib/lcmt_timestamped_saved_traj.hpp"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h"
 #include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h"
-#include "systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h"
-#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h"
 #include "systems/primitives/subvector_pass_through.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h"
 
 using drake::geometry::DrakeVisualizer;
 using drake::geometry::SceneGraph;
@@ -32,9 +34,14 @@ DEFINE_string(channel_reference, "KCMPC_REF",
               "MPC are published");
 DEFINE_string(
     trajectory_parameters,
-    "examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml",
+    "examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml",
     "YAML file that contains trajectory parameters such as speed, tolerance, "
-    "com_height");
+    "target_com_height");
+DEFINE_string(planner_parameters,
+              "examples/Cassie/kinematic_centroidal_planner/"
+              "kinematic_centroidal_mpc_gains.yaml",
+              "planner parameters containing initial states and other "
+              "regularization parameters");
 DEFINE_double(knot_points_to_publish, 10, "Number of knot points to publish");
 
 int DoMain(int argc, char* argv[]) {
@@ -43,8 +50,7 @@ int DoMain(int argc, char* argv[]) {
   auto traj_params = drake::yaml::LoadYamlFile<TrajectoryParameters>(
       FLAGS_trajectory_parameters);
   auto gains = drake::yaml::LoadYamlFile<KinematicCentroidalGains>(
-      "examples/Cassie/kinematic_centroidal_planner/"
-      "kinematic_centroidal_mpc_gains.yaml");
+      FLAGS_planner_parameters);
   // Create fix-spring Cassie MBP
   drake::systems::DiagramBuilder<double> builder;
   SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
@@ -57,47 +63,51 @@ int DoMain(int argc, char* argv[]) {
   Parser parser_vis(&plant_vis, &scene_graph);
 
   std::string full_name = dairlib::FindResourceOrThrow(
-      "examples/Cassie/urdf/cassie_fixed_springs.urdf");
+      "examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf");
   parser.AddModelFromFile(full_name);
   parser_vis.AddModelFromFile(full_name);
   plant.Finalize();
   plant_vis.Finalize();
 
+  auto plant_context = plant.CreateDefaultContext();
+
   // Create gaits
   auto stand = drake::yaml::LoadYamlFile<Gait>(
       "examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml");
   auto walk = drake::yaml::LoadYamlFile<Gait>(
       "examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml");
-  auto step = drake::yaml::LoadYamlFile<Gait>(
-      "examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml");
-  auto run = drake::yaml::LoadYamlFile<Gait>(
-      "examples/Cassie/kinematic_centroidal_planner/gaits/run.yaml");
-  walk.period = 1.6;
+  auto right_step = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/right_step.yaml");
+  auto left_step = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/left_step.yaml");
+  auto jump = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/jump.yaml");
+
+  std::unordered_map<std::string, Gait> gait_library;
+  gait_library["stand"] = stand;
+  gait_library["walk"] = walk;
+  gait_library["right_step"] = right_step;
+  gait_library["left_step"] = left_step;
+  gait_library["jump"] = jump;
   // Create reference
-  // TODO(yangwill): move this into the reference generator
-  // Specify knot points
-  std::vector<Gait> gait_samples = {stand, walk, walk};
-  DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size());
-  std::vector<double> durations = std::vector<double>(gait_samples.size());
-  for (int i = 0; i < gait_samples.size(); ++i) {
-    durations[i] = traj_params.duration_scaling[i] * gait_samples[i].period;
-  }
-  std::vector<double> time_points;
-  double start_time = 0;
-  time_points.push_back(start_time);
-  for (auto duration : durations) {
-    time_points.push_back(time_points.back() + duration);
+  std::vector<Gait> gait_samples;
+  for (auto gait : traj_params.gait_sequence){
+    gait_samples.push_back(gait_library.at(gait));
   }
+  DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size());
+  std::vector<double> time_points = KcmpcReferenceGenerator::GenerateTimePoints(
+      traj_params.duration_scaling, gait_samples);
 
   Eigen::VectorXd reference_state = GenerateNominalStand(
-      plant, traj_params.com_height, traj_params.stance_width, false);
+      plant, traj_params.target_com_height, traj_params.stance_width, false);
 
   // Create MPC and set gains
-  CassieKinematicCentroidalMPC mpc(
+  CassieKinematicCentroidalSolver mpc(
       plant, traj_params.n_knot_points,
       time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 5000, 10,
-      sqrt(pow(traj_params.com_height,2) + pow(traj_params.stance_width,2)), traj_params.stance_width);
+      sqrt(pow(traj_params.target_com_height,2) + pow(traj_params.stance_width,2)), traj_params.stance_width);
   mpc.SetGains(gains);
+  mpc.SetMinimumFootClearance(traj_params.swing_foot_minimum_height);
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
@@ -106,38 +116,38 @@ int DoMain(int argc, char* argv[]) {
   }
   mpc.SetComplexitySchedule(complexity_schedule);
 
-  KcmpcReferenceGenerator generator(plant,
-                                    reference_state.head(plant.num_positions()),
-                                    mpc.CreateContactPoints(plant, 0));
 
-  std::vector<Eigen::Vector3d> com_vel = {
-      {{0, 0, 0}, {traj_params.vel, 0, 0}, {0, 0, 0}}};
-  generator.SetComKnotPoints({time_points, com_vel});
+  KcmpcReferenceGenerator generator(plant, plant_context.get(),
+                                    CreateContactPoints(plant, 0));
+
+  generator.SetNominalReferenceConfiguration(
+      reference_state.head(plant.num_positions()));
+  generator.SetComKnotPoints({time_points, traj_params.com_vel_vector});
   generator.SetGaitSequence({time_points, gait_samples});
-  generator.Build();
+  generator.Generate();
 
   // Add reference and mode sequence
-  mpc.AddForceTrackingReference(
+  mpc.SetForceTrackingReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.grf_traj_));
-  mpc.AddGenPosReference(
+  mpc.SetGenPosReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.q_trajectory_));
-  mpc.AddGenVelReference(
+  mpc.SetGenVelReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.v_trajectory_));
-  mpc.AddComReference(
+  mpc.SetComReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.com_trajectory_));
-  mpc.AddContactTrackingReference(
+  mpc.SetContactTrackingReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.contact_traj_));
-  mpc.AddMomentumReference(
+  mpc.SetMomentumReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.momentum_trajectory_));
   mpc.SetModeSequence(generator.contact_sequence_);
-
-  // Set initial guess
+  //
+  //  // Set initial guess
   mpc.AddInitialStateConstraint(reference_state);
   mpc.SetRobotStateGuess(generator.q_trajectory_, generator.v_trajectory_);
   mpc.SetComPositionGuess(generator.com_trajectory_);
@@ -148,11 +158,11 @@ int DoMain(int argc, char* argv[]) {
   {
     drake::solvers::SolverOptions options;
     auto id = drake::solvers::IpoptSolver::id();
-    options.SetOption(id, "tol", traj_params.tol);
-    options.SetOption(id, "dual_inf_tol", traj_params.tol);
-    options.SetOption(id, "constr_viol_tol", traj_params.tol);
-    options.SetOption(id, "compl_inf_tol", traj_params.tol);
-    options.SetOption(id, "max_iter", 1000);
+    options.SetOption(id, "tol", gains.tol);
+    options.SetOption(id, "dual_inf_tol", gains.tol);
+    options.SetOption(id, "constr_viol_tol", gains.tol);
+    options.SetOption(id, "compl_inf_tol", gains.tol);
+    options.SetOption(id, "max_iter", 200);
     options.SetOption(id, "nlp_lower_bound_inf", -1e6);
     options.SetOption(id, "nlp_upper_bound_inf", 1e6);
     options.SetOption(id, "print_timing_statistics", "yes");
@@ -160,8 +170,8 @@ int DoMain(int argc, char* argv[]) {
 
     // Set to ignore overall tolerance/dual infeasibility, but terminate when
     // primal feasible and objective fails to increase over 5 iterations.
-    options.SetOption(id, "acceptable_compl_inf_tol", traj_params.tol);
-    options.SetOption(id, "acceptable_constr_viol_tol", traj_params.tol);
+    options.SetOption(id, "acceptable_compl_inf_tol", gains.tol);
+    options.SetOption(id, "acceptable_constr_viol_tol", gains.tol);
     options.SetOption(id, "acceptable_obj_change_tol", 1e-3);
     options.SetOption(id, "acceptable_tol", 1e2);
     options.SetOption(id, "acceptable_iter", 1);
@@ -170,12 +180,15 @@ int DoMain(int argc, char* argv[]) {
     mpc.Build(options);
   }
 
-    double alpha = .2;
-    mpc.CreateVisualizationCallback(
-        "examples/Cassie/urdf/cassie_fixed_springs.urdf", alpha);
-
   std::cout << "Solving optimization\n\n";
-  const auto pp_xtraj = mpc.Solve();
+  auto pp_xtraj = mpc.Solve();
+
+  auto lcm_traj = mpc.GenerateLcmTraj(FLAGS_knot_points_to_publish);
+  auto lcm = std::make_unique<drake::lcm::DrakeLcm>();
+  dairlib::lcmt_timestamped_saved_traj timestamped_msg;
+  timestamped_msg.saved_traj = lcm_traj;
+  timestamped_msg.utime = 1;
+  drake::lcm::Publish(lcm.get(), FLAGS_channel_reference, timestamped_msg);
   try {
     mpc.SaveSolutionToFile("examples/Cassie/saved_trajectories/kcmpc_solution");
   } catch (...) {
@@ -184,8 +197,6 @@ int DoMain(int argc, char* argv[]) {
               << std::endl;
   }
 
-  mpc.PublishSolution(FLAGS_channel_reference, FLAGS_knot_points_to_publish);
-
   auto traj_source =
       builder.AddSystem<drake::systems::TrajectorySource>(pp_xtraj);
   auto passthrough = builder.AddSystem<dairlib::systems::SubvectorPassThrough>(
@@ -207,7 +218,7 @@ int DoMain(int argc, char* argv[]) {
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(1);
+    simulator.set_target_realtime_rate(1.0);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
     sleep(2);
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
similarity index 74%
rename from examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.cc
rename to examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index c9721f6fe1..c5608e5b4c 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -1,47 +1,10 @@
-#include "cassie_kinematic_centroidal_mpc.h"
-#include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/multibody/parsing/parser.h"
+#include "cassie_kinematic_centroidal_solver.h"
+#include "drake/multibody/parsing/parser.h"
 #include "common/find_resource.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h"
 
-std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalMPC::CreateContactPoints(const drake::multibody::MultibodyPlant<
-    double> &plant,
-                                                                                                               double mu) {
-  auto left_toe_pair = dairlib::LeftToeFront(plant);
-  auto left_heel_pair = dairlib::LeftToeRear(plant);
-  auto right_toe_pair = dairlib::RightToeFront(plant);
-  auto right_heel_pair = dairlib::RightToeRear(plant);
-
-  std::vector<int> active_inds{0, 1, 2};
-
-  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, left_toe_pair.first, left_toe_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  left_toe_eval.set_frictional();
-  left_toe_eval.set_mu(mu);
-
-  auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, left_heel_pair.first, left_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  left_heel_eval.set_frictional();
-  left_heel_eval.set_mu(mu);
-
-  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, right_toe_pair.first, right_toe_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  right_toe_eval.set_frictional();
-  right_toe_eval.set_mu(mu);
-
-  auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, right_heel_pair.first, right_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  right_heel_eval.set_frictional();
-  right_heel_eval.set_mu(mu);
-
-  return {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval};
-}
-
-std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalMPC::CreateSlipContactPoints(const drake::multibody::MultibodyPlant<
+std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalSolver::CreateSlipContactPoints(const drake::multibody::MultibodyPlant<
     double> &plant, double mu) {
   auto left_toe_pair = dairlib::LeftToeFront(plant);
   auto right_toe_pair = dairlib::RightToeFront(plant);
@@ -62,7 +25,7 @@ std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCent
   return {left_toe_eval, right_toe_eval};
 }
 
-void CassieKinematicCentroidalMPC::AddLoopClosure() {
+void CassieKinematicCentroidalSolver::AddLoopClosure() {
   loop_closure_evaluators.add_evaluator(&l_loop_evaluator_);
   loop_closure_evaluators.add_evaluator(&r_loop_evaluator_);
   auto loop_closure =
@@ -77,7 +40,7 @@ void CassieKinematicCentroidalMPC::AddLoopClosure() {
 
 }
 
-void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
+void CassieKinematicCentroidalSolver::AddPlanarSlipConstraints(int knot_point) {
   for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
 
     prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 1.5);
@@ -109,7 +72,7 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipConstraints(int knot_point) {
   }
 }
 
-void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double terminal_gain) {
+void CassieKinematicCentroidalSolver::AddPlanarSlipCost(int knot_point, double terminal_gain) {
   const double t = dt_ * knot_point;
   if (com_ref_traj_) {
     prog_->AddQuadraticErrorCost(terminal_gain * Q_com_,
@@ -161,21 +124,21 @@ void CassieKinematicCentroidalMPC::AddPlanarSlipCost(int knot_point, double term
   }
 }
 
-void CassieKinematicCentroidalMPC::SetModeSequence(const std::vector<std::vector<bool>> &contact_sequence) {
-  KinematicCentroidalMPC::SetModeSequence(contact_sequence);
+void CassieKinematicCentroidalSolver::SetModeSequence(const std::vector<std::vector<bool>> &contact_sequence) {
+  KinematicCentroidalSolver::SetModeSequence(contact_sequence);
   MapModeSequence();
 }
-void CassieKinematicCentroidalMPC::SetModeSequence(const drake::trajectories::PiecewisePolynomial<double> &contact_sequence) {
-  KinematicCentroidalMPC::SetModeSequence(contact_sequence);
+void CassieKinematicCentroidalSolver::SetModeSequence(const drake::trajectories::PiecewisePolynomial<double> &contact_sequence) {
+  KinematicCentroidalSolver::SetModeSequence(contact_sequence);
   MapModeSequence();
 }
 
-void CassieKinematicCentroidalMPC::MapModeSequence() {
+void CassieKinematicCentroidalSolver::MapModeSequence() {
   for (int knot_point = 0; knot_point < num_knot_points(); knot_point++) {
     slip_contact_sequence_[knot_point] = complex_mode_to_slip_mode_.at(contact_sequence_[knot_point]);
   }
 }
-void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
+void CassieKinematicCentroidalSolver::AddSlipReductionConstraint(int knot_point) {
   prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point));
   prog_->AddConstraint(slip_vel_vars_[knot_point] * m_ == momentum_vars(knot_point).tail(3));
   prog_->AddConstraint(slip_contact_pos_vars(knot_point,0) == contact_pos_vars(knot_point,0));
@@ -189,7 +152,7 @@ void CassieKinematicCentroidalMPC::AddSlipReductionConstraint(int knot_point) {
                        {com_pos_vars(knot_point), slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
 }
 
-void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
+void CassieKinematicCentroidalSolver::AddSlipLiftingConstraint(int knot_point) {
   auto lifting_constraint =
       std::make_shared<PlanarSlipLiftingConstraint>(
           plant_, lifters_[knot_point], 2, n_contact_points_,knot_point);
@@ -199,7 +162,7 @@ void CassieKinematicCentroidalMPC::AddSlipLiftingConstraint(int knot_point) {
                         state_vars(knot_point)});
 }
 
-void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
+void CassieKinematicCentroidalSolver::AddSlipDynamics(int knot_point) {
   if(!is_last_knot(knot_point)) {
     auto slip_com_dynamics =
         std::make_shared<PlanarSlipDynamicsConstraint<double>>(
@@ -218,25 +181,25 @@ void CassieKinematicCentroidalMPC::AddSlipDynamics(int knot_point) {
   }
 }
 
-drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_pos_vars(int knot_point_index,
-                                                                                            int slip_foot_index) {
+drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalSolver::slip_contact_pos_vars(int knot_point_index,
+                                                                                               int slip_foot_index) {
   return slip_contact_pos_vars_[knot_point_index].segment(3 * slip_foot_index, 3);
 }
-drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalMPC::slip_contact_vel_vars(int knot_point_index,
-                                                                                            int slip_foot_index) {
+drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalSolver::slip_contact_vel_vars(int knot_point_index,
+                                                                                               int slip_foot_index) {
   return slip_contact_vel_vars_[knot_point_index].segment(3 * slip_foot_index, 3);
 }
 
-void CassieKinematicCentroidalMPC::SetComPositionGuess(const drake::trajectories::PiecewisePolynomial<double> &com_trajectory) {
+void CassieKinematicCentroidalSolver::SetComPositionGuess(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],
                            com_trajectory.value(dt_ * knot_point));
   }
-  KinematicCentroidalMPC::SetComPositionGuess(com_trajectory);
+  KinematicCentroidalSolver::SetComPositionGuess(com_trajectory);
 }
 
-void CassieKinematicCentroidalMPC::SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double> &q_traj,
-                                                      const drake::trajectories::PiecewisePolynomial<double> &v_traj) {
+void CassieKinematicCentroidalSolver::SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double> &q_traj,
+                                                         const drake::trajectories::PiecewisePolynomial<double> &v_traj) {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     dairlib::multibody::SetPositionsIfNew<double>(plant_, q_traj.value(dt_*knot_point),contexts_[knot_point].get());
     dairlib::multibody::SetVelocitiesIfNew<double>(plant_, v_traj.value(dt_*knot_point),contexts_[knot_point].get());
@@ -247,18 +210,18 @@ void CassieKinematicCentroidalMPC::SetRobotStateGuess(const drake::trajectories:
                              slip_contact_points_[contact].EvalFullTimeDerivative(*contexts_[knot_point]));
     }
   }
-  KinematicCentroidalMPC::SetRobotStateGuess(q_traj, v_traj);
+  KinematicCentroidalSolver::SetRobotStateGuess(q_traj, v_traj);
 }
 
-void CassieKinematicCentroidalMPC::SetMomentumGuess(const drake::trajectories::PiecewisePolynomial<double> &momentum_trajectory) {
+void CassieKinematicCentroidalSolver::SetMomentumGuess(const drake::trajectories::PiecewisePolynomial<double> &momentum_trajectory) {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     prog_->SetInitialGuess(slip_vel_vars_[knot_point],
                            drake::VectorX<double>(momentum_trajectory.value(dt_ * knot_point)).tail(3)/m_);
   }
-  KinematicCentroidalMPC::SetMomentumGuess(momentum_trajectory);
+  KinematicCentroidalSolver::SetMomentumGuess(momentum_trajectory);
 }
 
-drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_point) {
+drake::VectorX<double> CassieKinematicCentroidalSolver::LiftSlipSolution(int knot_point) {
   drake::VectorX<double> slip_state(3 + 3 + 6 * slip_contact_points_.size() + slip_contact_points_.size());
   slip_state<<result_->GetSolution(slip_com_vars_[knot_point]),
       result_->GetSolution(slip_vel_vars_[knot_point]),
@@ -283,14 +246,14 @@ drake::VectorX<double> CassieKinematicCentroidalMPC::LiftSlipSolution(int knot_p
   return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
 }
 
-void CassieKinematicCentroidalMPC::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
+void CassieKinematicCentroidalSolver::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
   for(const auto& force_vars : slip_force_vars_){
     //TODO find better initial guess
     prog_->SetInitialGuess(force_vars, 0 * drake::VectorX<double>::Ones(force_vars.size()));
   }
-  KinematicCentroidalMPC::SetForceGuess(force_trajectory);
+  KinematicCentroidalSolver::SetForceGuess(force_trajectory);
 }
-void CassieKinematicCentroidalMPC::Build(const drake::solvers::SolverOptions &solver_options) {
+void CassieKinematicCentroidalSolver::Build(const drake::solvers::SolverOptions &solver_options) {
   for (int knot = 0; knot < n_knot_points_; knot++) {
     lifters_.emplace_back(new PlanarSlipLifter(plant_,
                                                contexts_[knot].get(),
@@ -309,5 +272,5 @@ void CassieKinematicCentroidalMPC::Build(const drake::solvers::SolverOptions &so
                                                 k_,
                                                 b_, r0_, slip_contact_sequence_[knot]));
   }
-  KinematicCentroidalMPC::Build(solver_options);
+  KinematicCentroidalSolver::Build(solver_options);
 }
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
similarity index 83%
rename from examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h
rename to examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index beb674f5eb..36bb5698ab 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_mpc.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -1,26 +1,27 @@
 #pragma once
 #include <iostream>
-#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h"
 
 /*!
  * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop closure, joint limits, and cassie contact points
  */
-class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
+class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
  public:
-  CassieKinematicCentroidalMPC(const drake::multibody::MultibodyPlant<double>& plant,
-                               int n_knot_points,
-                               double dt,
-                               double mu,
-                               const drake::VectorX<double>& nominal_stand,
-                               double k = 1000,
-                               double b = 20,
-                               double r0 = 0.5,
-                               double stance_width = 0.2) :
-      KinematicCentroidalMPC(plant, n_knot_points, dt, CreateContactPoints(plant, mu)),
+  CassieKinematicCentroidalSolver(const drake::multibody::MultibodyPlant<double>& plant,
+                                  int n_knot_points,
+                                  double dt,
+                                  double mu,
+                                  const drake::VectorX<double>& nominal_stand,
+                                  double k = 1000,
+                                  double b = 20,
+                                  double r0 = 0.5,
+                                  double stance_width = 0.2) :
+      KinematicCentroidalSolver(plant, n_knot_points, dt, CreateContactPoints(plant, mu)),
       l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
       r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
       loop_closure_evaluators(Plant()),
@@ -52,14 +53,6 @@ class CassieKinematicCentroidalMPC : public KinematicCentroidalMPC {
     m_=plant_.CalcTotalMass(*contexts_[0]);
   }
 
-  /*!
-   * @brief creates vector of world point evaluators for cassie
-   * @param plant cassie plant
-   * @param mu coefficient of friction
-   * @return
-   */
-  std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
-
   std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateSlipContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
 
   /*!
diff --git a/examples/Cassie/kinematic_centroidal_planner/contact_scheduler.cc b/examples/Cassie/kinematic_centroidal_planner/contact_scheduler.cc
index 4ceb660994..e6c86bb8f5 100644
--- a/examples/Cassie/kinematic_centroidal_planner/contact_scheduler.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/contact_scheduler.cc
@@ -1,12 +1,12 @@
+#include "contact_scheduler.h"
+
 #include <iostream>
-#include "examples/Cassie/kinematic_centroidal_planner/contact_scheduler.h"
 
+using drake::systems::BasicVector;
 using drake::systems::Context;
 using drake::systems::DiscreteUpdateEvent;
 using drake::systems::DiscreteValues;
 using drake::systems::EventStatus;
-using drake::systems::BasicVector;
-using drake::systems::Context;
 using drake::trajectories::PiecewisePolynomial;
 using Eigen::VectorXd;
 using std::string;
@@ -15,10 +15,9 @@ namespace dairlib {
 namespace systems {
 
 ContactScheduler::ContactScheduler(
-    const drake::multibody::MultibodyPlant<double> &plant,
+    const drake::multibody::MultibodyPlant<double>& plant,
     const std::unordered_map<int, int>& contact_state_to_fsm_state_map)
     : contact_state_to_fsm_state_map_(contact_state_to_fsm_state_map) {
-
   // Input/Output Setup
   state_input_port_ =
       this->DeclareVectorInputPort("x, u, t",
@@ -29,32 +28,30 @@ ContactScheduler::ContactScheduler(
   PiecewisePolynomial<double> pp = PiecewisePolynomial<double>();
   contact_force_trajectory_input_port_ =
       this->DeclareAbstractInputPort(
-              "lambda",
-              drake::Value<PiecewisePolynomial<double>>(pp))
+              "lambda", drake::Value<PiecewisePolynomial<double>>(pp))
+          .get_index();
+  fsm_output_port_ =
+      this->DeclareVectorOutputPort("fsm", BasicVector<double>(1),
+                                    &ContactScheduler::CalcFiniteState)
           .get_index();
-  fsm_output_port_ = this->DeclareVectorOutputPort(
-          "fsm", BasicVector<double>(1),
-          &ContactScheduler::CalcFiniteState)
-      .get_index();
 }
 
-void ContactScheduler::CalcFiniteState(
-    const Context<double> &context, BasicVector<double> *fsm_state) const {
+void ContactScheduler::CalcFiniteState(const Context<double>& context,
+                                       BasicVector<double>* fsm_state) const {
   // Read in lcm message time
-  const OutputVector<double> *robot_output =
-      (OutputVector<double> *) this->EvalVectorInput(context,
-                                                     state_input_port_);
+  const OutputVector<double>* robot_output =
+      (OutputVector<double>*)this->EvalVectorInput(context, state_input_port_);
   auto current_sim_time = static_cast<double>(robot_output->get_timestamp());
-  const auto &contact_force_trajectory =
+  const auto& contact_force_trajectory =
       this->EvalInputValue<drake::trajectories::PiecewisePolynomial<double>>(
           context, contact_force_trajectory_input_port_);
   VectorXd desired_contact = contact_force_trajectory->value(current_sim_time);
 
   /// shift the active contacts to form contact state
-  const int contact_state = ((desired_contact[2] > kEpislonForce) * 1 << 0)
-      + ((desired_contact[5] > kEpislonForce) * 1 << 1)
-      + ((desired_contact[8] > kEpislonForce) * 1 << 2)
-      + ((desired_contact[11] > kEpislonForce) * 1 << 3);
+  const int contact_state = ((desired_contact[2] > kEpislonForce) * 1 << 0) +
+                            ((desired_contact[5] > kEpislonForce) * 1 << 1) +
+                            ((desired_contact[8] > kEpislonForce) * 1 << 2) +
+                            ((desired_contact[11] > kEpislonForce) * 1 << 3);
 
   // Assign fsm_state
   fsm_state->get_mutable_value()[0] =
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/gaits/BUILD.bazel
index c5a8d79ca4..d947cc5af7 100644
--- a/examples/Cassie/kinematic_centroidal_planner/gaits/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/BUILD.bazel
@@ -1,9 +1,12 @@
 package(default_visibility = ["//visibility:public"])
 
-exports_files(["stand.yaml"])
 
-exports_files(["walk.yaml"])
-
-exports_files(["step.yaml"])
-
-exports_files(["run.yaml"])
+filegroup(
+    name = "gaits",
+    data = ["stand.yaml",
+           "walk.yaml",
+           "jump.yaml",
+           "left_step.yaml",
+           "right_step.yaml"],
+    visibility = ["//visibility:public"],
+)
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/jump.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/jump.yaml
new file mode 100644
index 0000000000..944dd4ecbd
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/jump.yaml
@@ -0,0 +1,11 @@
+gait_pattern:
+  - start_phase: 0.0
+    end_phase: 0.4
+    contact_status: [true, true, true, true]
+  - start_phase: 0.4
+    end_phase: 0.75
+    contact_status: [false, false, false, false]
+  - start_phase: 0.75
+    end_phase: 1.0
+    contact_status: [true, true, true, true]
+period: 1.0
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/left_step.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/left_step.yaml
new file mode 100644
index 0000000000..34ec79c6b3
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/left_step.yaml
@@ -0,0 +1,8 @@
+gait_pattern:
+  - start_phase: 0.0
+    end_phase: 0.7
+    contact_status: [ false, false, true, true ]
+  - start_phase: 0.7
+    end_phase: 1.0
+    contact_status: [ true, true, true, true ]
+period: 0.5
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/right_step.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/right_step.yaml
new file mode 100644
index 0000000000..0e1fe0e976
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/right_step.yaml
@@ -0,0 +1,8 @@
+gait_pattern:
+  - start_phase: 0.0
+    end_phase: 0.7
+    contact_status: [ true, true, false, false ]
+  - start_phase: 0.7
+    end_phase: 1.0
+    contact_status: [ true, true, true, true ]
+period: 0.5
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
index ef0181b9ca..cc525ff774 100644
--- a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
@@ -2,4 +2,4 @@ gait_pattern:
   - start_phase: 0.0
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 0.5
+period: 1.0
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml
deleted file mode 100644
index 807818eaa4..0000000000
--- a/examples/Cassie/kinematic_centroidal_planner/gaits/step.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-gait_pattern:
-  - start_phase: 0.0
-    end_phase: 0.9
-    contact_status: [true, true, false, false]
-  - start_phase: 0.9
-    end_phase: 1.0
-    contact_status: [true, true, true, true]
-period: 0.5
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
index 2589a35887..c0107c8515 100644
--- a/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
@@ -11,4 +11,4 @@ gait_pattern:
   - start_phase: 0.9
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 1.25
+period: 1.6
diff --git a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
index fdf924c7c8..20b3554563 100644
--- a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
@@ -31,6 +31,6 @@ contact_pos: [0.1, 16, 0.001]
 contact_vel: [0.02, 0.02, 0.02]
 contact_force: [0.0001, 0.0001, 0.0001]
 lin_momentum: [0.00001, 0.00001, 0.00001]
-ang_momentum: [0.0000000, 0.0000000, 0.0000000]
+ang_momentum: [0.0000001, 0.0000001, 0.0000001]
 
-swing_foot_minimum_height: 0.015
+tol: 1e-2
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel
new file mode 100644
index 0000000000..a68c695a36
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel
@@ -0,0 +1,12 @@
+package(default_visibility = ["//visibility:public"])
+
+filegroup(
+    name = "motions",
+    data = ["motion_test.yaml",
+            "motion_left_step.yaml",
+            "motion_right_step.yaml",
+           "motion_jump.yaml",
+           "motion_walk.yaml"],
+    visibility = ["//visibility:public"],
+)
+
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
new file mode 100644
index 0000000000..ffc7579313
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
@@ -0,0 +1,11 @@
+n_knot_points: 25
+target_com_height: 0.8
+stance_width: 0.25
+swing_foot_minimum_height: 0.015
+duration_scaling:
+  [ 1.0, 1.0, 1.0 ]
+gait_sequence: [ 'stand', 'jump', 'stand' ]
+com_vel_values:
+  [ 0, 0, 0,
+    0.5, 0, 0,
+    0, 0, 0 ]
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
new file mode 100644
index 0000000000..aee4daa77a
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
@@ -0,0 +1,11 @@
+n_knot_points: 20
+target_com_height: 0.8
+stance_width: 0.25
+swing_foot_minimum_height: 0.1
+duration_scaling:
+  [ 1.0, 1.0, 1.0 ]
+gait_sequence: [ 'stand', 'left_step', 'stand' ]
+com_vel_values:
+  [ 0, 0, 0,
+    1.0, 0, 0,
+    0, 0, 0 ]
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
new file mode 100644
index 0000000000..feb7ef4c5b
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
@@ -0,0 +1,11 @@
+n_knot_points: 20
+target_com_height: 0.8
+stance_width: 0.25
+swing_foot_minimum_height: 0.1
+duration_scaling:
+  [ 1.0, 1.0, 1.0 ]
+gait_sequence: [ 'stand', 'right_step', 'stand' ]
+com_vel_values:
+  [ 0, 0, 0,
+    1.0, 0, 0,
+    0, 0, 0 ]
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
new file mode 100644
index 0000000000..ee2d58ea91
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
@@ -0,0 +1,11 @@
+n_knot_points: 25
+target_com_height: 0.8
+stance_width: 0.25
+swing_foot_minimum_height: 0.03
+duration_scaling:
+  [ 1.0, 1.0, 1.0 ]
+gait_sequence: [ 'stand', 'left_step', 'stand' ]
+com_vel_values:
+  [ 0, 0, 0,
+    0, 0, 0,
+    0, 0, 0 ]
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
new file mode 100644
index 0000000000..02e3834e1b
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
@@ -0,0 +1,11 @@
+n_knot_points: 45
+target_com_height: 0.8
+stance_width: 0.25
+swing_foot_minimum_height: 0.03
+duration_scaling:
+  [ 1.0, 1.0, 1.0 ]
+gait_sequence: [ 'stand', 'walk', 'walk' ]
+com_vel_values:
+  [ 0, 0, 0,
+    0, 0, 0,
+    0, 0, 0 ]
diff --git a/examples/Cassie/kinematic_centroidal_planner/osc_centroidal_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/osc_centroidal_gains.yaml
index bf258425d3..3ab408fad4 100644
--- a/examples/Cassie/kinematic_centroidal_planner/osc_centroidal_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/osc_centroidal_gains.yaml
@@ -14,7 +14,7 @@ w_hip_yaw: 1
 hip_yaw_kp: 40
 hip_yaw_kd: 5
 
-impact_threshold: 0.050
+impact_threshold: 0.025
 
 x_offset: 0.025
 landing_delay: 0.050
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
index 5c54f61f88..ef4f486ddf 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
@@ -28,7 +28,7 @@ cc_binary(
         "//multibody:visualization_utils",
         "//multibody/kinematic",
         "//systems/primitives",
-        "//examples/Cassie/kinematic_centroidal_planner:kinematic_centroidal_reference_generator",
+        "//examples/Cassie/kinematic_centroidal_planner:cassie_reference_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
diff --git a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
index b1fa2bc6f3..dfa027ca20 100644
--- a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
+++ b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
@@ -6,19 +6,30 @@
 
 struct TrajectoryParameters {
   double n_knot_points;
-  double com_height;
+  double target_com_height;
   double stance_width;
-  double vel;
-  double tol;
+  double swing_foot_minimum_height;
+
   std::vector<double> duration_scaling;
+  std::vector<std::string> gait_sequence;
+  std::vector<double> com_vel_values;
+
+  std::vector<Eigen::Vector3d> com_vel_vector;
 
   template <typename Archive>
   void Serialize(Archive* a) {
     a->Visit(DRAKE_NVP(n_knot_points));
-    a->Visit(DRAKE_NVP(com_height));
+    a->Visit(DRAKE_NVP(target_com_height));
     a->Visit(DRAKE_NVP(stance_width));
-    a->Visit(DRAKE_NVP(vel));
-    a->Visit(DRAKE_NVP(tol));
+    a->Visit(DRAKE_NVP(swing_foot_minimum_height));
     a->Visit(DRAKE_NVP(duration_scaling));
+    a->Visit(DRAKE_NVP(gait_sequence));
+    a->Visit(DRAKE_NVP(com_vel_values));
+    Eigen::Map<Eigen::Matrix3Xd> input(com_vel_values.data(), 3,
+                                       com_vel_values.size() / 3);
+    for (int i = 0; i < input.cols(); ++i) {
+      com_vel_vector.push_back(input.col(i));
+    }
+    DRAKE_DEMAND(duration_scaling.size() == gait_sequence.size());
   }
-};
\ No newline at end of file
+};
diff --git a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml
deleted file mode 100644
index 02bfc91688..0000000000
--- a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-n_knot_points: 45
-duration_scaling:
-  [1.0, 3.0, 1.0]
-com_height: 0.8
-stance_width: 0.25
-vel: 0.5
-tol: 1e-2
\ No newline at end of file
diff --git a/examples/Cassie/run_osc_walking_controller.cc b/examples/Cassie/run_osc_walking_controller.cc
index d36a9d96f1..17d1d92d7b 100644
--- a/examples/Cassie/run_osc_walking_controller.cc
+++ b/examples/Cassie/run_osc_walking_controller.cc
@@ -181,7 +181,7 @@ int DoMain(int argc, char* argv[]) {
             FLAGS_cassie_out_channel, &lcm_local));
     builder.Connect(*cassie_out_receiver, *cassie_out_to_radio);
     builder.Connect(cassie_out_to_radio->get_output_port(),
-                    high_level_command->get_radio_port());
+                    high_level_command->get_input_port_radio());
   } else {
     high_level_command = builder.AddSystem<cassie::osc::HighLevelCommand>(
         plant_w_spr, context_w_spr.get(), gains.kp_yaw, gains.kd_yaw,
diff --git a/examples/Cassie/run_osc_walking_controller_alip.cc b/examples/Cassie/run_osc_walking_controller_alip.cc
index 80d1cf536c..404979a24c 100644
--- a/examples/Cassie/run_osc_walking_controller_alip.cc
+++ b/examples/Cassie/run_osc_walking_controller_alip.cc
@@ -199,7 +199,7 @@ int DoMain(int argc, char* argv[]) {
             FLAGS_cassie_out_channel, &lcm_local));
     builder.Connect(*cassie_out_receiver, *cassie_out_to_radio);
     builder.Connect(cassie_out_to_radio->get_output_port(),
-                    high_level_command->get_radio_port());
+                    high_level_command->get_input_port_radio());
   } else {
     high_level_command = builder.AddSystem<cassie::osc::HighLevelCommand>(
         plant_w_spr, context_w_spr.get(), gains.kp_yaw, gains.kd_yaw,
diff --git a/lcmtypes/lcmt_timestamped_saved_traj.lcm b/lcmtypes/lcmt_timestamped_saved_traj.lcm
new file mode 100644
index 0000000000..7a49bd7c2b
--- /dev/null
+++ b/lcmtypes/lcmt_timestamped_saved_traj.lcm
@@ -0,0 +1,10 @@
+package dairlib;
+
+/*  lcmtype analog for LcmTrajectory WITH TIMESTAMP to enable using a
+   LcmSerializer to save/load trajectories
+*/
+
+struct lcmt_timestamped_saved_traj {
+  int64_t utime;
+  lcmt_saved_traj saved_traj;
+}
diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel
index 459862d4bf..876cf5477f 100644
--- a/solvers/BUILD.bazel
+++ b/solvers/BUILD.bazel
@@ -14,6 +14,7 @@ cc_library(
 cc_library(
     name = "osqp_solver_options",
     hdrs = ["osqp_solver_options.h"],
+    data = ["osqp_options_default.yaml"],
     deps = [
         "@drake//:drake_shared_library",
     ],
diff --git a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
index 9e2f8c1fa2..628b05d616 100644
--- a/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
+++ b/systems/controllers/kinematic_centroidal_mpc/BUILD.bazel
@@ -1,42 +1,32 @@
 package(default_visibility = ["//visibility:public"])
 
-
 cc_library(
     name = "kinematic_centroidal_mpc",
     srcs = ["kinematic_centroidal_mpc.cc",
-            "kinematic_centroidal_constraints.cc"],
+            ],
     hdrs = ["kinematic_centroidal_mpc.h",
-            "kinematic_centroidal_constraints.h"],
+            ],
     deps = [
-        "//solvers:constraints",
-        "//multibody/kinematic:kinematic",
-        "//multibody/kinematic:constraints",
-        "//multibody:multipose_visualizer",
-        "//systems/controllers/kinematic_centroidal_mpc:kcmpc_gains",
-        "//lcm:lcm_trajectory_saver",
+        "//systems/trajectory_optimization/kinematic_centroidal_planner:kinematic_centroidal_planner",
+        "//examples/Cassie/kinematic_centroidal_planner:cassie_kinematic_centroidal_mpc",
+        "//examples/Cassie/kinematic_centroidal_planner:trajectory_parameters",
+        "//examples/Cassie/kinematic_centroidal_planner:cassie_reference_utils",
+        "//systems/framework:vector",
         "@drake//:drake_shared_library",
     ],
 )
 
 cc_library(
-    name = "kcmpc_gains",
-    hdrs = ["kinematic_centroidal_gains.h"],
+    name = "kinematic_centroidal_visualizer",
+    srcs = ["kinematic_centroidal_visualizer.cc",
+            ],
+    hdrs = ["kinematic_centroidal_visualizer.h",
+            ],
     deps = [
-        "@drake//:drake_shared_library",
-    ],
-)
-
-cc_library(
-    name = "kinematic_centroidal_reference_generator",
-    srcs = ["kcmpc_reference_generator.cc",
-            "reference_generation_utils.cc",
-            "gait.cc"],
-    hdrs = ["kcmpc_reference_generator.h",
-            "reference_generation_utils.h",
-            "gait.h"],
-    deps = [
-        "//common",
-        "//multibody/kinematic:kinematic",
+        "//systems/framework:vector",
+        "//multibody:multipose_visualizer",
+        "//lcmtypes:lcmt_robot",
+        "//lcm:lcm_trajectory_saver",
         "@drake//:drake_shared_library",
     ],
 )
diff --git a/systems/controllers/kinematic_centroidal_mpc/README.md b/systems/controllers/kinematic_centroidal_mpc/README.md
new file mode 100644
index 0000000000..918407c294
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/README.md
@@ -0,0 +1,26 @@
+This directory contains the necessary LeafSystems (
+see `drake::systems::LeafSystem`) to generate kinematic centroidal reference
+trajectories based on the current robot state.
+
+The generation is split up into two LeafSystems:
+- KinematicCentroidalReferenceGenerator
+  - this class is responsible for outputting a `KCReferenceVector` which contains reference trajectories for:
+    - force
+    - generalized positions
+    - generalized velocities
+    - center of mass
+    - boolean contact activation
+  - we separate this reference generator into a separate class in order to support multiple reference generators
+  - inputs:
+    - target velocity (target center of mass translation velocity as a trajectory)
+    - (potentially) gait specification, currently fixed
+    - (potentially) gait durations
+    - current robot state (used in computing the reference trajectories)
+- KinematicCentroidalMPC
+  - this class solves the KinematicCentroidal planning problem
+  - inputs:
+    - current robot state (used in initial state constraint)
+    - `KCReferenceVector` (target reference)
+  - output:
+    - `lcmt_saved_traj` (contains state and contact force trajectories)
+    - 
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 51c66d6c20..374ea4f982 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -1,702 +1,225 @@
 #include "kinematic_centroidal_mpc.h"
 
-#include <iostream>
-#include <utility>
+#include <drake/common/yaml/yaml_io.h>
 
-#include "lcm/lcm_trajectory.h"
-#include "multibody/kinematic/kinematic_constraints.h"
-#include "multibody/multibody_utils.h"
-#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h"
+//#include "multibody/multibody_utils.h"
+#include "common/eigen_utils.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
+#include "systems/framework/output_vector.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h"
 
 #include "drake/common/trajectories/piecewise_polynomial.h"
-
-using dairlib::LcmTrajectory;
-using Eigen::MatrixXd;
+#include "drake/systems/framework/leaf_system.h"
+//#include <iostream>
+
+using dairlib::systems::BasicVector;
+using dairlib::systems::OutputVector;
+using drake::systems::Context;
+using drake::systems::DiscreteValues;
+using drake::systems::EventStatus;
+using drake::trajectories::Trajectory;
 using Eigen::VectorXd;
 
-KinematicCentroidalMPC::KinematicCentroidalMPC(
-    const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points,
-    double dt,
-    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
-        contact_points)
-    : plant_(plant),
-      n_knot_points_(n_knot_points),
-      dt_(dt),
-      contact_points_(contact_points),
-      n_q_(plant.num_positions()),
-      n_v_(plant.num_velocities()),
-      n_contact_points_(contact_points.size()),
-      Q_q_(Eigen::MatrixXd::Zero(n_q_, n_q_)),
-      Q_v_(Eigen::MatrixXd::Zero(n_v_, n_v_)),
-      Q_com_(Eigen::MatrixXd::Zero(3, 3)),
-      Q_mom_(Eigen::MatrixXd::Zero(6, 6)),
-      Q_contact_(
-          Eigen::MatrixXd::Zero(6 * n_contact_points_, 6 * n_contact_points_)),
-      Q_force_(
-          Eigen::MatrixXd::Zero(3 * n_contact_points_, 3 * n_contact_points_)),
-      contact_sequence_(n_knot_points),
-      contexts_(n_knot_points),
-      complexity_schedule_(n_knot_points){
-  n_joint_q_ = n_q_ - kCentroidalPosDim;
-  n_joint_v_ = n_v_ - kCentroidalVelDim;
-  prog_ = std::make_unique<drake::solvers::MathematicalProgram>();
-  solver_ = std::make_unique<drake::solvers::IpoptSolver>();
-  result_ = std::make_unique<drake::solvers::MathematicalProgramResult>();
-  for (int contact_index = 0; contact_index < n_contact_points_;
-       contact_index++) {
-    contact_sets_.emplace_back(plant_);
-    contact_sets_[contact_index].add_evaluator(&contact_points_[contact_index]);
-  }
-
-  for (int knot = 0; knot < n_knot_points; knot++) {
-    contexts_[knot] = plant_.CreateDefaultContext();
-    x_vars_.push_back(prog_->NewContinuousVariables(
-        n_q_ + n_v_, "x_vars_" + std::to_string(knot)));
-    mom_vars_.push_back(
-        prog_->NewContinuousVariables(6, "mom_vars_" + std::to_string(knot)));
-    com_vars_.push_back(
-        prog_->NewContinuousVariables(3, "com_vars_" + std::to_string(knot)));
-    contact_pos_.push_back(prog_->NewContinuousVariables(
-        3 * n_contact_points_, "contact_pos_" + std::to_string(knot)));
-    contact_vel_.push_back(prog_->NewContinuousVariables(
-        3 * n_contact_points_, "contact_vel_" + std::to_string(knot)));
-    contact_force_.push_back(prog_->NewContinuousVariables(
-        3 * n_contact_points_, "contact_force_" + std::to_string(knot)));
-  }
-  std::vector<bool> stance_mode(n_contact_points_);
-  std::fill(stance_mode.begin(), stance_mode.end(), true);
-  std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
-  std::fill(complexity_schedule_.begin(), complexity_schedule_.end(), Complexity::KINEMATIC_CENTROIDAL);
-}
-
-void KinematicCentroidalMPC::AddGenPosReference(
-    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
-  q_ref_traj_ = std::move(ref_traj);
-}
-
-void KinematicCentroidalMPC::AddGenVelReference(
-    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
-  v_ref_traj_ = std::move(ref_traj);
-}
-
-void KinematicCentroidalMPC::AddContactTrackingReference(
-    std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj) {
-  contact_ref_traj_ = std::move(contact_ref_traj);
-}
-
-void KinematicCentroidalMPC::AddForceTrackingReference(
-    std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj) {
-  force_ref_traj_ = std::move(force_ref_traj);
-}
-
-void KinematicCentroidalMPC::AddComReference(
-    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
-  com_ref_traj_ = std::move(ref_traj);
-}
-
-void KinematicCentroidalMPC::AddMomentumReference(
-    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
-  mom_ref_traj_ = std::move(ref_traj);
-}
-
-void KinematicCentroidalMPC::AddCentroidalDynamics(int knot_point) {
-  if(!(is_last_knot(knot_point))){
-    auto constraint = std::make_shared<CentroidalDynamicsConstraint<double>>(
-        plant_, contexts_[knot_point].get(), n_contact_points_, dt_,
-        knot_point);
-    centroidal_dynamics_binding_.push_back(prog_->AddConstraint(
-        constraint,
-        {momentum_vars(knot_point), momentum_vars(knot_point + 1),
-         com_pos_vars(knot_point), contact_pos_[knot_point],
-         contact_force_[knot_point], com_pos_vars(knot_point + 1),
-         contact_pos_[knot_point + 1], contact_force_[knot_point + 1]}));
-  }
-}
-
-void KinematicCentroidalMPC::AddKinematicsIntegrator(int knot_point) {
-  if(!is_last_knot(knot_point)) {
-    // Integrate generalized velocities to get generalized positions
-    auto constraint = std::make_shared<KinematicIntegratorConstraint<double>>(
-        plant_, contexts_[knot_point].get(), contexts_[knot_point + 1].get(),
-        dt_, knot_point);
-    prog_->AddConstraint(constraint, {state_vars(knot_point).head(n_q_),
-                                      state_vars(knot_point + 1).head(n_q_),
-                                      state_vars(knot_point).tail(n_v_),
-                                      state_vars(knot_point + 1).tail(n_v_)});
-
-    // Integrate foot states
-    for (int contact_index = 0; contact_index < n_contact_points_;
-         contact_index++) {
-      prog_->AddConstraint(
-          contact_pos_vars(knot_point + 1, contact_index) ==
-          contact_pos_vars(knot_point, contact_index) +
-              0.5 * dt_ *
-                  (contact_vel_vars(knot_point, contact_index) +
-                   contact_vel_vars(knot_point + 1, contact_index)));
-    }
-  }
-}
-
-void KinematicCentroidalMPC::AddContactConstraints(int knot_point) {
-  for (int contact_index = 0; contact_index < n_contact_points_;
-       contact_index++) {
-    // Make sure feet in stance are not moving and on the ground
-    if (contact_sequence_[knot_point][contact_index]) {
-      prog_->AddBoundingBoxConstraint(
-          Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
-          contact_vel_vars(knot_point, contact_index));
-      if (knot_point != 0) {
-        prog_->AddBoundingBoxConstraint(
-            0, 0, contact_pos_vars(knot_point, contact_index)[2]);
-      }
-    } else {
-      // Feet are above the ground
-      double lb = 0;
-      // Check if at least one of the time points before or after is also in
-      // 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
-          (!contact_sequence_[knot_point - 1][contact_index] or
-           !contact_sequence_[knot_point + 1][contact_index])) {
-        lb = swing_foot_minimum_height_;
-      }
-      prog_->AddBoundingBoxConstraint(
-          lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
-    }
-  }
-}
-
-void KinematicCentroidalMPC::AddCentroidalKinematicConsistency(int knot_point) {
-  // Ensure linear and angular momentum line up
-  auto centroidal_momentum =
-      std::make_shared<CentroidalMomentumConstraint<double>>(
-          plant_, contexts_[knot_point].get(), knot_point);
-  prog_->AddConstraint(centroidal_momentum,
-                       {state_vars(knot_point), com_pos_vars(knot_point),
-                        momentum_vars(knot_point)});
-  for (int contact_index = 0; contact_index < n_contact_points_;
-       contact_index++) {
-    // Ensure foot position line up with kinematics
-    auto foot_position_constraint = std::make_shared<
-        dairlib::multibody::KinematicPositionConstraint<double>>(
-        plant_, contact_sets_[contact_index], Eigen::Vector3d::Zero(),
-        Eigen::Vector3d::Zero(), full_constraint_relative_);
-    prog_->AddConstraint(foot_position_constraint,
-                         {state_vars(knot_point).head(n_q_),
-                          contact_pos_vars(knot_point, contact_index)});
-  }
-  // Constrain com position
-  auto com_position =
-      std::make_shared<CenterofMassPositionConstraint<double>>(
-          plant_, contexts_[knot_point].get(), knot_point);
-  prog_->AddConstraint(com_position,
-                       {com_pos_vars(knot_point), state_vars(knot_point)});
-}
-
-void KinematicCentroidalMPC::AddFrictionConeConstraints(int knot_point) {
-  for (int contact_index = 0; contact_index < n_contact_points_;
-       contact_index++) {
-    auto force_constraints_vec =
-        contact_points_[contact_index].CreateLinearFrictionConstraints();
-    for (const auto& constraint : force_constraints_vec) {
-      prog_->AddConstraint(constraint,
-                           contact_force_vars(knot_point, contact_index));
-    }
-  }
-}
-
-void KinematicCentroidalMPC::AddFlightContactForceConstraints(int knot_point) {
-  for (int contact_index = 0; contact_index < n_contact_points_;
-       contact_index++) {
-    // Feet in flight produce no force
-    if (!contact_sequence_[knot_point][contact_index]) {
-      prog_->AddBoundingBoxConstraint(
-          Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
-          contact_force_vars(knot_point, contact_index));
-    }
-  }
-}
-
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::state_vars(
-    int knotpoint_index) const {
-  return x_vars_[knotpoint_index];
-}
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::joint_pos_vars(
-    int knotpoint_index) const {
-  return x_vars_[knotpoint_index].segment(kCentroidalPosDim, n_joint_q_);
-}
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::joint_vel_vars(
-    int knotpoint_index) const {
-  return x_vars_[knotpoint_index].segment(n_q_ + kCentroidalVelDim, n_joint_v_);
-}
+namespace dairlib {
 
-void KinematicCentroidalMPC::Build(
-    const drake::solvers::SolverOptions& solver_options) {
-  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
-    switch (complexity_schedule_[knot_point]) {
-      case KINEMATIC_CENTROIDAL:
-        //Add complex constraints
-        AddContactConstraints(knot_point);
-        AddCentroidalKinematicConsistency(knot_point);
-        AddFrictionConeConstraints(knot_point);
-        AddFlightContactForceConstraints(knot_point);
-        //Add complex dynamics
-        AddCentroidalDynamics(knot_point);
-        AddKinematicsIntegrator(knot_point);
-        if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
-          AddSlipReductionConstraint(knot_point + 1);
-          AddCentroidalKinematicConsistency(knot_point + 1);
-        }
-        break;
-      case PLANAR_SLIP:
-        AddPlanarSlipConstraints(knot_point);
-        if(!is_last_knot(knot_point)){
-          switch (complexity_schedule_[knot_point+1]) {
-            case KINEMATIC_CENTROIDAL:
-              AddCentroidalDynamics(knot_point);
-              AddKinematicsIntegrator(knot_point);
-              AddSlipLiftingConstraint(knot_point);
-              break;
-            case PLANAR_SLIP:
-              AddSlipDynamics(knot_point);
-              break;
-          }
-        }
-        break;
-    }
-  }
-  AddCosts();
-  prog_->SetSolverOptions(solver_options);
-}
-
-void KinematicCentroidalMPC::AddConstantMomentumReference(
-    const drake::VectorX<double>& value) {
-  AddMomentumReference(
+KinematicCentroidalMPC::KinematicCentroidalMPC(
+    const drake::multibody::MultibodyPlant<double>& plant_w_spr,
+    const drake::multibody::MultibodyPlant<double>& plant_wo_spr,
+    drake::systems::Context<double>* context,
+    const TrajectoryParameters& motion, const KinematicCentroidalGains& gains)
+    : plant_w_spr_(plant_w_spr),
+      plant_wo_spr_(plant_wo_spr),
+      context_wo_spr_(context),
+      world_(plant_wo_spr.world_frame()),
+      n_q_(plant_wo_spr.num_positions()),
+      n_v_(plant_wo_spr.num_velocities()) {
+  this->set_name("kinematic_centroidal_planner");
+
+  // Create gaits
+  auto stand = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml");
+  auto walk = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml");
+  auto right_step = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/right_step.yaml");
+  auto left_step = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/left_step.yaml");
+  auto jump = drake::yaml::LoadYamlFile<Gait>(
+      "examples/Cassie/kinematic_centroidal_planner/gaits/jump.yaml");
+
+  gait_library_["stand"] = stand;
+  gait_library_["walk"] = walk;
+  gait_library_["right_step"] = right_step;
+  gait_library_["left_step"] = left_step;
+  gait_library_["jump"] = jump;
+
+  for (const auto& gait : motion.gait_sequence) {
+    gait_samples_.push_back(gait_library_.at(gait));
+  }
+  DRAKE_DEMAND(gait_samples_.size() == motion.duration_scaling.size());
+
+  auto time_vector = KcmpcReferenceGenerator::GenerateTimePoints(
+      motion.duration_scaling, gait_samples_);
+
+  time_points_ = Eigen::Vector4d(time_vector.data());
+
+  n_knot_points_ = motion.n_knot_points;
+  // Create MPC and set gains
+  solver_ = std::make_unique<CassieKinematicCentroidalSolver>(
+      plant_wo_spr_, n_knot_points_, time_vector.back() / (n_knot_points_ - 1),
+      0.4);
+  solver_->SetGains(gains);
+  solver_->SetMinimumFootClearance(motion.swing_foot_minimum_height);
+
+  reference_generator_ = std::make_unique<KcmpcReferenceGenerator>(
+      plant_wo_spr_, context_wo_spr_, CreateContactPoints(plant_wo_spr_, 0));
+  reference_state_ = GenerateNominalStand(
+      plant_wo_spr_, motion.target_com_height, motion.stance_width, false);
+  reference_generator_->SetNominalReferenceConfiguration(
+      reference_state_.head(plant_wo_spr_.num_positions()));
+  reference_generator_->SetComKnotPoints({time_vector, motion.com_vel_vector});
+  reference_generator_->SetGaitSequence({time_vector, gait_samples_});
+  reference_generator_->Generate();
+
+  solver_->SetForceTrackingReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
-          value));
-}
-
-void KinematicCentroidalMPC::AddCosts() {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    const double terminal_gain = is_last_knot(knot_point) ? 50 : 1;
-    const double collocation_gain =
-        (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
-    double t = dt_ * knot_point;
-
-    switch (complexity_schedule_[knot_point]) {
-      case KINEMATIC_CENTROIDAL:
-        if (q_ref_traj_) {
-          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_q_,
-                                       q_ref_traj_->value(t),
-                                       state_vars(knot_point).head(n_q_));
-        }
-        if (v_ref_traj_) {
-          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_v_,
-                                       v_ref_traj_->value(t),
-                                       state_vars(knot_point).tail(n_v_));
-        }
-        if (com_ref_traj_) {
-          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_com_,
-                                       com_ref_traj_->value(t),
-                                       com_pos_vars(knot_point));
-        }
-        if (mom_ref_traj_) {
-          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_mom_,
-                                       mom_ref_traj_->value(t),
-                                       momentum_vars(knot_point));
-        }
-        if (contact_ref_traj_) {
-          prog_->AddQuadraticErrorCost(
-              collocation_gain * terminal_gain * Q_contact_,
-              contact_ref_traj_->value(t),
-              {contact_pos_[knot_point], contact_vel_[knot_point]});
-        }
-        if (force_ref_traj_) {
-          prog_->AddQuadraticErrorCost(collocation_gain * terminal_gain * Q_force_,
-                                       force_ref_traj_->value(t),
-                                       contact_force_[knot_point]);
-        }
-        break;
-      case PLANAR_SLIP:
-        AddPlanarSlipCost(knot_point, terminal_gain * collocation_gain);
-        break;
-    }
-  }
-}
-
-void KinematicCentroidalMPC::SetZeroInitialGuess() {
-  Eigen::VectorXd initialGuess =
-      Eigen::VectorXd::Zero(n_q_ + n_v_ + n_contact_points_ * 9 + 6 + 3);
-  prog_->SetInitialGuessForAllVariables(
-      initialGuess.replicate(n_knot_points_, 1));
-  // Make sure unit quaternions
-  for (int i = 0; i < n_knot_points_; i++) {
-    auto xi = state_vars(i);
-    prog_->SetInitialGuess(xi(0), 1);
-    prog_->SetInitialGuess(xi(1), 0);
-    prog_->SetInitialGuess(xi(2), 0);
-    prog_->SetInitialGuess(xi(3), 0);
-  }
-}
-
-drake::trajectories::PiecewisePolynomial<double>
-KinematicCentroidalMPC::Solve() {
-  for (int i = 0; i < 1; i++) {
-    auto start = std::chrono::high_resolution_clock::now();
-    solver_->Solve(*prog_, prog_->initial_guess(), prog_->solver_options(),
-                   result_.get());
-    auto finish = std::chrono::high_resolution_clock::now();
-    std::chrono::duration<double> elapsed = finish - start;
-    std::cout << "Solve time:" << elapsed.count() << std::endl;
-    std::cout << "Cost:" << result_->get_optimal_cost() << std::endl;
-    prog_->SetInitialGuessForAllVariables(result_->GetSolution());
-  }
-
-  std::vector<double> time_points;
-  std::vector<drake::MatrixX<double>> states;
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    time_points.emplace_back(dt_ * knot_point);
-    switch (complexity_schedule_[knot_point]) {
-      case KINEMATIC_CENTROIDAL:
-        states.emplace_back(result_->GetSolution(state_vars(knot_point)));
-        break;
-      case PLANAR_SLIP:
-        states.emplace_back(LiftSlipSolution(knot_point));
-        break;
-    }
-  }
-  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(
-      time_points, states);
-}
-
-void KinematicCentroidalMPC::SerializeSolution(int n_knot_points) {
-  DRAKE_DEMAND(result_ != nullptr);
-
-  Eigen::MatrixXd state_points;
-  Eigen::MatrixXd contact_force_points;
-  std::vector<double> time_samples;
-  this->SetFromSolution(*result_, &state_points, &contact_force_points,
-                        &time_samples);
-
-  int knot_points_to_serialize =
-      n_knot_points == -1 ? time_samples.size() : n_knot_points;
-
-  dairlib::LcmTrajectory::Trajectory state_traj;
-  dairlib::LcmTrajectory::Trajectory contact_force_traj;
-  state_traj.traj_name = "state_traj";
-  state_traj.datapoints = state_points.leftCols(knot_points_to_serialize);
-  state_traj.time_vector =
-      Eigen::Map<VectorXd>(time_samples.data(), knot_points_to_serialize);
-  state_traj.datatypes =
-      dairlib::multibody::CreateStateNameVectorFromMap(plant_);
-
-  contact_force_traj.traj_name = "contact_force_traj";
-  contact_force_traj.datapoints =
-      contact_force_points.leftCols(knot_points_to_serialize);
-  contact_force_traj.time_vector =
-      //      Eigen::Map<VectorXd>(time_samples.data(), time_samples.size());
-      Eigen::Map<VectorXd>(time_samples.data(), knot_points_to_serialize);
-  contact_force_traj.datatypes =
-      std::vector<std::string>(contact_force_traj.datapoints.rows());
-
-  std::vector<dairlib::LcmTrajectory::Trajectory> trajectories;
-  trajectories.push_back(state_traj);
-  trajectories.push_back(contact_force_traj);
-  std::vector<std::string> trajectory_names = {state_traj.traj_name,
-                                               contact_force_traj.traj_name};
-  lcm_trajectory_ =
-      LcmTrajectory(trajectories, trajectory_names, "centroidal_mpc_solution",
-                    "centroidal_mpc_solution");
-}
-
-void KinematicCentroidalMPC::PublishSolution(const std::string& lcm_channel,
-                                             int n_knot_points) {
-  SerializeSolution(n_knot_points);
-  if (!lcm_) {
-    lcm_ = std::make_unique<drake::lcm::DrakeLcm>();
-  }
-  auto lcm_msg = lcm_trajectory_.GenerateLcmObject();
-  drake::lcm::Publish(lcm_.get(), lcm_channel, lcm_msg);
-}
-
-bool KinematicCentroidalMPC::SaveSolutionToFile(const std::string& filepath) {
-  SerializeSolution(-1);
-  lcm_trajectory_.WriteToFile(filepath);
-  return true;
-}
-
-void KinematicCentroidalMPC::SetFromSolution(
-    const drake::solvers::MathematicalProgramResult& result,
-    Eigen::MatrixXd* state_samples, Eigen::MatrixXd* contact_force_samples,
-    std::vector<double>* time_samples) const {
-  DRAKE_ASSERT(state_samples != nullptr);
-  DRAKE_ASSERT(contact_force_samples != nullptr);
-  DRAKE_ASSERT(time_samples->empty());
-
-  *state_samples = MatrixXd(n_q_ + n_v_, n_knot_points_);
-  *contact_force_samples = MatrixXd(3 * n_contact_points_, n_knot_points_);
-  time_samples->resize(n_knot_points_);
-
-  for (int knot_point = 0; knot_point < num_knot_points(); knot_point++) {
-    time_samples->at(knot_point) = knot_point * dt_;
-
-    VectorXd x = result.GetSolution(state_vars(knot_point));
-    VectorXd contact_force = result.GetSolution(contact_force_.at(knot_point));
-    state_samples->col(knot_point) = x;
-    contact_force_samples->col(knot_point) = contact_force;
-  }
-}
-
-void KinematicCentroidalMPC::CreateVisualizationCallback(
-    std::string model_file, double alpha, std::string weld_frame_to_world) {
-  DRAKE_DEMAND(!callback_visualizer_);  // Cannot be set twice
-
-  // Assemble variable list
-  drake::solvers::VectorXDecisionVariable vars(n_knot_points_ / 2 * n_q_);
-  for (int knot_point = 0; knot_point < n_knot_points_ / 2; knot_point++) {
-    vars.segment(knot_point * n_q_, n_q_) =
-        state_vars(knot_point * 2).head(n_q_);
-  }
-  Eigen::VectorXd alpha_vec =
-      Eigen::VectorXd::Constant(n_knot_points_ / 2, alpha);
-  alpha_vec(0) = 1;
-  alpha_vec(n_knot_points_ / 2 - 1) = 1;
-
-  // Create visualizer
-  callback_visualizer_ =
-      std::make_unique<dairlib::multibody::MultiposeVisualizer>(
-          model_file, n_knot_points_ / 2, alpha_vec, weld_frame_to_world);
-
-  // Callback lambda function
-  auto my_callback = [this](const Eigen::Ref<const Eigen::VectorXd>& vars) {
-    Eigen::VectorXd vars_copy = vars;
-    Eigen::Map<Eigen::MatrixXd> states(vars_copy.data(), this->n_q_,
-                                       this->n_knot_points_ / 2);
-    this->callback_visualizer_->DrawPoses(states);
-  };
-
-  prog_->AddVisualizationCallback(my_callback, vars);
-}
-void KinematicCentroidalMPC::AddContactPointPositionConstraint(
-    int contact_index, const Eigen::Vector3d& lb, const Eigen::Vector3d& ub) {
-  //{TODO} eventually make in body frame
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->AddBoundingBoxConstraint(
-        lb, ub, contact_pos_vars(knot_point, contact_index));
-  }
-}
-void KinematicCentroidalMPC::AddPlantJointLimits(
-    const std::vector<std::string>& joint_to_constrain) {
-  std::map<std::string, int> positions_map =
-      dairlib::multibody::MakeNameToPositionsMap(plant_);
-  for (const auto& member : joint_to_constrain) {
-    for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-      prog_->AddBoundingBoxConstraint(
-          plant_.GetJointByName(member).position_lower_limits()(0),
-          plant_.GetJointByName(member).position_upper_limits()(0),
-          state_vars(knot_point)[positions_map.at(member)]);
-    }
-  }
-}
-
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::com_pos_vars(
-    int knotpoint_index) const {
-  return com_vars_[knotpoint_index];
-}
-drake::solvers::VectorXDecisionVariable
-KinematicCentroidalMPC::contact_pos_vars(int knotpoint_index,
-                                         int contact_index) const {
-  return contact_pos_[knotpoint_index].segment(contact_index * 3, 3);
-}
-drake::solvers::VectorXDecisionVariable
-KinematicCentroidalMPC::contact_vel_vars(int knotpoint_index,
-                                         int contact_index) const {
-  return contact_vel_[knotpoint_index].segment(contact_index * 3, 3);
-}
-drake::solvers::VectorXDecisionVariable
-KinematicCentroidalMPC::contact_force_vars(int knotpoint_index,
-                                           int contact_index) const {
-  return contact_force_[knotpoint_index].segment(contact_index * 3, 3);
-}
-void KinematicCentroidalMPC::AddKinematicConstraint(
-    std::shared_ptr<dairlib::multibody::KinematicPositionConstraint<double>>
-        con,
-    const Eigen::Ref<const drake::solvers::VectorXDecisionVariable>& vars) {
-  prog_->AddConstraint(std::move(con), vars);
-}
-void KinematicCentroidalMPC::SetRobotStateGuess(
-    const drake::VectorX<double>& state) {
-  DRAKE_DEMAND(state.size() == n_q_ + n_v_);
-
-  for (const auto& state_vars : x_vars_) {
-    prog_->SetInitialGuess(state_vars, state);
-  }
-}
-
-void KinematicCentroidalMPC::SetRobotStateGuess(
-    const drake::trajectories::PiecewisePolynomial<double>& state_trajectory) {
-  DRAKE_DEMAND(state_trajectory.rows() == n_q_ + n_v_);
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->SetInitialGuess(state_vars(knot_point),
-                           state_trajectory.value(dt_ * knot_point));
-  }
-}
-
-void KinematicCentroidalMPC::SetRobotStateGuess(
-    const drake::trajectories::PiecewisePolynomial<double>& q_traj,
-    const drake::trajectories::PiecewisePolynomial<double>& v_traj) {
-  DRAKE_DEMAND(q_traj.rows() == n_q_);
-  DRAKE_DEMAND(v_traj.rows() == n_v_);
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->SetInitialGuess(state_vars(knot_point).head(n_q_),
-                           q_traj.value(dt_ * knot_point));
-    prog_->SetInitialGuess(state_vars(knot_point).tail(n_v_),
-                           v_traj.value(dt_ * knot_point));
-  }
-}
-
-drake::solvers::VectorXDecisionVariable KinematicCentroidalMPC::momentum_vars(
-    int knotpoint_index) const {
-  return mom_vars_[knotpoint_index];
-}
-void KinematicCentroidalMPC::AddComHeightBoundingConstraint(double lb,
-                                                            double ub) {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->AddBoundingBoxConstraint(lb, ub, com_pos_vars(knot_point)[2]);
-  }
-}
-void KinematicCentroidalMPC::SetComPositionGuess(
-    const drake::Vector3<double>& state) {
-  for (const auto& com_pos : com_vars_) {
-    prog_->SetInitialGuess(com_pos, state);
-  }
-}
-
-void KinematicCentroidalMPC::SetComPositionGuess(
-    const drake::trajectories::PiecewisePolynomial<double>& com_trajectory) {
-  DRAKE_DEMAND(com_trajectory.rows() == 3);
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->SetInitialGuess(com_pos_vars(knot_point),
-                           com_trajectory.value(dt_ * knot_point));
-  }
-}
-
-void KinematicCentroidalMPC::SetContactGuess(
-    const drake::trajectories::PiecewisePolynomial<double>&
-        contact_trajectory) {
-  DRAKE_DEMAND(contact_trajectory.rows() == 6 * n_contact_points_);
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->SetInitialGuess(
-        contact_pos_[knot_point],
-        drake::VectorX<double>(contact_trajectory.value(dt_ * knot_point))
-            .head(3 * n_contact_points_));
-    prog_->SetInitialGuess(
-        contact_vel_[knot_point],
-        drake::VectorX<double>(contact_trajectory.value(dt_ * knot_point))
-            .tail(3 * n_contact_points_));
-  }
-}
-void KinematicCentroidalMPC::SetForceGuess(
-    const drake::trajectories::PiecewisePolynomial<double>& force_trajectory) {
-  DRAKE_DEMAND(force_trajectory.rows() == 3 * n_contact_points_);
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->SetInitialGuess(contact_force_[knot_point],
-                           force_trajectory.value(dt_ * knot_point));
-  }
-}
-
-void KinematicCentroidalMPC::SetMomentumGuess(
-    const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory) {
-  DRAKE_DEMAND(momentum_trajectory.rows() == 6);
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->SetInitialGuess(mom_vars_[knot_point],
-                           momentum_trajectory.value(dt_ * knot_point));
-  }
-}
-
-void KinematicCentroidalMPC::SetModeSequence(
-    const std::vector<std::vector<bool>>& contact_sequence) {
-  contact_sequence_ = contact_sequence;
-}
-void KinematicCentroidalMPC::SetModeSequence(
-    const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) {
-  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
-      contact_sequence_[knot_point][contact_index] =
-          contact_sequence.value(dt_ * knot_point).coeff(contact_index);
-    }
-  }
-}
-
-void KinematicCentroidalMPC::AddInitialStateConstraint(
-    const Eigen::VectorXd& state) {
-  DRAKE_DEMAND(state.size() == state_vars(0).size());
-  prog_->AddBoundingBoxConstraint(state, state, state_vars((0)));
-}
-void KinematicCentroidalMPC::SetGains(const KinematicCentroidalGains& gains) {
-  gains_ = gains;
-  std::map<std::string, int> positions_map =
-      dairlib::multibody::MakeNameToPositionsMap(plant_);
-  std::map<std::string, int> velocities_map =
-      dairlib::multibody::MakeNameToVelocitiesMap(plant_);
-
-  // Generalize state
-  for (const auto& [name, index] : positions_map) {
-    const auto it = gains.generalized_positions.find(name);
-    if (it != gains.generalized_positions.end()) {
-      Q_q_(index, index) = it->second;
-    } else if (name.find("left") != std::string::npos) {
-      const auto it_left =
-          gains.generalized_positions.find(name.substr(0, name.size() - 4));
-      if (it_left != gains.generalized_positions.end()) {
-        Q_q_(index, index) = it_left->second;
-      }
-    } else if (name.find("right") != std::string::npos) {
-      const auto it_right =
-          gains.generalized_positions.find(name.substr(0, name.size() - 5));
-      if (it_right != gains.generalized_positions.end()) {
-        Q_q_(index, index) = it_right->second;
-      }
-    }
-  }
-
-  for (const auto& [name, index] : velocities_map) {
-    const auto it = gains.generalized_velocities.find(name);
-    if (it != gains.generalized_velocities.end()) {
-      Q_v_(index, index) = it->second;
-    } else if (name.find("left") != std::string::npos) {
-      const auto it_left =
-          gains.generalized_velocities.find(name.substr(0, name.size() - 7));
-      if (it_left != gains.generalized_velocities.end()) {
-        Q_v_(index, index) = it_left->second;
-      }
-    } else if (name.find("right") != std::string::npos) {
-      const auto it_right =
-          gains.generalized_velocities.find(name.substr(0, name.size() - 8));
-      if (it_right != gains.generalized_velocities.end()) {
-        Q_v_(index, index) = it_right->second;
-      }
-    }
-  }
-
-  // com
-  Q_com_ = gains.com_position.asDiagonal();
-
-  // momentum
-  Q_mom_.block<3, 3>(0, 0, 3, 3) = gains.ang_momentum.asDiagonal();
-  Q_mom_.block<3, 3>(3, 3, 3, 3) = gains.lin_momentum.asDiagonal();
-
-  // contact pos, contact vel
-  Q_contact_.block<Eigen::Dynamic, Eigen::Dynamic>(0, 0, 3 * n_contact_points_,
-                                                   3 * n_contact_points_) =
-      gains.contact_pos.replicate(n_contact_points_, 1).asDiagonal();
-  Q_contact_.block<Eigen::Dynamic, Eigen::Dynamic>(
-      3 * n_contact_points_, 3 * n_contact_points_, 3 * n_contact_points_,
-      3 * n_contact_points_) =
-      gains.contact_vel.replicate(n_contact_points_, 1).asDiagonal();
-
-  // contact force
-  Q_force_ = gains.contact_force.replicate(n_contact_points_, 1).asDiagonal();
-
-  swing_foot_minimum_height_ = gains.swing_foot_minimum_height;
-}
+          reference_generator_->grf_traj_));
+  solver_->SetGenPosReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->q_trajectory_));
+  solver_->SetGenVelReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->v_trajectory_));
+  solver_->SetComReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->com_trajectory_));
+  solver_->SetContactTrackingReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->contact_traj_));
+  solver_->SetConstantMomentumReference(Eigen::VectorXd::Zero(6));
+  solver_->SetModeSequence(reference_generator_->contact_sequence_);
+
+  solver_->AddInitialStateConstraint(reference_state_);
+  solver_->SetRobotStateGuess(reference_generator_->q_trajectory_,
+                              reference_generator_->v_trajectory_);
+  solver_->SetComPositionGuess(reference_generator_->com_trajectory_);
+  solver_->SetContactGuess(reference_generator_->contact_traj_);
+  solver_->SetForceGuess(reference_generator_->grf_traj_);
+
+  {
+    drake::solvers::SolverOptions options;
+    auto id = drake::solvers::IpoptSolver::id();
+    options.SetOption(id, "tol", ipopt_tol_);
+    options.SetOption(id, "dual_inf_tol", ipopt_tol_);
+    options.SetOption(id, "constr_viol_tol", ipopt_tol_);
+    options.SetOption(id, "compl_inf_tol", ipopt_tol_);
+    options.SetOption(id, "max_iter", 200);
+    options.SetOption(id, "nlp_lower_bound_inf", -1e6);
+    options.SetOption(id, "nlp_upper_bound_inf", 1e6);
+    options.SetOption(id, "print_timing_statistics", "yes");
+    //    options.SetOption(id, "print_level", 5);
+
+    // Set to ignore overall tolerance/dual infeasibility, but terminate when
+    // primal feasible and objective fails to increase over 5 iterations.
+    options.SetOption(id, "acceptable_compl_inf_tol", ipopt_tol_);
+    options.SetOption(id, "acceptable_constr_viol_tol", ipopt_tol_);
+    options.SetOption(id, "acceptable_obj_change_tol", 1e-3);
+    options.SetOption(id, "acceptable_tol", 1e2);
+    options.SetOption(id, "acceptable_iter", 1);
+
+    options.SetOption(id, "warm_start_init_point", "no");
+    solver_->Build(options);
+  }
+
+  // Input/Output Setup
+  state_port_ =
+      this->DeclareVectorInputPort(
+              "x, u, t", OutputVector<double>(plant_w_spr_.num_positions(),
+                                              plant_w_spr_.num_velocities(),
+                                              plant_w_spr_.num_actuators()))
+          .get_index();
+  clock_port_ = this->DeclareVectorInputPort("t_clock", BasicVector<double>(1))
+                    .get_index();
+  target_vel_port_ = this->DeclareVectorInputPort(
+                             "v_des", BasicVector<double>(VectorXd::Zero(2)))
+                         .get_index();
+
+  DeclarePerStepDiscreteUpdateEvent(
+      &KinematicCentroidalMPC::DiscreteVariableUpdate);
+
+  this->DeclareAbstractOutputPort("lcmt_target_trajectory",
+                                  &KinematicCentroidalMPC::CalcTraj);
+}
+
+EventStatus KinematicCentroidalMPC::DiscreteVariableUpdate(
+    const Context<double>& context,
+    DiscreteValues<double>* discrete_state) const {
+  return EventStatus::Succeeded();
+}
+
+void KinematicCentroidalMPC::CalcTraj(
+    const drake::systems::Context<double>& context,
+    dairlib::lcmt_timestamped_saved_traj* traj) const {
+  const auto robot_output =
+      this->template EvalVectorInput<OutputVector>(context, state_port_);
+  const auto desired_pelvis_vel_xy =
+      this->EvalVectorInput(context, target_vel_port_)->get_value();
+
+  VectorXd q_w_spr = robot_output->GetPositions();
+  VectorXd v_w_spr = robot_output->GetVelocities();
+
+  VectorXd x_wo_spr(n_q_ + n_v_);
+  x_wo_spr << map_position_from_spring_to_no_spring_ * q_w_spr,
+      map_velocity_from_spring_to_no_spring_ * v_w_spr;
+
+  std::vector<Eigen::Vector3d> com_vel = {
+      {{0, 0, 0},
+       {desired_pelvis_vel_xy[0], -desired_pelvis_vel_xy[1], 0},
+       {0, 0, 0}}};
+
+  reference_generator_->SetNominalReferenceConfiguration(
+      reference_state_.head(plant_wo_spr_.num_positions()));
+  reference_generator_->SetGaitSequence(
+      {CopyVectorXdToStdVector(time_points_), gait_samples_});
+  reference_generator_->SetComKnotPoints(
+      {CopyVectorXdToStdVector(time_points_), com_vel});
+  reference_generator_->Generate();
+
+  solver_->SetGenPosReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->q_trajectory_));
+  solver_->SetGenVelReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->v_trajectory_));
+  solver_->SetComReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->com_trajectory_));
+  solver_->SetConstantMomentumReference(Eigen::VectorXd::Zero(6));
+  solver_->SetContactTrackingReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->contact_traj_));
+  solver_->SetForceTrackingReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          reference_generator_->grf_traj_));
+  solver_->SetModeSequence(reference_generator_->contact_sequence_);
+
+  solver_->UpdateInitialStateConstraint(x_wo_spr);
+  solver_->SetRobotStateGuess(reference_generator_->q_trajectory_,
+                              reference_generator_->v_trajectory_);
+  solver_->SetComPositionGuess(reference_generator_->com_trajectory_);
+  solver_->SetContactGuess(reference_generator_->contact_traj_);
+  solver_->SetForceGuess(reference_generator_->grf_traj_);
+  solver_->UpdateCosts();
+
+  std::cout << "Solving optimization\n\n";
+  solver_->Solve();
+  // (TODO) yangwill: think carefully about this time offset. This is just a
+  // placeholder for now
+  traj->saved_traj =
+      solver_->GenerateLcmTraj(n_knot_points_, robot_output->get_timestamp());
+  traj->utime = robot_output->get_timestamp() * 1e6;
+}
+
+}  // namespace dairlib
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
index dd29beb342..304146080b 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.h
@@ -1,451 +1,76 @@
 #pragma once
-#include <drake/lcm/drake_lcm.h>
-#include <drake/solvers/ipopt_solver.h>
-#include <drake/solvers/mathematical_program.h>
 
-#include "lcm/lcm_trajectory.h"
-#include "multibody/kinematic/kinematic_constraints.h"
-#include "multibody/kinematic/kinematic_evaluator_set.h"
-#include "multibody/kinematic/world_point_evaluator.h"
-#include "multibody/multipose_visualizer.h"
-#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h"
-#include "systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h"
+#include <drake/multibody/plant/multibody_plant.h>
+
+#include "dairlib/lcmt_timestamped_saved_traj.hpp"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h"
+#include "examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h"
+#include "systems/framework/output_vector.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h"
 
 #include "drake/common/trajectories/piecewise_polynomial.h"
-#include "drake/multibody/plant/multibody_plant.h"
-#include "drake/systems/framework/context.h"
+#include "drake/systems/framework/leaf_system.h"
 
-enum Complexity{
-  KINEMATIC_CENTROIDAL,
-  PLANAR_SLIP
-};
+namespace dairlib {
 
-/*!
- * @brief Class for solving nonlinear kinematic centroidal mpc. Implementation
- * is based on Dai, Hongkai, Andres Valenzuela, and Russ Tedrake. “Whole-Body
- * Motion Planning with Centroidal Dynamics and Full Kinematics.” 2014 IEEE-RAS
- * International Conference on Humanoid Robots (November 2014).
- *
- * The optimization contains two coupled problems. The centroidal problem
- * optimizes over: Angular momentum Linear momentum Center of mass position
- *      Contact position
- *      Contact velocity
- *      Contact force
- *  While the kinematics problem optimzes over:
- *      Generalized positions (including floating base)
- *      Generalized velocities (including floating boase)
- *
- * A series of constraints couple the kinematic state to the centroidal state
- * ensuring the contact point's, com position, com velocity, body orientation,
- * body angular velocity all align up
- */
-class KinematicCentroidalMPC {
+class KinematicCentroidalMPC : public drake::systems::LeafSystem<double> {
  public:
-  /*!
-   * @brief Constructor for MPC which initializes decision variables
-   * @param plant robot model
-   * @param n_knot_points number of knot points
-   * @param dt time step
-   * @param contact_points vector of world point evaluators which describes the
-   * points on the robot which might make contact with the world. This is not
-   * the mode sequence
-   */
   KinematicCentroidalMPC(
-      const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points,
-      double dt,
-      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
-          contact_points);
-
-  /*!
-   * @brief Sets the cost for the mpc problem using a gains struct
-   * @param gains
-   */
-  void SetGains(const KinematicCentroidalGains& gains);
-
-  /*!
-   * @brief Adds a cost reference for the generalized position of the robot of
-   * the form (x - x_ref)^T Q (x - x_ref)
-   * @param ref_traj trajectory in time
-   */
-  void AddGenPosReference(
-      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
-
-  /*!
-   * @brief Adds a cost reference for the generalized velocity of the robot of
-   * the form (x - x_ref)^T Q (x - x_ref)
-   * @param ref_traj trajectory in time
-   */
-  void AddGenVelReference(
-      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
-
-  /*!
-   * @brief Adds a cost and reference for the center of mass position of the
-   * robot (x - x_ref)^T Q (x - x_ref)
-   * @param ref_traj trajectory in time
-   */
-  void AddComReference(
-      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
-
-  /*!
-   * @brief Adds a cost and reference for the contact position and velocity (x -
-   * x_ref)^T Q (x - x_ref)
-   * @param contact_ref_traj trajectory in time, order is all contact pos, all
-   * contact vel
-   */
-  void AddContactTrackingReference(
-      std::unique_ptr<drake::trajectories::Trajectory<double>>
-          contact_ref_traj);
-
-  /*!
-   * @brief Adds a cost and reference for the angular and linear momentum of the
-   * robot (x - x_ref)^T Q (x - x_ref)
-   * @param ref_traj
-   */
-  void AddMomentumReference(
-      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
-
-  /*!
-   * @brief Add a cost and reference for the contact forces (x - x_ref)^T Q (x -
-   * x_ref)
-   * @param force_ref_traj trajectory in time
-   */
-  void AddForceTrackingReference(
-      std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj);
-
-  void AddConstantMomentumReference(const drake::VectorX<double>& value);
-
-  /*!
-   * @brief accessor for robot state decision vars
-   * @param knotpoint_index
-   * @return
-   */
-  drake::solvers::VectorXDecisionVariable state_vars(int knotpoint_index) const;
-
-  /*!
-   * @brief accessor for joint position decision vars
-   * @param knotpoint_index
-   * @return
-   */
-  drake::solvers::VectorXDecisionVariable joint_pos_vars(
-      int knotpoint_index) const;
-
-  /*!
-   * @brief accessor for joint velocity decision vars
-   * @param knotpoint_index
-   * @return
-   */
-  drake::solvers::VectorXDecisionVariable joint_vel_vars(
-      int knotpoint_index) const;
-
-  /*!
-   * @brief accessor for center of mass position decision vars
-   * @param knotpoint_index
-   * @return
-   */
-  drake::solvers::VectorXDecisionVariable com_pos_vars(
-      int knotpoint_index) const;
-
-  /*!
-   * @brief accessor for center of momentum decision variables
-   * @param knotpoint_index
-   * @return
-   */
-  drake::solvers::VectorXDecisionVariable momentum_vars(
-      int knotpoint_index) const;
-
-  /*!
-   * @brief accessor for contact position decision variables
-   * @param knotpoint_index
-   * @param contact integer corresponding to the contact point based on order of
-   * contact points in constructor
-   * @return [x,y,z] vector of decision variables for contact 'contact''s
-   * position at knotpoint `knotpoint_index`
-   */
-  drake::solvers::VectorXDecisionVariable contact_pos_vars(
-      int knotpoint_index, int contact_index) const;
-
-  /*!
-   * @brief accessor for contact velocity decision variables
-   * @param knotpoint_index
-   * @param contact_index integer corresponding to the contact point based on
-   * order of contact points in constructor
-   * @return [x,y,z] vector of decision variables for contact 'contact''s
-   * velocity at knotpoint `knotpoint_index`
-   */
-  drake::solvers::VectorXDecisionVariable contact_vel_vars(
-      int knotpoint_index, int contact_index) const;
-
-  /*!
-   * @brief accessor for contact force decision variables
-   * @param knotpoint_index
-   * @param contact_index integer corresponding to the contact point based on
-   * order of contact points in constructor
-   * @return [x,y,z] vector of decision variables for contact 'contact''s force
-   * at knotpoint `knotpoint_index`
-   */
-  drake::solvers::VectorXDecisionVariable contact_force_vars(
-      int knotpoint_index, int contact_index) const;
-
-  /*!
-   * @brief Adds standard constraints to optimization problem and sets options
-   * @param solver_options
-   */
-  virtual void Build(const drake::solvers::SolverOptions& solver_options);
-
-  /*!
-   * @brief Solve the optimization problem and return a piecewise linear
-   * trajectory of state
-   * @return piecewise linear trajectory of state
-   */
-  drake::trajectories::PiecewisePolynomial<double> Solve();
-
-  /*!
-   *
-   * @brief Serializes the solution into a member variable in order to
-   *        be published via LCM or saved to a file
-   * @param n_knot_points lcm_channel for where to publish
-   * @throws exception Solve() has not been called.
-   */
-  void SerializeSolution(int n_knot_points);
-
-  /*!
-   *
-   * @brief Saves the solution at filepath
-   * @param lcm_channel lcm_channel for where to publish
-   * @throws exception Solve() has not been called.
-   */
-  void PublishSolution(const std::string& lcm_channel, int n_knot_points);
-
-  /*!
-   *
-   * @brief Saves the solution at filepath
-   * @param filepath relative filepath for where to save the solution
-   * @return whether saving was successful
-   * @throws exception Solve() has not been called.
-   */
-  bool SaveSolutionToFile(const std::string& filepath);
-
-  /*!
-   *@brief Populate the state and time vector with the solution from result
-   */
-  void SetFromSolution(const drake::solvers::MathematicalProgramResult& result,
-                       Eigen::MatrixXd* state_samples,
-                       Eigen::MatrixXd* contact_force_samples,
-                       std::vector<double>* time_samples) const;
-
-  /*!
-   *@brief Sets initial guess corresponding to zero for everything except
-   *quaternions
-   */
-  void SetZeroInitialGuess();
-
-  void SetRobotStateGuess(const drake::VectorX<double>& state);
-
-  void SetRobotStateGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& state_trajectory);
-
-  // TODO remove once drake has trajectory stacking
-  virtual void SetRobotStateGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& q_traj,
-      const drake::trajectories::PiecewisePolynomial<double>& v_traj);
-
-  void SetComPositionGuess(const drake::Vector3<double>& state);
-
-  virtual void SetComPositionGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& com_trajectory);
-
-  virtual void SetContactGuess(const drake::trajectories::PiecewisePolynomial<double>&
-                           contact_trajectory);
-
-  virtual void SetForceGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& force_trajectory);
-
-  virtual void SetMomentumGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory);
-
-    void CreateVisualizationCallback(std::string model_file, double alpha,
-                                   std::string weld_frame_to_world = "");
-
-  /*!
-   * @brief Add a bounding box constraint to a contact position for all knot
-   * points
-   * @param contact_index integer corresponding to the contact point based on
-   * order of contact points in constructor
-   * @param lb lower bound
-   * @param ub upper bound
-   */
-  void AddContactPointPositionConstraint(int contact_index,
-                                         const Eigen::Vector3d& lb,
-                                         const Eigen::Vector3d& ub);
-
-  /*!
-   * @brief Adds joint limits to the joint names included in joints_to_limits
-   * using values in the plant
-   * @param joints_to_limit vector of strings
-   */
-  void AddPlantJointLimits(const std::vector<std::string>& joints_to_limit);
-
-  /*!
-   * @brief Adds a kinematic position constraint to the optimization
-   * @param con a shared pointer to the constraint
-   * @param vars the decision variables for the constraint
-   */
-  void AddKinematicConstraint(
-      std::shared_ptr<dairlib::multibody::KinematicPositionConstraint<double>>
-          con,
-      const Eigen::Ref<const drake::solvers::VectorXDecisionVariable>& vars);
-
-  /*!
-   * @brief Adds a constraint on com height to all knot points
-   * @param lb
-   * @param ub
-   */
-  void AddComHeightBoundingConstraint(double lb, double ub);
-
-  int num_knot_points() const { return n_knot_points_; }
-
-  /*!
-   * @brief Set the mode sequence
-   * @param contact_sequence vector where
-   * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point`
-   * is `contact_index` active
-   */
-  virtual void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
-
-  /*!
-   * @brief Set the mode sequence via a trajectory. The value of the trajectory
-   * at each time, cast to a bool is if a contact point is active or not
-   * @param contact_sequence
-   */
-  virtual void SetModeSequence(
-      const drake::trajectories::PiecewisePolynomial<double>& contact_sequence);
-
-  void AddInitialStateConstraint(const Eigen::VectorXd& state);
-
-  const drake::multibody::MultibodyPlant<double>& Plant() { return plant_; };
-
-  void SetComplexitySchedule(const std::vector<Complexity>& complexity_schedule){complexity_schedule_ = complexity_schedule;};
- protected:
-  /*!
-   * @brief Adds dynamics for centroidal state
-   */
-  void AddCentroidalDynamics(int knot_point);
-
-  /*!
-   * @brief Enforces zero force for feet in flight
-   */
-  void AddFlightContactForceConstraints(int knot_point);
-
-  /*!
-   * @brief Enforce dynamics for kinematics and location of the contacts
-   */
-  void AddKinematicsIntegrator(int knot_point);
-
-  /*!
-   * @brief Feet that in stance are not moving and on the ground, feet in the
-   * air are above the ground
-   */
-  void AddContactConstraints(int knot_point);
-
-  /*!
-   * @brief Ensures that contact point for feet line up with kinematics, and
-   * centroidal state lines up with kinematic state
-   */
-  void AddCentroidalKinematicConsistency(int knot_point);
-
-  /*!
-   * @brief Ensures feet are not pulling on the ground
-   */
-  void AddFrictionConeConstraints(int knot_point);
-
-  //  void AddTorqueLimits();
-
-  virtual void AddPlanarSlipConstraints(int knot_point){ DRAKE_DEMAND(false);};
-
-  virtual void AddPlanarSlipCost(int knot_point, double terminal_gain){ DRAKE_DEMAND(false);};
-
-  virtual void AddSlipReductionConstraint(int knot_point){ DRAKE_DEMAND(false);};
-
-  virtual void AddSlipLiftingConstraint(int knot_point){ DRAKE_DEMAND(false);};
-
-  virtual void AddSlipDynamics(int knot_point){ DRAKE_DEMAND(false);};
-
-  virtual drake::VectorX<double> LiftSlipSolution(int knot_point){ DRAKE_DEMAND(false);};
-
-  /*!
-   * @brief Add costs from internally stored variables
-   */
-  void AddCosts();
-
-  bool is_first_knot(int knot_point_index) { return knot_point_index == 0; };
-  bool is_last_knot(int knot_point_index) {
-    return knot_point_index == n_knot_points_ - 1;
-  };
-
-  const drake::multibody::MultibodyPlant<double>& plant_;
-
-  int n_knot_points_;
-  double dt_;
-
-  std::vector<dairlib::multibody::WorldPointEvaluator<double>> contact_points_;
-  std::vector<dairlib::multibody::KinematicEvaluatorSet<double>> contact_sets_;
-
-  static const int kCentroidalPosDim = 7;
-  static const int kCentroidalVelDim = 6;
-  int n_q_;
-  int n_v_;
-  int n_joint_q_;
-  int n_joint_v_;
-  int n_contact_points_;
-
-  /// References and cost matrixes
-  std::unique_ptr<drake::trajectories::Trajectory<double>> q_ref_traj_;
-  Eigen::MatrixXd Q_q_;
-  std::unique_ptr<drake::trajectories::Trajectory<double>> v_ref_traj_;
-  Eigen::MatrixXd Q_v_;
-  std::unique_ptr<drake::trajectories::Trajectory<double>> com_ref_traj_;
-  Eigen::MatrixXd Q_com_;
-  std::unique_ptr<drake::trajectories::Trajectory<double>> mom_ref_traj_;
-  Eigen::MatrixXd Q_mom_;
-  std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj_;
-  Eigen::MatrixXd Q_contact_;
-  std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj_;
-  Eigen::MatrixXd Q_force_;
-
-  std::vector<std::vector<bool>> contact_sequence_;
-  // MathematicalProgram
-  std::unique_ptr<drake::solvers::MathematicalProgram> prog_;
-  std::unique_ptr<drake::solvers::IpoptSolver> solver_;
-  std::unique_ptr<drake::solvers::MathematicalProgramResult> result_;
-
-  std::vector<drake::solvers::Binding<drake::solvers::Constraint>>
-      centroidal_dynamics_binding_;
-
-  // DecisionVariables
-  // Full robot state
-  std::vector<drake::solvers::VectorXDecisionVariable> x_vars_;
-  // angular and linear momentum variables (in that order)
-  std::vector<drake::solvers::VectorXDecisionVariable> mom_vars_;
-  // center of mass position
-  std::vector<drake::solvers::VectorXDecisionVariable> com_vars_;
-
-  // Contact position, velocity, and force [n_knot_points, 3 * n_contact_points]
-  std::vector<drake::solvers::VectorXDecisionVariable> contact_pos_;
-  std::vector<drake::solvers::VectorXDecisionVariable> contact_vel_;
-  std::vector<drake::solvers::VectorXDecisionVariable> contact_force_;
-
-  std::vector<std::unique_ptr<drake::systems::Context<double>>> contexts_;
-
-  std::unique_ptr<dairlib::multibody::MultiposeVisualizer> callback_visualizer_;
-
-  const std::set<int> full_constraint_relative_ = {0, 1, 2};
-
-  double swing_foot_minimum_height_;
-
-  // saving and publishing solutions
-  std::unique_ptr<drake::lcm::DrakeLcm> lcm_;
-  dairlib::LcmTrajectory lcm_trajectory_;
-
-  std::vector<Complexity> complexity_schedule_;
-  KinematicCentroidalGains gains_;
+      const drake::multibody::MultibodyPlant<double>& plant_w_spr,
+      const drake::multibody::MultibodyPlant<double>& plant_wo_spr,
+      drake::systems::Context<double>* context, const TrajectoryParameters& motion,
+      const KinematicCentroidalGains& gains);
+
+  const drake::systems::InputPort<double>& get_state_input_port() const {
+    return this->get_input_port(state_port_);
+  }
+  const drake::systems::InputPort<double>& get_fsm_input_port() const {
+    return this->get_input_port(fsm_port_);
+  }
+  const drake::systems::InputPort<double>& get_input_port_clock() const {
+    return this->get_input_port(clock_port_);
+  }
+  const drake::systems::InputPort<double>& get_input_port_target_vel() const {
+    return this->get_input_port(target_vel_port_);
+  }
+  void SetSpringMaps(Eigen::MatrixXd& pos_map, Eigen::MatrixXd& vel_map) {
+    map_position_from_spring_to_no_spring_ = pos_map;
+    map_velocity_from_spring_to_no_spring_ = vel_map;
+  }
+
+ private:
+  drake::systems::EventStatus DiscreteVariableUpdate(
+      const drake::systems::Context<double>& context,
+      drake::systems::DiscreteValues<double>* discrete_state) const;
+
+  void CalcTraj(const drake::systems::Context<double>& context,
+                dairlib::lcmt_timestamped_saved_traj* traj) const;
+
+  const drake::multibody::MultibodyPlant<double>& plant_w_spr_;
+  const drake::multibody::MultibodyPlant<double>& plant_wo_spr_;
+  drake::systems::Context<double>* context_wo_spr_;
+  const drake::multibody::BodyFrame<double>& world_;
+
+  drake::systems::InputPortIndex state_port_;
+  drake::systems::InputPortIndex fsm_port_;
+  drake::systems::InputPortIndex clock_port_;
+  drake::systems::InputPortIndex target_vel_port_;
+
+  std::unique_ptr<CassieKinematicCentroidalSolver> solver_;
+  std::unique_ptr<KcmpcReferenceGenerator> reference_generator_;
+
+  Eigen::MatrixXd map_position_from_spring_to_no_spring_;
+  Eigen::MatrixXd map_velocity_from_spring_to_no_spring_;
+
+  Eigen::VectorXd reference_state_;
+  const int n_q_;
+  const int n_v_;
+  std::unordered_map<std::string, Gait> gait_library_;
+  std::vector<Gait> gait_samples_;
+  Eigen::VectorXd time_points_;
+  int n_knot_points_ = 10;
+  double ipopt_tol_ = 1e-2;
 };
+
+}  // namespace dairlib
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.cc
new file mode 100644
index 0000000000..e8b4ffdc7a
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.cc
@@ -0,0 +1,64 @@
+#include "kinematic_centroidal_visualizer.h"
+
+#include <iostream>
+#include <utility>
+
+#include <dairlib/lcmt_saved_traj.hpp>
+#include <dairlib/lcmt_timestamped_saved_traj.hpp>
+
+#include "lcm/lcm_trajectory.h"
+#include "multibody/multipose_visualizer.h"
+
+#include "drake/common/trajectories/piecewise_polynomial.h"
+#include "drake/systems/framework/leaf_system.h"
+
+using dairlib::systems::BasicVector;
+using dairlib::systems::OutputVector;
+using drake::systems::Context;
+using drake::systems::DiscreteValues;
+using drake::systems::EventStatus;
+using drake::trajectories::Trajectory;
+using Eigen::MatrixXd;
+
+namespace dairlib {
+
+KinematicCentroidalVisualizer::KinematicCentroidalVisualizer(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    drake::systems::Context<double>* context,
+    std::unique_ptr<multibody::MultiposeVisualizer> visualizer)
+    : plant_(plant), context_(context), visualizer_(std::move(visualizer)) {
+  this->set_name("kinematic_centroidal_visualizer");
+
+  // Input/Output Setup
+  trajectory_port_ =
+      this->DeclareAbstractInputPort(
+              "kcmpc_trajectory",
+              drake::Value<dairlib::lcmt_timestamped_saved_traj>{})
+          .get_index();
+
+  DeclarePerStepDiscreteUpdateEvent(
+      &KinematicCentroidalVisualizer::DrawTrajectory);
+}
+
+EventStatus KinematicCentroidalVisualizer::DrawTrajectory(
+    const Context<double>& context,
+    DiscreteValues<double>* discrete_state) const {
+  const auto* timestamped_lcm_traj =
+      this->EvalInputValue<dairlib::lcmt_timestamped_saved_traj>(
+          context, trajectory_port_);
+  MatrixXd poses =
+      MatrixXd::Zero(visualizer_->num_positions(), visualizer_->num_poses());
+  std::cout << "drawing poses: " << std::endl;
+  LcmTrajectory trajectory = LcmTrajectory(timestamped_lcm_traj->saved_traj);
+  auto state_traj = trajectory.GetTrajectory("state_traj");
+  for (int i = 0; i < poses.cols(); ++i) {
+    int col_index = (int)(state_traj.datapoints.cols() * i / poses.cols());
+    poses.col(i) =
+        state_traj.datapoints.col(col_index).head(visualizer_->num_positions());
+  }
+
+  visualizer_->DrawPoses(poses);
+  return EventStatus::Succeeded();
+}
+
+}  // namespace dairlib
\ No newline at end of file
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.h b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.h
new file mode 100644
index 0000000000..f81896670f
--- /dev/null
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_visualizer.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <drake/multibody/plant/multibody_plant.h>
+
+#include "multibody/multipose_visualizer.h"
+#include "systems/framework/output_vector.h"
+
+#include "drake/common/trajectories/piecewise_polynomial.h"
+#include "drake/systems/framework/leaf_system.h"
+
+using drake::systems::Context;
+using drake::systems::DiscreteValues;
+
+namespace dairlib {
+
+class KinematicCentroidalVisualizer
+    : public drake::systems::LeafSystem<double> {
+ public:
+  KinematicCentroidalVisualizer(
+      const drake::multibody::MultibodyPlant<double>& plant,
+      drake::systems::Context<double>* context,
+      std::unique_ptr<multibody::MultiposeVisualizer> visualizer);
+
+  const drake::systems::InputPort<double>& get_input_port_trajectory() const {
+    return this->get_input_port(trajectory_port_);
+  }
+
+ private:
+  drake::systems::EventStatus DrawTrajectory(
+      const Context<double>& context,
+      DiscreteValues<double>* discrete_state) const;
+
+  const drake::multibody::MultibodyPlant<double>& plant_;
+  drake::systems::Context<double>* context_;
+
+  std::unique_ptr<multibody::MultiposeVisualizer> visualizer_;
+  drake::systems::InputPortIndex trajectory_port_;
+};
+
+}  // namespace dairlib
diff --git a/systems/trajectory_optimization/BUILD.bazel b/systems/trajectory_optimization/BUILD.bazel
index 5ea479bc9c..3ea2b8d1b1 100644
--- a/systems/trajectory_optimization/BUILD.bazel
+++ b/systems/trajectory_optimization/BUILD.bazel
@@ -55,6 +55,7 @@ cc_library(
     ],
     deps = [
         "//lcm:lcm_trajectory_saver",
+        "//common:find_resource",
         "@drake//:drake_shared_library",
     ],
 )
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/BUILD.bazel b/systems/trajectory_optimization/kinematic_centroidal_planner/BUILD.bazel
new file mode 100644
index 0000000000..f60a25ae54
--- /dev/null
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/BUILD.bazel
@@ -0,0 +1,52 @@
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "kinematic_centroidal_planner",
+    srcs = [],
+    deps = [
+        ":kinematic_centroidal_solver",
+        ":kcmpc_gains",
+        ":kinematic_centroidal_reference_generator",
+    ],
+)
+
+cc_library(
+    name = "kinematic_centroidal_solver",
+    srcs = ["kinematic_centroidal_solver.cc",
+            "kinematic_centroidal_constraints.cc"],
+    hdrs = ["kinematic_centroidal_solver.h",
+            "kinematic_centroidal_constraints.h"],
+    deps = [
+        "//solvers:constraints",
+        "//multibody/kinematic:kinematic",
+        "//multibody/kinematic:constraints",
+        "//multibody:multipose_visualizer",
+        ":kcmpc_gains",
+        "//lcm:lcm_trajectory_saver",
+        "@drake//:drake_shared_library",
+    ],
+)
+
+cc_library(
+    name = "kcmpc_gains",
+    hdrs = ["kinematic_centroidal_gains.h"],
+    deps = [
+        "@drake//:drake_shared_library",
+    ],
+)
+
+cc_library(
+    name = "kinematic_centroidal_reference_generator",
+    srcs = ["kcmpc_reference_generator.cc",
+            "reference_generation_utils.cc",
+            "gait.cc"],
+    hdrs = ["kcmpc_reference_generator.h",
+            "reference_generation_utils.h",
+            "gait.h"],
+    deps = [
+        "//common",
+        "//examples/Cassie:cassie_utils",
+        "//multibody/kinematic:kinematic",
+        "@drake//:drake_shared_library",
+    ],
+)
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/gait.cc
similarity index 92%
rename from systems/controllers/kinematic_centroidal_mpc/gait.cc
rename to systems/trajectory_optimization/kinematic_centroidal_planner/gait.cc
index 906c0c560e..3553410d51 100644
--- a/systems/controllers/kinematic_centroidal_mpc/gait.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/gait.cc
@@ -1,4 +1,4 @@
-#include "gait.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h"
 
 #include <iostream>
 
@@ -39,6 +39,7 @@ drake::trajectories::PiecewisePolynomial<double> Gait::ToTrajectory(
     current_phase = gait_pattern[current_mode].start_phase;
   }
   break_points.push_back(end_time);
+  // Contact doesn't change so just repeat it for the last break_point
   samples.push_back(samples[samples.size() - 1]);
   return drake::trajectories::PiecewisePolynomial<double>::ZeroOrderHold(
       break_points, samples);
diff --git a/systems/controllers/kinematic_centroidal_mpc/gait.h b/systems/trajectory_optimization/kinematic_centroidal_planner/gait.h
similarity index 64%
rename from systems/controllers/kinematic_centroidal_mpc/gait.h
rename to systems/trajectory_optimization/kinematic_centroidal_planner/gait.h
index a0dda58cea..08d3115d7c 100644
--- a/systems/controllers/kinematic_centroidal_mpc/gait.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/gait.h
@@ -1,16 +1,19 @@
 #pragma once
 
 #include <drake/common/trajectories/piecewise_polynomial.h>
-#include "drake/common/yaml/yaml_read_archive.h"
+
 #include "yaml-cpp/yaml.h"
 
+#include "drake/common/yaml/yaml_read_archive.h"
+
 /*!
  * @brief Struct for a given mode as part of a mode sequence
  */
-struct Mode{
+struct Mode {
   double start_phase;
   double end_phase;
-  drake::VectorX<bool> contact_status; /// Vector describing which contacts are active
+  /// Vector describing which contacts are active
+  drake::VectorX<bool> contact_status;
 
   template <typename Archive>
   void Serialize(Archive* a) {
@@ -21,16 +24,19 @@ struct Mode{
 };
 
 /*!
- * @brief Struct for defining a gait consisting of a vector of Modes, and a period for the whole gait.
+ * @brief Struct for defining a gait consisting of a vector of Modes, and a
+ * period for the whole gait.
  */
-struct Gait{
-/*!
- * @brief converts the gait into a trajectory from current time to end time
- * @param current_time
- * @param end_time
- * @return trajectory where value at time t converted to a bool is a vector of which feet are in stance
- */
-  drake::trajectories::PiecewisePolynomial<double> ToTrajectory(double current_time, double end_time) const;
+struct Gait {
+  /*!
+   * @brief converts the gait into a trajectory from current time to end time
+   * @param current_time
+   * @param end_time
+   * @return trajectory where value at time t converted to a bool is a vector of
+   * which feet are in stance
+   */
+  drake::trajectories::PiecewisePolynomial<double> ToTrajectory(
+      double current_time, double end_time) const;
 
   /*!
    * @brief Checks to make sure the gait is valid, Asserts if not valid
@@ -50,7 +56,8 @@ struct Gait{
   int GetCurrentMode(double time_now) const;
 
   double period;
-  std::vector<Mode> gait_pattern; /// Vector of modes. Start phase of the first mode must be 0, end phase of the last mode
+  /// Vector of modes. Start phase of the first mode must be
+  /// 0, end phase of the last mode
   /// must be 1, and no time gaps between start and end of sequential modes
-
+  std::vector<Mode> gait_pattern;
 };
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc
similarity index 58%
rename from systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
rename to systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc
index 38b8d28012..60e421e72f 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc
@@ -1,35 +1,28 @@
-#include "kcmpc_reference_generator.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h"
 
 #include "multibody/multibody_utils.h"
-#include "systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h"
 
 KcmpcReferenceGenerator::KcmpcReferenceGenerator(
     const drake::multibody::MultibodyPlant<double>& plant,
-    const Eigen::VectorXd& nominal_stand,
+    drake::systems::Context<double>* context,
     const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
         contacts)
-    : plant_(plant),
-      contacts_(contacts),
-      nominal_stand_(nominal_stand),
-      context_(plant.CreateDefaultContext()) {
-  dairlib::multibody::SetPositionsIfNew<double>(plant_, nominal_stand_,
-                                                context_.get());
-  p_ScmBase_W_ = nominal_stand_.segment(4, 3) -
-                 plant_.CalcCenterOfMassPositionInWorld(*context_);
+    : plant_(plant), context_(context), contacts_(contacts) {
   m_ = plant_.CalcTotalMass(*context_);
 }
 
-void KcmpcReferenceGenerator::Build() {
-  Build(nominal_stand_.segment(4, 3) - p_ScmBase_W_);
-}
-
-void KcmpcReferenceGenerator::Build(const Eigen::Vector3d& com) {
+void KcmpcReferenceGenerator::Generate() {
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, q_ref_, context_);
+  p_ScmBase_W_ =
+      q_ref_.segment(4, 3) - plant_.CalcCenterOfMassPositionInWorld(*context_);
+  Eigen::Vector3d com = q_ref_.segment(4, 3) - p_ScmBase_W_;
   com_trajectory_ = GenerateComTrajectory(com, com_vel_knot_points_.samples,
                                           com_vel_knot_points_.times);
   momentum_trajectory_ = GenerateMomentumTrajectory(com_vel_knot_points_.samples,
                                           com_vel_knot_points_.times,
                                           m_);
-  q_trajectory_ = GenerateGeneralizedPosTrajectory(nominal_stand_, p_ScmBase_W_,
+  q_trajectory_ = GenerateGeneralizedPosTrajectory(q_ref_, p_ScmBase_W_,
                                                    com_trajectory_, 4);
   v_trajectory_ = GenerateGeneralizedVelTrajectory(com_trajectory_,
                                                    plant_.num_velocities(), 3);
@@ -56,3 +49,19 @@ void KcmpcReferenceGenerator::SetGaitSequence(
   }
   gait_knot_points_ = gait_knot_points;
 }
+
+std::vector<double> KcmpcReferenceGenerator::GenerateTimePoints(
+    const std::vector<double>& duration_scaling,
+    std::vector<Gait> gait_sequence) {
+  std::vector<double> durations = std::vector<double>(gait_sequence.size());
+  for (int i = 0; i < gait_sequence.size(); ++i) {
+    durations[i] = duration_scaling[i] * gait_sequence[i].period;
+  }
+  std::vector<double> time_points;
+  double start_time = 0;
+  time_points.push_back(start_time);
+  for (auto duration : durations) {
+    time_points.push_back(time_points.back() + duration);
+  }
+  return time_points;
+}
diff --git a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h
similarity index 66%
rename from systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
rename to systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h
index 2473b84271..421a53247a 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kcmpc_reference_generator.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h
@@ -1,32 +1,35 @@
 #pragma once
 
-#include <drake/multibody/plant/multibody_plant.h>
 #include <drake/common/trajectories/piecewise_polynomial.h>
+#include <drake/multibody/plant/multibody_plant.h>
+
 #include "multibody/kinematic/world_point_evaluator.h"
-#include "systems/controllers/kinematic_centroidal_mpc/gait.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h"
 
 /*!
- * @brief struct consisting of a collection of times and samples used for passing inputs to reference generator
+ * @brief struct consisting of a collection of times and samples used for
+ * passing inputs to reference generator
  * @tparam T
  */
 template <typename T>
-struct KnotPoints{
+struct KnotPoints {
   std::vector<double> times;
   std::vector<T> samples;
 };
 
-class KcmpcReferenceGenerator{
+class KcmpcReferenceGenerator {
  public:
-
   /*!
    * @brief constructor for reference generator
    * @param plant
    * @param nominal_stand nominal stand of size [nq]
    * @param contacts Vector of world point evalator describing contacts
    */
-  KcmpcReferenceGenerator(const drake::multibody::MultibodyPlant<double>& plant,
-                          const Eigen::VectorXd& nominal_stand,
-                          const std::vector<dairlib::multibody::WorldPointEvaluator<double>>& contacts);
+  KcmpcReferenceGenerator(
+      const drake::multibody::MultibodyPlant<double>& plant,
+      drake::systems::Context<double>* context,
+      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+          contacts);
 
   /*!
    * @brief specify com velocity at specific times
@@ -41,15 +44,26 @@ class KcmpcReferenceGenerator{
   void SetGaitSequence(const KnotPoints<Gait>& gait_knot_points);
 
   /*!
-   * @brief construct references assuming initial com is the from the nominal stand
+   * @brief return gait sequence
+   * @param gait_knot_points
    */
-  void Build();
+  static std::vector<double> GenerateTimePoints(
+      const std::vector<double>& duration_scaling,
+      std::vector<Gait> gait_knot_points);
 
   /*!
-   * @brief Constructs references based on the time of the knot points
+   * @brief construct references assuming initial com is the from the nominal
+   * stand
+   */
+  void Generate();
+
+  /*!
+   * @brief Set the nominal reference configuration q_ref
    * @param com initial location of the com
    */
-  void Build(const Eigen::Vector3d& com);
+  void SetNominalReferenceConfiguration(const Eigen::VectorXd& q_ref) {
+    q_ref_ = q_ref;
+  }
 
   drake::trajectories::PiecewisePolynomial<double> com_trajectory_;
   drake::trajectories::PiecewisePolynomial<double> q_trajectory_;
@@ -59,15 +73,13 @@ class KcmpcReferenceGenerator{
   drake::trajectories::PiecewisePolynomial<double> contact_traj_;
   drake::trajectories::PiecewisePolynomial<double> momentum_trajectory_;
 
-
  private:
   const drake::multibody::MultibodyPlant<double>& plant_;
+  drake::systems::Context<double>* context_;
   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> contacts_;
-  Eigen::VectorXd nominal_stand_;
+  Eigen::VectorXd q_ref_;
   KnotPoints<Eigen::Vector3d> com_vel_knot_points_;
   KnotPoints<Gait> gait_knot_points_;
   double m_;
   Eigen::Vector3d p_ScmBase_W_;
-  std::unique_ptr<drake::systems::Context<double>> context_;
-
-};
\ No newline at end of file
+};
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.cc
similarity index 99%
rename from systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
rename to systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.cc
index 03d8f344b5..95111e7d81 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.cc
@@ -1,6 +1,6 @@
 #include <drake/math/quaternion.h>
 #include <iostream>
-#include "kinematic_centroidal_constraints.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.h"
 #include "multibody/multibody_utils.h"
 
 template<typename T>
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.h
similarity index 100%
rename from systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_constraints.h
rename to systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.h
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h
similarity index 86%
rename from systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
rename to systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h
index 97ba599ae3..1a3464209e 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_gains.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h
@@ -1,8 +1,9 @@
 #pragma once
-#include "drake/common/yaml/yaml_read_archive.h"
 #include "yaml-cpp/yaml.h"
 
-struct KinematicCentroidalGains{
+#include "drake/common/yaml/yaml_read_archive.h"
+
+struct KinematicCentroidalGains {
   Eigen::Vector3d com_position;
   std::unordered_map<std::string, double> generalized_positions;
   std::unordered_map<std::string, double> generalized_velocities;
@@ -12,7 +13,7 @@ struct KinematicCentroidalGains{
   Eigen::Vector3d lin_momentum;
   Eigen::Vector3d ang_momentum;
 
-  double swing_foot_minimum_height;
+  double tol;
   template <typename Archive>
   void Serialize(Archive* a) {
     a->Visit(DRAKE_NVP(com_position));
@@ -23,6 +24,6 @@ struct KinematicCentroidalGains{
     a->Visit(DRAKE_NVP(contact_force));
     a->Visit(DRAKE_NVP(lin_momentum));
     a->Visit(DRAKE_NVP(ang_momentum));
-    a->Visit(DRAKE_NVP(swing_foot_minimum_height));
+    a->Visit(DRAKE_NVP(tol));
   }
-};
\ No newline at end of file
+};
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
new file mode 100644
index 0000000000..70c73bc0ce
--- /dev/null
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -0,0 +1,729 @@
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h"
+
+#include <iostream>
+#include <utility>
+
+#include "lcm/lcm_trajectory.h"
+#include "multibody/kinematic/kinematic_constraints.h"
+#include "multibody/multibody_utils.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.h"
+
+#include "drake/common/trajectories/piecewise_polynomial.h"
+
+using dairlib::LcmTrajectory;
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+
+KinematicCentroidalSolver::KinematicCentroidalSolver(
+    const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points,
+    double dt,
+    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+        contact_points)
+    : plant_(plant),
+      n_knot_points_(n_knot_points),
+      dt_(dt),
+      contact_points_(contact_points),
+      n_q_(plant.num_positions()),
+      n_v_(plant.num_velocities()),
+      n_contact_points_(contact_points.size()),
+      Q_q_(Eigen::MatrixXd::Zero(n_q_, n_q_)),
+      Q_v_(Eigen::MatrixXd::Zero(n_v_, n_v_)),
+      Q_com_(Eigen::MatrixXd::Zero(3, 3)),
+      Q_mom_(Eigen::MatrixXd::Zero(6, 6)),
+      Q_contact_(
+          Eigen::MatrixXd::Zero(6 * n_contact_points_, 6 * n_contact_points_)),
+      Q_force_(
+          Eigen::MatrixXd::Zero(3 * n_contact_points_, 3 * n_contact_points_)),
+      contact_sequence_(n_knot_points),
+      contexts_(n_knot_points),
+      complexity_schedule_(n_knot_points){
+  n_joint_q_ = n_q_ - kCentroidalPosDim;
+  n_joint_v_ = n_v_ - kCentroidalVelDim;
+  prog_ = std::make_unique<drake::solvers::MathematicalProgram>();
+  solver_ = std::make_unique<drake::solvers::IpoptSolver>();
+  result_ = std::make_unique<drake::solvers::MathematicalProgramResult>();
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    contact_sets_.emplace_back(plant_);
+    contact_sets_[contact_index].add_evaluator(&contact_points_[contact_index]);
+  }
+
+  for (int knot = 0; knot < n_knot_points; knot++) {
+    contexts_[knot] = plant_.CreateDefaultContext();
+    x_vars_.push_back(prog_->NewContinuousVariables(
+        n_q_ + n_v_, "x_vars_" + std::to_string(knot)));
+    mom_vars_.push_back(
+        prog_->NewContinuousVariables(6, "mom_vars_" + std::to_string(knot)));
+    com_vars_.push_back(
+        prog_->NewContinuousVariables(3, "com_vars_" + std::to_string(knot)));
+    contact_pos_.push_back(prog_->NewContinuousVariables(
+        3 * n_contact_points_, "contact_pos_" + std::to_string(knot)));
+    contact_vel_.push_back(prog_->NewContinuousVariables(
+        3 * n_contact_points_, "contact_vel_" + std::to_string(knot)));
+    contact_force_.push_back(prog_->NewContinuousVariables(
+        3 * n_contact_points_, "contact_force_" + std::to_string(knot)));
+  }
+  std::vector<bool> stance_mode(n_contact_points_);
+  std::fill(stance_mode.begin(), stance_mode.end(), true);
+  std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
+  std::fill(complexity_schedule_.begin(), complexity_schedule_.end(), Complexity::KINEMATIC_CENTROIDAL);
+}
+
+void KinematicCentroidalSolver::SetGenPosReference(
+    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
+  q_ref_traj_ = std::move(ref_traj);
+}
+
+void KinematicCentroidalSolver::SetGenVelReference(
+    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
+  v_ref_traj_ = std::move(ref_traj);
+}
+
+void KinematicCentroidalSolver::SetComReference(
+    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
+  com_ref_traj_ = std::move(ref_traj);
+}
+
+void KinematicCentroidalSolver::SetContactTrackingReference(
+    std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj) {
+  contact_ref_traj_ = std::move(contact_ref_traj);
+}
+
+void KinematicCentroidalSolver::SetForceTrackingReference(
+    std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj) {
+  force_ref_traj_ = std::move(force_ref_traj);
+}
+
+void KinematicCentroidalSolver::SetMomentumReference(
+    std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj) {
+  mom_ref_traj_ = std::move(ref_traj);
+}
+
+void KinematicCentroidalSolver::AddCentroidalDynamics(int knot_point) {
+  if(!(is_last_knot(knot_point))){
+    auto constraint = std::make_shared<CentroidalDynamicsConstraint<double>>(
+        plant_, contexts_[knot_point].get(), n_contact_points_, dt_,
+        knot_point);
+    centroidal_dynamics_binding_.push_back(prog_->AddConstraint(
+        constraint,
+        {momentum_vars(knot_point), momentum_vars(knot_point + 1),
+         com_pos_vars(knot_point), contact_pos_[knot_point],
+         contact_force_[knot_point], com_pos_vars(knot_point + 1),
+         contact_pos_[knot_point + 1], contact_force_[knot_point + 1]}));
+  }
+}
+
+void KinematicCentroidalSolver::AddKinematicsIntegrator(int knot_point) {
+  if(!is_last_knot(knot_point)) {
+    // Integrate generalized velocities to get generalized positions
+    auto constraint = std::make_shared<KinematicIntegratorConstraint<double>>(
+        plant_, contexts_[knot_point].get(), contexts_[knot_point + 1].get(),
+        dt_, knot_point);
+    prog_->AddConstraint(constraint, {state_vars(knot_point).head(n_q_),
+                                      state_vars(knot_point + 1).head(n_q_),
+                                      state_vars(knot_point).tail(n_v_),
+                                      state_vars(knot_point + 1).tail(n_v_)});
+
+    // Integrate foot states
+    for (int contact_index = 0; contact_index < n_contact_points_;
+         contact_index++) {
+      prog_->AddConstraint(
+          contact_pos_vars(knot_point + 1, contact_index) ==
+          contact_pos_vars(knot_point, contact_index) +
+              0.5 * dt_ *
+                  (contact_vel_vars(knot_point, contact_index) +
+                   contact_vel_vars(knot_point + 1, contact_index)));
+    }
+  }
+}
+
+void KinematicCentroidalSolver::AddContactConstraints(int knot_point) {
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    // Make sure feet in stance are not moving and on the ground
+    if (contact_sequence_[knot_point][contact_index]) {
+      prog_->AddBoundingBoxConstraint(
+          Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
+          contact_vel_vars(knot_point, contact_index));
+      if (knot_point != 0) {
+        prog_->AddBoundingBoxConstraint(
+            0, 0, contact_pos_vars(knot_point, contact_index)[2]);
+      }
+    } else {
+      // Feet are above the ground
+      double lb = 0;
+      // Check if at least one of the time points before or after is also in
+      // 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
+          (!contact_sequence_[knot_point - 1][contact_index] or
+           !contact_sequence_[knot_point + 1][contact_index])) {
+        lb = swing_foot_minimum_height_;
+      }
+      prog_->AddBoundingBoxConstraint(
+          lb, 10, contact_pos_vars(knot_point, contact_index)[2]);
+    }
+  }
+}
+
+void KinematicCentroidalSolver::AddCentroidalKinematicConsistency(int knot_point) {
+  // Ensure linear and angular momentum line up
+  auto centroidal_momentum =
+      std::make_shared<CentroidalMomentumConstraint<double>>(
+          plant_, contexts_[knot_point].get(), knot_point);
+  prog_->AddConstraint(centroidal_momentum,
+                       {state_vars(knot_point), com_pos_vars(knot_point),
+                        momentum_vars(knot_point)});
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    // Ensure foot position line up with kinematics
+    auto foot_position_constraint = std::make_shared<
+        dairlib::multibody::KinematicPositionConstraint<double>>(
+        plant_, contact_sets_[contact_index], Eigen::Vector3d::Zero(),
+        Eigen::Vector3d::Zero(), full_constraint_relative_);
+    prog_->AddConstraint(foot_position_constraint,
+                         {state_vars(knot_point).head(n_q_),
+                          contact_pos_vars(knot_point, contact_index)});
+  }
+  // Constrain com position
+  auto com_position =
+      std::make_shared<CenterofMassPositionConstraint<double>>(
+          plant_, contexts_[knot_point].get(), knot_point);
+  prog_->AddConstraint(com_position,
+                       {com_pos_vars(knot_point), state_vars(knot_point)});
+}
+
+void KinematicCentroidalSolver::AddFrictionConeConstraints(int knot_point) {
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    auto force_constraints_vec =
+        contact_points_[contact_index].CreateLinearFrictionConstraints();
+    for (const auto& constraint : force_constraints_vec) {
+      prog_->AddConstraint(constraint,
+                           contact_force_vars(knot_point, contact_index));
+    }
+  }
+}
+
+void KinematicCentroidalSolver::AddFlightContactForceConstraints(int knot_point) {
+  for (int contact_index = 0; contact_index < n_contact_points_;
+       contact_index++) {
+    // Feet in flight produce no force
+    if (!contact_sequence_[knot_point][contact_index]) {
+      prog_->AddBoundingBoxConstraint(
+          Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
+          contact_force_vars(knot_point, contact_index));
+    }
+  }
+}
+
+drake::solvers::VectorXDecisionVariable KinematicCentroidalSolver::state_vars(
+    int knotpoint_index) const {
+  return x_vars_[knotpoint_index];
+}
+drake::solvers::VectorXDecisionVariable
+KinematicCentroidalSolver::joint_pos_vars(int knotpoint_index) const {
+  return x_vars_[knotpoint_index].segment(kCentroidalPosDim, n_joint_q_);
+}
+drake::solvers::VectorXDecisionVariable
+KinematicCentroidalSolver::joint_vel_vars(int knotpoint_index) const {
+  return x_vars_[knotpoint_index].segment(n_q_ + kCentroidalVelDim, n_joint_v_);
+}
+
+void KinematicCentroidalSolver::Build(
+    const drake::solvers::SolverOptions& solver_options) {
+  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+    switch (complexity_schedule_[knot_point]) {
+      case KINEMATIC_CENTROIDAL:
+        //Add complex constraints
+        AddContactConstraints(knot_point);
+        AddCentroidalKinematicConsistency(knot_point);
+        AddFrictionConeConstraints(knot_point);
+        AddFlightContactForceConstraints(knot_point);
+        //Add complex dynamics
+        AddCentroidalDynamics(knot_point);
+        AddKinematicsIntegrator(knot_point);
+        if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
+          AddSlipReductionConstraint(knot_point + 1);
+          AddCentroidalKinematicConsistency(knot_point + 1);
+        }
+        break;
+      case PLANAR_SLIP:
+        AddPlanarSlipConstraints(knot_point);
+        if(!is_last_knot(knot_point)){
+          switch (complexity_schedule_[knot_point+1]) {
+            case KINEMATIC_CENTROIDAL:
+              AddCentroidalDynamics(knot_point);
+              AddKinematicsIntegrator(knot_point);
+              AddSlipLiftingConstraint(knot_point);
+              break;
+            case PLANAR_SLIP:
+              AddSlipDynamics(knot_point);
+              break;
+          }
+        }
+        break;
+    }
+  }
+  AddCosts();
+  prog_->SetSolverOptions(solver_options);
+}
+
+void KinematicCentroidalSolver::SetConstantMomentumReference(
+    const drake::VectorX<double>& value) {
+  SetMomentumReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          value));
+}
+
+double KinematicCentroidalSolver::GetKnotpointGain(int knot_point) const {
+  const double terminal_gain = is_last_knot(knot_point) ? 100 : 1;
+  const double collocation_gain =
+      (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
+  return terminal_gain * collocation_gain;
+}
+
+void KinematicCentroidalSolver::AddCosts() {
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    double knot_point_gain = GetKnotpointGain(knot_point);
+    double t = dt_ * knot_point;
+
+    switch (complexity_schedule_[knot_point]) {
+      case KINEMATIC_CENTROIDAL:
+        if (q_ref_traj_) {
+          prog_->AddQuadraticErrorCost(knot_point_gain * Q_q_,
+                                       q_ref_traj_->value(t),
+                                       state_vars(knot_point).head(n_q_));
+        }
+        if (v_ref_traj_) {
+          prog_->AddQuadraticErrorCost(knot_point_gain * Q_v_,
+                                       v_ref_traj_->value(t),
+                                       state_vars(knot_point).tail(n_v_));
+        }
+        if (com_ref_traj_) {
+          prog_->AddQuadraticErrorCost(knot_point_gain * Q_com_,
+                                       com_ref_traj_->value(t),
+                                       com_pos_vars(knot_point));
+        }
+        if (mom_ref_traj_) {
+          prog_->AddQuadraticErrorCost(knot_point_gain * Q_mom_,
+                                       mom_ref_traj_->value(t),
+                                       momentum_vars(knot_point));
+        }
+        if (contact_ref_traj_) {
+          prog_->AddQuadraticErrorCost(
+              knot_point_gain * Q_contact_,
+              contact_ref_traj_->value(t),
+              {contact_pos_[knot_point], contact_vel_[knot_point]});
+        }
+        if (force_ref_traj_) {
+          prog_->AddQuadraticErrorCost(knot_point_gain * Q_force_,
+                                       force_ref_traj_->value(t),
+                                       contact_force_[knot_point]);
+        }
+        break;
+      case PLANAR_SLIP:
+        AddPlanarSlipCost(knot_point, knot_point_gain);
+        break;
+    }
+  }
+}
+
+void KinematicCentroidalSolver::UpdateCosts() {}
+
+void KinematicCentroidalSolver::SetZeroInitialGuess() {
+  Eigen::VectorXd initialGuess =
+      Eigen::VectorXd::Zero(n_q_ + n_v_ + n_contact_points_ * 9 + 6 + 3);
+  prog_->SetInitialGuessForAllVariables(
+      initialGuess.replicate(n_knot_points_, 1));
+  // Make sure unit quaternions
+  for (int i = 0; i < n_knot_points_; i++) {
+    auto xi = state_vars(i);
+    prog_->SetInitialGuess(xi(0), 1);
+    prog_->SetInitialGuess(xi(1), 0);
+    prog_->SetInitialGuess(xi(2), 0);
+    prog_->SetInitialGuess(xi(3), 0);
+  }
+}
+
+drake::trajectories::PiecewisePolynomial<double>
+KinematicCentroidalSolver::Solve() {
+  for (int i = 0; i < 1; i++) {
+    auto start = std::chrono::high_resolution_clock::now();
+    solver_->Solve(*prog_, prog_->initial_guess(), prog_->solver_options(),
+                   result_.get());
+    auto finish = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> elapsed = finish - start;
+    solve_time_ = elapsed.count();
+    std::cout << "Solve time:" << elapsed.count() << std::endl;
+    std::cout << "Cost:" << result_->get_optimal_cost() << std::endl;
+    prog_->SetInitialGuessForAllVariables(result_->GetSolution());
+  }
+
+  std::vector<double> time_points;
+  std::vector<drake::MatrixX<double>> states;
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    time_points.emplace_back(dt_ * knot_point);
+    switch (complexity_schedule_[knot_point]) {
+      case KINEMATIC_CENTROIDAL:
+        states.emplace_back(result_->GetSolution(state_vars(knot_point)));
+        break;
+      case PLANAR_SLIP:
+        states.emplace_back(LiftSlipSolution(knot_point));
+        break;
+    }
+  }
+  return drake::trajectories::PiecewisePolynomial<double>::FirstOrderHold(
+      time_points, states);
+}
+
+void KinematicCentroidalSolver::SerializeSolution(int n_knot_points,
+                                                  double time_offset) {
+  DRAKE_DEMAND(result_->is_success());
+
+  Eigen::MatrixXd state_points;
+  Eigen::MatrixXd contact_force_points;
+  std::vector<double> time_samples;
+  this->SetFromSolution(*result_, &state_points, &contact_force_points,
+                        &time_samples);
+  for (auto& t : time_samples) {
+    t += time_offset + solve_time_;
+  }
+
+  int knot_points_to_serialize =
+      n_knot_points == -1 ? time_samples.size() : n_knot_points;
+
+  dairlib::LcmTrajectory::Trajectory state_traj;
+  dairlib::LcmTrajectory::Trajectory contact_force_traj;
+  state_traj.traj_name = "state_traj";
+  state_traj.datapoints = state_points.leftCols(knot_points_to_serialize);
+  state_traj.time_vector =
+      Eigen::Map<VectorXd>(time_samples.data(), knot_points_to_serialize);
+  state_traj.datatypes =
+      dairlib::multibody::CreateStateNameVectorFromMap(plant_);
+
+  contact_force_traj.traj_name = "contact_force_traj";
+  contact_force_traj.datapoints =
+      contact_force_points.leftCols(knot_points_to_serialize);
+  contact_force_traj.time_vector =
+      //      Eigen::Map<VectorXd>(time_samples.data(), time_samples.size());
+      Eigen::Map<VectorXd>(time_samples.data(), knot_points_to_serialize);
+  contact_force_traj.datatypes =
+      std::vector<std::string>(contact_force_traj.datapoints.rows());
+
+  std::vector<dairlib::LcmTrajectory::Trajectory> trajectories;
+  trajectories.push_back(state_traj);
+  trajectories.push_back(contact_force_traj);
+  std::vector<std::string> trajectory_names = {state_traj.traj_name,
+                                               contact_force_traj.traj_name};
+  lcm_trajectory_ =
+      LcmTrajectory(trajectories, trajectory_names, "centroidal_mpc_solution",
+                    "centroidal_mpc_solution");
+}
+
+dairlib::lcmt_saved_traj KinematicCentroidalSolver::GenerateLcmTraj(
+    int n_knot_points, double time_offset) {
+  SerializeSolution(n_knot_points, time_offset);
+  return lcm_trajectory_.GenerateLcmObject();
+}
+
+void KinematicCentroidalSolver::PublishSolution(const std::string& lcm_channel,
+                                                int n_knot_points) {
+  if (!lcm_) {
+    lcm_ = std::make_unique<drake::lcm::DrakeLcm>();
+  }
+  auto lcm_msg = GenerateLcmTraj(n_knot_points);
+  drake::lcm::Publish(lcm_.get(), lcm_channel, lcm_msg);
+}
+
+bool KinematicCentroidalSolver::SaveSolutionToFile(
+    const std::string& filepath) {
+  SerializeSolution(-1);
+  lcm_trajectory_.WriteToFile(filepath);
+  return true;
+}
+
+void KinematicCentroidalSolver::SetFromSolution(
+    const drake::solvers::MathematicalProgramResult& result,
+    Eigen::MatrixXd* state_samples, Eigen::MatrixXd* contact_force_samples,
+    std::vector<double>* time_samples) const {
+  DRAKE_ASSERT(state_samples != nullptr);
+  DRAKE_ASSERT(contact_force_samples != nullptr);
+  DRAKE_ASSERT(time_samples->empty());
+
+  *state_samples = MatrixXd(n_q_ + n_v_, n_knot_points_);
+  *contact_force_samples = MatrixXd(3 * n_contact_points_, n_knot_points_);
+  time_samples->resize(n_knot_points_);
+
+  for (int knot_point = 0; knot_point < num_knot_points(); knot_point++) {
+    time_samples->at(knot_point) = knot_point * dt_;
+
+    VectorXd x = result.GetSolution(state_vars(knot_point));
+    VectorXd contact_force = result.GetSolution(contact_force_.at(knot_point));
+    state_samples->col(knot_point) = x;
+    contact_force_samples->col(knot_point) = contact_force;
+  }
+}
+
+void KinematicCentroidalSolver::CreateVisualizationCallback(
+    const std::string& model_file, double alpha,
+    const std::string& weld_frame_to_world) {
+  DRAKE_DEMAND(!callback_visualizer_);  // Cannot be set twice
+
+  // Assemble variable list
+  drake::solvers::VectorXDecisionVariable vars(n_knot_points_ / 2 * n_q_);
+  for (int knot_point = 0; knot_point < n_knot_points_ / 2; knot_point++) {
+    vars.segment(knot_point * n_q_, n_q_) =
+        state_vars(knot_point * 2).head(n_q_);
+  }
+  Eigen::VectorXd alpha_vec =
+      Eigen::VectorXd::Constant(n_knot_points_ / 2, alpha);
+  alpha_vec(0) = 1;
+  alpha_vec(n_knot_points_ / 2 - 1) = 1;
+
+  // Create visualizer
+  callback_visualizer_ =
+      std::make_unique<dairlib::multibody::MultiposeVisualizer>(
+          model_file, n_knot_points_ / 2, alpha_vec, weld_frame_to_world);
+
+  // Callback lambda function
+  auto my_callback = [this](const Eigen::Ref<const Eigen::VectorXd>& vars) {
+    Eigen::VectorXd vars_copy = vars;
+    Eigen::Map<Eigen::MatrixXd> states(vars_copy.data(), this->n_q_,
+                                       this->n_knot_points_ / 2);
+    this->callback_visualizer_->DrawPoses(states);
+  };
+
+  prog_->AddVisualizationCallback(my_callback, vars);
+}
+void KinematicCentroidalSolver::AddContactPointPositionConstraint(
+    int contact_index, const Eigen::Vector3d& lb, const Eigen::Vector3d& ub) {
+  //{TODO} eventually make in body frame
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->AddBoundingBoxConstraint(
+        lb, ub, contact_pos_vars(knot_point, contact_index));
+  }
+}
+void KinematicCentroidalSolver::AddPlantJointLimits(
+    const std::vector<std::string>& joint_to_constrain) {
+  std::map<std::string, int> positions_map =
+      dairlib::multibody::MakeNameToPositionsMap(plant_);
+  for (const auto& member : joint_to_constrain) {
+    for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+      prog_->AddBoundingBoxConstraint(
+          plant_.GetJointByName(member).position_lower_limits()(0),
+          plant_.GetJointByName(member).position_upper_limits()(0),
+          state_vars(knot_point)[positions_map.at(member)]);
+    }
+  }
+}
+
+drake::solvers::VectorXDecisionVariable KinematicCentroidalSolver::com_pos_vars(
+    int knotpoint_index) const {
+  return com_vars_[knotpoint_index];
+}
+drake::solvers::VectorXDecisionVariable
+KinematicCentroidalSolver::contact_pos_vars(int knotpoint_index,
+                                            int contact_index) const {
+  return contact_pos_[knotpoint_index].segment(contact_index * 3, 3);
+}
+drake::solvers::VectorXDecisionVariable
+KinematicCentroidalSolver::contact_vel_vars(int knotpoint_index,
+                                            int contact_index) const {
+  return contact_vel_[knotpoint_index].segment(contact_index * 3, 3);
+}
+drake::solvers::VectorXDecisionVariable
+KinematicCentroidalSolver::contact_force_vars(int knotpoint_index,
+                                              int contact_index) const {
+  return contact_force_[knotpoint_index].segment(contact_index * 3, 3);
+}
+void KinematicCentroidalSolver::AddKinematicConstraint(
+    std::shared_ptr<dairlib::multibody::KinematicPositionConstraint<double>>
+        con,
+    const Eigen::Ref<const drake::solvers::VectorXDecisionVariable>& vars) {
+  prog_->AddConstraint(std::move(con), vars);
+}
+void KinematicCentroidalSolver::SetRobotStateGuess(
+    const drake::VectorX<double>& state) {
+  DRAKE_DEMAND(state.size() == n_q_ + n_v_);
+
+  for (const auto& state_vars : x_vars_) {
+    prog_->SetInitialGuess(state_vars, state);
+  }
+}
+
+void KinematicCentroidalSolver::SetRobotStateGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& state_trajectory) {
+  DRAKE_DEMAND(state_trajectory.rows() == n_q_ + n_v_);
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(state_vars(knot_point),
+                           state_trajectory.value(dt_ * knot_point));
+  }
+}
+
+void KinematicCentroidalSolver::SetRobotStateGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& q_traj,
+    const drake::trajectories::PiecewisePolynomial<double>& v_traj) {
+  DRAKE_DEMAND(q_traj.rows() == n_q_);
+  DRAKE_DEMAND(v_traj.rows() == n_v_);
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(state_vars(knot_point).head(n_q_),
+                           q_traj.value(dt_ * knot_point));
+    prog_->SetInitialGuess(state_vars(knot_point).tail(n_v_),
+                           v_traj.value(dt_ * knot_point));
+  }
+}
+
+drake::solvers::VectorXDecisionVariable
+KinematicCentroidalSolver::momentum_vars(int knotpoint_index) const {
+  return mom_vars_[knotpoint_index];
+}
+void KinematicCentroidalSolver::AddComHeightBoundingConstraint(double lb,
+                                                               double ub) {
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->AddBoundingBoxConstraint(lb, ub, com_pos_vars(knot_point)[2]);
+  }
+}
+void KinematicCentroidalSolver::SetComPositionGuess(
+    const drake::Vector3<double>& state) {
+  for (const auto& com_pos : com_vars_) {
+    prog_->SetInitialGuess(com_pos, state);
+  }
+}
+
+void KinematicCentroidalSolver::SetComPositionGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& com_trajectory) {
+  DRAKE_DEMAND(com_trajectory.rows() == 3);
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(com_pos_vars(knot_point),
+                           com_trajectory.value(dt_ * knot_point));
+  }
+}
+
+void KinematicCentroidalSolver::SetContactGuess(
+    const drake::trajectories::PiecewisePolynomial<double>&
+        contact_trajectory) {
+  DRAKE_DEMAND(contact_trajectory.rows() == 6 * n_contact_points_);
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(
+        contact_pos_[knot_point],
+        drake::VectorX<double>(contact_trajectory.value(dt_ * knot_point))
+            .head(3 * n_contact_points_));
+    prog_->SetInitialGuess(
+        contact_vel_[knot_point],
+        drake::VectorX<double>(contact_trajectory.value(dt_ * knot_point))
+            .tail(3 * n_contact_points_));
+  }
+}
+void KinematicCentroidalSolver::SetForceGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& force_trajectory) {
+  DRAKE_DEMAND(force_trajectory.rows() == 3 * n_contact_points_);
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(contact_force_[knot_point],
+                           force_trajectory.value(dt_ * knot_point));
+  }
+}
+
+void KinematicCentroidalSolver::SetMomentumGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory) {
+  DRAKE_DEMAND(momentum_trajectory.rows() == 6);
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    prog_->SetInitialGuess(mom_vars_[knot_point],
+                           momentum_trajectory.value(dt_ * knot_point));
+  }
+}
+
+void KinematicCentroidalSolver::SetModeSequence(
+    const std::vector<std::vector<bool>>& contact_sequence) {
+  contact_sequence_ = contact_sequence;
+}
+void KinematicCentroidalSolver::SetModeSequence(
+    const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) {
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
+    for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
+      contact_sequence_[knot_point][contact_index] =
+          contact_sequence.value(dt_ * knot_point).coeff(contact_index);
+    }
+  }
+}
+
+void KinematicCentroidalSolver::AddInitialStateConstraint(
+    const Eigen::VectorXd& state) {
+  DRAKE_DEMAND(state.size() == state_vars(0).size());
+  init_state_constraint_ =
+      prog_->AddBoundingBoxConstraint(state, state, state_vars((0)))
+          .evaluator()
+          .get();
+}
+
+void KinematicCentroidalSolver::UpdateInitialStateConstraint(
+    const Eigen::VectorXd& state) {
+  DRAKE_DEMAND(state.size() == state_vars(0).size());
+  init_state_constraint_->set_bounds(state, state);
+}
+
+void KinematicCentroidalSolver::SetGains(
+    const KinematicCentroidalGains& gains) {
+  std::map<std::string, int> positions_map =
+      dairlib::multibody::MakeNameToPositionsMap(plant_);
+  std::map<std::string, int> velocities_map =
+      dairlib::multibody::MakeNameToVelocitiesMap(plant_);
+
+  // Generalize state
+  for (const auto& [name, index] : positions_map) {
+    const auto it = gains.generalized_positions.find(name);
+    if (it != gains.generalized_positions.end()) {
+      Q_q_(index, index) = it->second;
+    } else if (name.find("left") != std::string::npos) {
+      const auto it_left =
+          gains.generalized_positions.find(name.substr(0, name.size() - 4));
+      if (it_left != gains.generalized_positions.end()) {
+        Q_q_(index, index) = it_left->second;
+      }
+    } else if (name.find("right") != std::string::npos) {
+      const auto it_right =
+          gains.generalized_positions.find(name.substr(0, name.size() - 5));
+      if (it_right != gains.generalized_positions.end()) {
+        Q_q_(index, index) = it_right->second;
+      }
+    }
+  }
+
+  for (const auto& [name, index] : velocities_map) {
+    const auto it = gains.generalized_velocities.find(name);
+    if (it != gains.generalized_velocities.end()) {
+      Q_v_(index, index) = it->second;
+    } else if (name.find("left") != std::string::npos) {
+      const auto it_left =
+          gains.generalized_velocities.find(name.substr(0, name.size() - 7));
+      if (it_left != gains.generalized_velocities.end()) {
+        Q_v_(index, index) = it_left->second;
+      }
+    } else if (name.find("right") != std::string::npos) {
+      const auto it_right =
+          gains.generalized_velocities.find(name.substr(0, name.size() - 8));
+      if (it_right != gains.generalized_velocities.end()) {
+        Q_v_(index, index) = it_right->second;
+      }
+    }
+  }
+
+  // com
+  Q_com_ = gains.com_position.asDiagonal();
+
+  // momentum
+  Q_mom_.block<3, 3>(0, 0, 3, 3) = gains.ang_momentum.asDiagonal();
+  Q_mom_.block<3, 3>(3, 3, 3, 3) = gains.lin_momentum.asDiagonal();
+
+  // contact pos, contact vel
+  Q_contact_.block<Eigen::Dynamic, Eigen::Dynamic>(0, 0, 3 * n_contact_points_,
+                                                   3 * n_contact_points_) =
+      gains.contact_pos.replicate(n_contact_points_, 1).asDiagonal();
+  Q_contact_.block<Eigen::Dynamic, Eigen::Dynamic>(
+      3 * n_contact_points_, 3 * n_contact_points_, 3 * n_contact_points_,
+      3 * n_contact_points_) =
+      gains.contact_vel.replicate(n_contact_points_, 1).asDiagonal();
+
+  // contact force
+  Q_force_ = gains.contact_force.replicate(n_contact_points_, 1).asDiagonal();
+}
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
new file mode 100644
index 0000000000..bdc2f853bc
--- /dev/null
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
@@ -0,0 +1,494 @@
+#pragma once
+#include <drake/lcm/drake_lcm.h>
+#include <drake/solvers/ipopt_solver.h>
+#include <drake/solvers/mathematical_program.h>
+
+#include "lcm/lcm_trajectory.h"
+#include "multibody/kinematic/kinematic_constraints.h"
+#include "multibody/kinematic/kinematic_evaluator_set.h"
+#include "multibody/kinematic/world_point_evaluator.h"
+#include "multibody/multipose_visualizer.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_constraints.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h"
+
+#include "drake/common/trajectories/piecewise_polynomial.h"
+#include "drake/multibody/plant/multibody_plant.h"
+#include "drake/systems/framework/context.h"
+
+enum Complexity{
+  KINEMATIC_CENTROIDAL,
+  PLANAR_SLIP
+};
+
+/*!
+ * @brief Class for solving nonlinear kinematic centroidal mpc. Implementation
+ * is based on Dai, Hongkai, Andres Valenzuela, and Russ Tedrake. “Whole-Body
+ * Motion Planning with Centroidal Dynamics and Full Kinematics.” 2014 IEEE-RAS
+ * International Conference on Humanoid Robots (November 2014).
+ *
+ * The optimization contains two coupled problems. The centroidal problem
+ * optimizes over: Angular momentum Linear momentum Center of mass position
+ *      Contact position
+ *      Contact velocity
+ *      Contact force
+ *  While the kinematics problem optimzes over:
+ *      Generalized positions (including floating base)
+ *      Generalized velocities (including floating boase)
+ *
+ * A series of constraints couple the kinematic state to the centroidal state
+ * ensuring the contact point's, com position, com velocity, body orientation,
+ * body angular velocity all align up
+ */
+class KinematicCentroidalSolver {
+ public:
+  /*!
+   * @brief Constructor for MPC which initializes decision variables
+   * @param plant robot model
+   * @param n_knot_points number of knot points
+   * @param dt time step
+   * @param contact_points vector of world point evaluators which describes the
+   * points on the robot which might make contact with the world. This is not
+   * the mode sequence
+   */
+  KinematicCentroidalSolver(
+      const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points,
+      double dt,
+      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+          contact_points);
+
+  /*!
+   * @brief Sets the cost for the mpc problem using a gains struct
+   * @param gains
+   */
+  void SetGains(const KinematicCentroidalGains& gains);
+
+  /*!
+   * @brief Sets the minimum foot clearance height
+   * @param gains
+   */
+  void SetMinimumFootClearance(const double& swing_foot_clearance) {
+    swing_foot_minimum_height_ = swing_foot_clearance;
+  }
+
+  /*!
+   * @brief Adds a cost reference for the generalized position of the robot of
+   * the form (x - x_ref)^T Q (x - x_ref)
+   * @param ref_traj trajectory in time
+   */
+  void SetGenPosReference(
+      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
+
+  /*!
+   * @brief Adds a cost reference for the generalized velocity of the robot of
+   * the form (x - x_ref)^T Q (x - x_ref)
+   * @param ref_traj trajectory in time
+   */
+  void SetGenVelReference(
+      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
+
+  /*!
+   * @brief Adds a cost and reference for the center of mass position of the
+   * robot (x - x_ref)^T Q (x - x_ref)
+   * @param ref_traj trajectory in time
+   */
+  void SetComReference(
+      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
+
+  /*!
+   * @brief Adds a cost and reference for the contact position and velocity (x -
+   * x_ref)^T Q (x - x_ref)
+   * @param contact_ref_traj trajectory in time, order is all contact pos, all
+   * contact vel
+   */
+  void SetContactTrackingReference(
+      std::unique_ptr<drake::trajectories::Trajectory<double>>
+          contact_ref_traj);
+
+  /*!
+   * @brief Add a cost and reference for the contact forces (x - x_ref)^T Q (x -
+   * x_ref)
+   * @param force_ref_traj trajectory in time
+   */
+  void SetForceTrackingReference(
+      std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj);
+
+  /*!
+   * @brief Adds a cost and reference for the angular and linear momentum of the
+   * robot (x - x_ref)^T Q (x - x_ref)
+   * @param ref_traj
+   */
+  void SetMomentumReference(
+      std::unique_ptr<drake::trajectories::Trajectory<double>> ref_traj);
+
+  void SetConstantMomentumReference(const drake::VectorX<double>& value);
+
+  /*!
+   * @brief accessor for robot state decision vars
+   * @param knotpoint_index
+   * @return
+   */
+  drake::solvers::VectorXDecisionVariable state_vars(int knotpoint_index) const;
+
+  /*!
+   * @brief accessor for joint position decision vars
+   * @param knotpoint_index
+   * @return
+   */
+  drake::solvers::VectorXDecisionVariable joint_pos_vars(
+      int knotpoint_index) const;
+
+  /*!
+   * @brief accessor for joint velocity decision vars
+   * @param knotpoint_index
+   * @return
+   */
+  drake::solvers::VectorXDecisionVariable joint_vel_vars(
+      int knotpoint_index) const;
+
+  /*!
+   * @brief accessor for center of mass position decision vars
+   * @param knotpoint_index
+   * @return
+   */
+  drake::solvers::VectorXDecisionVariable com_pos_vars(
+      int knotpoint_index) const;
+
+  /*!
+   * @brief accessor for center of momentum decision variables
+   * @param knotpoint_index
+   * @return
+   */
+  drake::solvers::VectorXDecisionVariable momentum_vars(
+      int knotpoint_index) const;
+
+  /*!
+   * @brief accessor for contact position decision variables
+   * @param knotpoint_index
+   * @param contact integer corresponding to the contact point based on order of
+   * contact points in constructor
+   * @return [x,y,z] vector of decision variables for contact 'contact''s
+   * position at knotpoint `knotpoint_index`
+   */
+  drake::solvers::VectorXDecisionVariable contact_pos_vars(
+      int knotpoint_index, int contact_index) const;
+
+  /*!
+   * @brief accessor for contact velocity decision variables
+   * @param knotpoint_index
+   * @param contact_index integer corresponding to the contact point based on
+   * order of contact points in constructor
+   * @return [x,y,z] vector of decision variables for contact 'contact''s
+   * velocity at knotpoint `knotpoint_index`
+   */
+  drake::solvers::VectorXDecisionVariable contact_vel_vars(
+      int knotpoint_index, int contact_index) const;
+
+  /*!
+   * @brief accessor for contact force decision variables
+   * @param knotpoint_index
+   * @param contact_index integer corresponding to the contact point based on
+   * order of contact points in constructor
+   * @return [x,y,z] vector of decision variables for contact 'contact''s force
+   * at knotpoint `knotpoint_index`
+   */
+  drake::solvers::VectorXDecisionVariable contact_force_vars(
+      int knotpoint_index, int contact_index) const;
+
+  /*!
+   * @brief Adds standard constraints to optimization problem and sets options
+   * @param solver_options
+   */
+  virtual void Build(const drake::solvers::SolverOptions& solver_options);
+
+  /*!
+   * @brief Solve the optimization problem and return a piecewise linear
+   * trajectory of state
+   * @return piecewise linear trajectory of state
+   */
+  drake::trajectories::PiecewisePolynomial<double> Solve();
+
+  /*!
+   *
+   * @brief Serializes the solution into a member variable in order to
+   *        be published via LCM or saved to a file
+   * @param n_knot_points lcm_channel for where to publish
+   * @throws exception Solve() has not been called.
+   */
+  void SerializeSolution(int n_knot_points, double time_offset = 0);
+
+  /*!
+   *
+   * @brief Generate lcmt_saved_traj
+   * @param lcm_channel lcm_channel for where to publish
+   * @throws exception Solve() has not been called.
+   */
+  dairlib::lcmt_saved_traj GenerateLcmTraj(int n_knot_points,
+                                           double time_offset = 0);
+
+  /*!
+   *
+   * @brief Publish the solution on lcm_channel
+   * @param lcm_channel lcm_channel for where to publish
+   * @throws exception Solve() has not been called.
+   */
+  void PublishSolution(const std::string& lcm_channel, int n_knot_points);
+
+  /*!
+   *
+   * @brief Saves the solution at filepath
+   * @param filepath relative filepath for where to save the solution
+   * @return whether saving was successful
+   * @throws exception Solve() has not been called.
+   */
+  bool SaveSolutionToFile(const std::string& filepath);
+
+  /*!
+   *@brief Populate the state and time vector with the solution from result
+   */
+  void SetFromSolution(const drake::solvers::MathematicalProgramResult& result,
+                       Eigen::MatrixXd* state_samples,
+                       Eigen::MatrixXd* contact_force_samples,
+                       std::vector<double>* time_samples) const;
+
+  /*!
+   *@brief Sets initial guess corresponding to zero for everything except
+   *quaternions
+   */
+  void SetZeroInitialGuess();
+
+  void SetRobotStateGuess(const drake::VectorX<double>& state);
+
+  void SetRobotStateGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& state_trajectory);
+
+  // TODO remove once drake has trajectory stacking
+  virtual void SetRobotStateGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& q_traj,
+      const drake::trajectories::PiecewisePolynomial<double>& v_traj);
+
+  void SetComPositionGuess(const drake::Vector3<double>& state);
+
+  virtual void SetComPositionGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& com_trajectory);
+
+  virtual void SetContactGuess(const drake::trajectories::PiecewisePolynomial<double>&
+                           contact_trajectory);
+
+  virtual void SetForceGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& force_trajectory);
+
+  virtual void SetMomentumGuess(
+      const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory);
+
+  void CreateVisualizationCallback(const std::string& model_file, double alpha,
+                                   const std::string& weld_frame_to_world = "");
+
+  /*!
+   * @brief Add a bounding box constraint to a contact position for all knot
+   * points
+   * @param contact_index integer corresponding to the contact point based on
+   * order of contact points in constructor
+   * @param lb lower bound
+   * @param ub upper bound
+   */
+  void AddContactPointPositionConstraint(int contact_index,
+                                         const Eigen::Vector3d& lb,
+                                         const Eigen::Vector3d& ub);
+
+  /*!
+   * @brief Adds joint limits to the joint names included in joints_to_limits
+   * using values in the plant
+   * @param joints_to_limit vector of strings
+   */
+  void AddPlantJointLimits(const std::vector<std::string>& joints_to_limit);
+
+  /*!
+   * @brief Adds a kinematic position constraint to the optimization
+   * @param con a shared pointer to the constraint
+   * @param vars the decision variables for the constraint
+   */
+  void AddKinematicConstraint(
+      std::shared_ptr<dairlib::multibody::KinematicPositionConstraint<double>>
+          con,
+      const Eigen::Ref<const drake::solvers::VectorXDecisionVariable>& vars);
+
+  /*!
+   * @brief Adds a constraint on com height to all knot points
+   * @param lb
+   * @param ub
+   */
+  void AddComHeightBoundingConstraint(double lb, double ub);
+
+  int num_knot_points() const { return n_knot_points_; }
+
+  /*!
+   * @brief Set the mode sequence
+   * @param contact_sequence vector where
+   * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point`
+   * is `contact_index` active
+   */
+  virtual void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
+
+  /*!
+   * @brief Set the mode sequence via a trajectory. The value of the trajectory
+   * at each time, cast to a bool is if a contact point is active or not
+   * @param contact_sequence
+   */
+  virtual void SetModeSequence(
+      const drake::trajectories::PiecewisePolynomial<double>& contact_sequence);
+
+  void AddInitialStateConstraint(const Eigen::VectorXd& state);
+
+  void UpdateInitialStateConstraint(const Eigen::VectorXd& state);
+
+  const drake::multibody::MultibodyPlant<double>& Plant() { return plant_; };
+
+  /*!
+   * @brief Update costs from internally stored variables using current
+   * references
+   */
+  void UpdateCosts();
+
+  void SetComplexitySchedule(const std::vector<Complexity>& complexity_schedule){complexity_schedule_ = complexity_schedule;};
+ protected:
+  /*!
+   * @brief Adds dynamics for centroidal state
+   */
+  void AddCentroidalDynamics(int knot_point);
+
+  /*!
+   * @brief Enforces zero force for feet in flight
+   */
+  void AddFlightContactForceConstraints(int knot_point);
+
+  /*!
+   * @brief Enforce dynamics for kinematics and location of the contacts
+   */
+  void AddKinematicsIntegrator(int knot_point);
+
+  /*!
+   * @brief Feet that in stance are not moving and on the ground, feet in the
+   * air are above the ground
+   */
+  void AddContactConstraints(int knot_point);
+
+  /*!
+   * @brief Ensures that contact point for feet line up with kinematics, and
+   * centroidal state lines up with kinematic state
+   */
+  void AddCentroidalKinematicConsistency(int knot_point);
+
+  /*!
+   * @brief Ensures feet are not pulling on the ground
+   */
+  void AddFrictionConeConstraints(int knot_point);
+
+  //  void AddTorqueLimits();
+  /*!
+   * @brief Get corresponding weight for a knot_point
+   */
+  double GetKnotpointGain(int knot_point) const;
+
+  virtual void AddPlanarSlipConstraints(int knot_point){ DRAKE_DEMAND(false);};
+
+  virtual void AddPlanarSlipCost(int knot_point, double terminal_gain){ DRAKE_DEMAND(false);};
+
+  virtual void AddSlipReductionConstraint(int knot_point){ DRAKE_DEMAND(false);};
+
+  virtual void AddSlipLiftingConstraint(int knot_point){ DRAKE_DEMAND(false);};
+
+  virtual void AddSlipDynamics(int knot_point){ DRAKE_DEMAND(false);};
+
+  virtual drake::VectorX<double> LiftSlipSolution(int knot_point){ DRAKE_DEMAND(false);};
+
+  /*!
+   * @brief Add costs from internally stored variables
+   */
+  void AddCosts();
+
+  bool is_first_knot(int knot_point_index) const {
+    return knot_point_index == 0;
+  };
+  bool is_last_knot(int knot_point_index) const {
+    return knot_point_index == n_knot_points_ - 1;
+  };
+
+  const drake::multibody::MultibodyPlant<double>& plant_;
+
+  int n_knot_points_;
+  double dt_;
+
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>> contact_points_;
+  std::vector<dairlib::multibody::KinematicEvaluatorSet<double>> contact_sets_;
+
+  static const int kCentroidalPosDim = 7;
+  static const int kCentroidalVelDim = 6;
+  int n_q_;
+  int n_v_;
+  int n_joint_q_;
+  int n_joint_v_;
+  int n_contact_points_;
+
+  /// References and cost matrixes
+  std::unique_ptr<drake::trajectories::Trajectory<double>> q_ref_traj_;
+  Eigen::MatrixXd Q_q_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> v_ref_traj_;
+  Eigen::MatrixXd Q_v_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> com_ref_traj_;
+  Eigen::MatrixXd Q_com_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> mom_ref_traj_;
+  Eigen::MatrixXd Q_mom_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> contact_ref_traj_;
+  Eigen::MatrixXd Q_contact_;
+  std::unique_ptr<drake::trajectories::Trajectory<double>> force_ref_traj_;
+  Eigen::MatrixXd Q_force_;
+
+  std::vector<std::vector<bool>> contact_sequence_;
+  // MathematicalProgram
+  std::unique_ptr<drake::solvers::MathematicalProgram> prog_;
+  std::unique_ptr<drake::solvers::IpoptSolver> solver_;
+  std::unique_ptr<drake::solvers::MathematicalProgramResult> result_;
+  double solve_time_;
+
+  // Constraint bindings
+  drake::solvers::BoundingBoxConstraint* init_state_constraint_;
+  std::vector<drake::solvers::Binding<drake::solvers::Constraint>>
+      centroidal_dynamics_binding_;
+
+  // Cost bindings
+  std::vector<std::shared_ptr<drake::solvers::QuadraticCost>> q_ref_cost_;
+  std::vector<std::shared_ptr<drake::solvers::QuadraticCost>> v_ref_cost_;
+  std::vector<std::shared_ptr<drake::solvers::QuadraticCost>> com_ref_cost_;
+  std::vector<std::shared_ptr<drake::solvers::QuadraticCost>> mom_ref_cost_;
+  std::vector<std::shared_ptr<drake::solvers::QuadraticCost>> contact_ref_cost_;
+  std::vector<std::shared_ptr<drake::solvers::QuadraticCost>> force_ref_cost_;
+
+  // DecisionVariables
+  // Full robot state
+  std::vector<drake::solvers::VectorXDecisionVariable> x_vars_;
+  // angular and linear momentum variables (in that order)
+  std::vector<drake::solvers::VectorXDecisionVariable> mom_vars_;
+  // center of mass position
+  std::vector<drake::solvers::VectorXDecisionVariable> com_vars_;
+
+  // Contact position, velocity, and force [n_knot_points, 3 *
+  // n_contact_points]
+  std::vector<drake::solvers::VectorXDecisionVariable> contact_pos_;
+  std::vector<drake::solvers::VectorXDecisionVariable> contact_vel_;
+  std::vector<drake::solvers::VectorXDecisionVariable> contact_force_;
+
+  std::vector<std::unique_ptr<drake::systems::Context<double>>> contexts_;
+
+  std::unique_ptr<dairlib::multibody::MultiposeVisualizer> callback_visualizer_;
+
+  const std::set<int> full_constraint_relative_ = {0, 1, 2};
+
+  double swing_foot_minimum_height_;
+
+  // saving and publishing solutions
+  std::unique_ptr<drake::lcm::DrakeLcm> lcm_;
+  dairlib::LcmTrajectory lcm_trajectory_;
+
+  std::vector<Complexity> complexity_schedule_;
+  KinematicCentroidalGains gains_;
+};
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
similarity index 74%
rename from systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
rename to systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
index 2271de548b..5189a9262c 100644
--- a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
@@ -1,7 +1,45 @@
-#include <iostream>
-#include "reference_generation_utils.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h"
 
 #include "multibody/multibody_utils.h"
+#include "examples/Cassie/cassie_utils.h"
+
+
+std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<
+    double> &plant,
+                                                                                                                  double mu) {
+  auto left_toe_pair = dairlib::LeftToeFront(plant);
+  auto left_heel_pair = dairlib::LeftToeRear(plant);
+  auto right_toe_pair = dairlib::RightToeFront(plant);
+  auto right_heel_pair = dairlib::RightToeRear(plant);
+
+  std::vector<int> active_inds{0, 1, 2};
+
+  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, left_toe_pair.first, left_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  left_toe_eval.set_frictional();
+  left_toe_eval.set_mu(mu);
+
+  auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, left_heel_pair.first, left_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  left_heel_eval.set_frictional();
+  left_heel_eval.set_mu(mu);
+
+  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, right_toe_pair.first, right_toe_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  right_toe_eval.set_frictional();
+  right_toe_eval.set_mu(mu);
+
+  auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
+      plant, right_heel_pair.first, right_heel_pair.second,
+      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  right_heel_eval.set_frictional();
+  right_heel_eval.set_mu(mu);
+
+  return {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval};
+}
 
 drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(
     const Eigen::Vector3d& current_com,
diff --git a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
similarity index 93%
rename from systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
rename to systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
index 81163d3c21..ae139e44c4 100644
--- a/systems/controllers/kinematic_centroidal_mpc/reference_generation_utils.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
@@ -3,7 +3,16 @@
 #include <drake/multibody/plant/multibody_plant.h>
 #include <drake/common/trajectories/piecewise_polynomial.h>
 #include "multibody/kinematic/world_point_evaluator.h"
-#include "systems/controllers/kinematic_centroidal_mpc/gait.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h"
+
+/*!
+ * @brief creates vector of world point evaluators for cassie
+ * @param plant cassie plant
+ * @param mu coefficient of friction
+ * @return
+ */
+std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
+
 
 /*!
  * @brief given an initial com and velocity of com emrt w, calculate a com trajectory
@@ -81,4 +90,4 @@ drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(c
                                                                                const drake::trajectories::PiecewisePolynomial<
                                                                                    double> &q_traj,
                                                                                const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &v_traj);
\ No newline at end of file
+                                                                                   double> &v_traj);
diff --git a/systems/trajectory_optimization/lcm_trajectory_systems.cc b/systems/trajectory_optimization/lcm_trajectory_systems.cc
index 667e9dfa4b..d77bfa10b6 100644
--- a/systems/trajectory_optimization/lcm_trajectory_systems.cc
+++ b/systems/trajectory_optimization/lcm_trajectory_systems.cc
@@ -1,37 +1,47 @@
 #include "lcm_trajectory_systems.h"
 
+#include "dairlib/lcmt_timestamped_saved_traj.hpp"
+#include "common/find_resource.h"
+#include <iostream>
+
 namespace dairlib {
 namespace systems {
 
-using drake::trajectories::PiecewisePolynomial;
 using drake::trajectories::PiecewisePolynomial;
 
 LcmTrajectoryReceiver::LcmTrajectoryReceiver(std::string trajectory_name)
     : trajectory_name_(std::move(trajectory_name)) {
-  trajectory_input_port_ = this->DeclareAbstractInputPort("lcmt_saved_traj",
-                                                          drake::Value<dairlib::lcmt_saved_traj>{}).get_index();
+  trajectory_input_port_ =
+      this->DeclareAbstractInputPort(
+              "lcmt_timestamped_saved_traj",
+              drake::Value<dairlib::lcmt_timestamped_saved_traj>{})
+          .get_index();
 
   PiecewisePolynomial<double> traj_inst(Eigen::VectorXd(0));
   this->set_name(trajectory_name_);
-  trajectory_output_port_ = this->DeclareAbstractOutputPort(trajectory_name_, traj_inst,
-                                  &LcmTrajectoryReceiver::OutputTrajectory).get_index();
-
+  trajectory_output_port_ =
+      this->DeclareAbstractOutputPort(trajectory_name_, traj_inst,
+                                      &LcmTrajectoryReceiver::OutputTrajectory)
+          .get_index();
+  lcm_traj_ = LcmTrajectory(dairlib::FindResourceOrThrow(nominal_stand_path_));
 
 }
 
 void LcmTrajectoryReceiver::OutputTrajectory(
-    const drake::systems::Context<double> &context,
-    PiecewisePolynomial<double> *output_trajectory) const {
-  const auto &lcm_traj = this->EvalInputValue<dairlib::lcmt_saved_traj>(
-      context, trajectory_input_port_);
-  auto lcm_trajs = LcmTrajectory(*lcm_traj);
-  const auto trajectory_block =
-      lcm_trajs.GetTrajectory(trajectory_name_);
-
-  *output_trajectory =
-      PiecewisePolynomial<double>::FirstOrderHold(trajectory_block.time_vector,
-                                                  trajectory_block.datapoints);
+    const drake::systems::Context<double>& context,
+    PiecewisePolynomial<double>* output_trajectory) const {
+  if (this->EvalInputValue<dairlib::lcmt_timestamped_saved_traj>(
+      context, trajectory_input_port_)->utime > 1e-3) {
+    const auto& lcm_traj =
+        this->EvalInputValue<dairlib::lcmt_timestamped_saved_traj>(
+            context, trajectory_input_port_);
+    lcm_traj_ = LcmTrajectory(lcm_traj->saved_traj);
+  }
+  const auto trajectory_block = lcm_traj_.GetTrajectory(trajectory_name_);
+
+  *output_trajectory = PiecewisePolynomial<double>::FirstOrderHold(
+      trajectory_block.time_vector, trajectory_block.datapoints);
 }
 
-}
-}
+}  // namespace systems
+}  // namespace dairlib
diff --git a/systems/trajectory_optimization/lcm_trajectory_systems.h b/systems/trajectory_optimization/lcm_trajectory_systems.h
index 0d89ef680a..b079954196 100644
--- a/systems/trajectory_optimization/lcm_trajectory_systems.h
+++ b/systems/trajectory_optimization/lcm_trajectory_systems.h
@@ -32,6 +32,9 @@ class LcmTrajectoryReceiver : public drake::systems::LeafSystem<double> {
   drake::systems::InputPortIndex trajectory_input_port_;
   drake::systems::OutputPortIndex trajectory_output_port_;
   const std::string trajectory_name_;
+
+  mutable LcmTrajectory lcm_traj_;
+  std::string nominal_stand_path_ = "examples/Cassie/saved_trajectories/kcmpc_stand";
 };
 
 }  // namespace systems

From 454e6e34e90e2428709b75801dc662648bfe7e27 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 13 Dec 2022 15:13:34 -0500
Subject: [PATCH 63/76] Working on resolving bouncing

---
 .../kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc      | 4 ++--
 .../cassie_kinematic_centroidal_solver.cc                     | 2 +-
 .../kinematic_centroidal_mpc_gains.yaml                       | 2 +-
 .../kinematic_centroidal_planner/motions/motion_walk.yaml     | 4 ++--
 .../kinematic_centroidal_solver.cc                            | 1 +
 5 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index dea369c1d4..0c9392367e 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -34,7 +34,7 @@ DEFINE_string(channel_reference, "KCMPC_REF",
               "MPC are published");
 DEFINE_string(
     trajectory_parameters,
-    "examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml",
+    "examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml",
     "YAML file that contains trajectory parameters such as speed, tolerance, "
     "target_com_height");
 DEFINE_string(planner_parameters,
@@ -111,7 +111,7 @@ int DoMain(int argc, char* argv[]) {
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
-  for(int i = 3; i <traj_params.n_knot_points ; i++){
+  for(int i = 1; i <traj_params.n_knot_points ; i++){
     complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index c5608e5b4c..5c731eee57 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -228,7 +228,7 @@ drake::VectorX<double> CassieKinematicCentroidalSolver::LiftSlipSolution(int kno
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-  if(knot_point == 3){
+  if(knot_point == 1){
     auto grf_constraint =
         std::make_shared<SlipGrfReductionConstrain>(
             plant_, reducers[knot_point], 2, 4,knot_point);
diff --git a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
index 20b3554563..679e570245 100644
--- a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
@@ -31,6 +31,6 @@ contact_pos: [0.1, 16, 0.001]
 contact_vel: [0.02, 0.02, 0.02]
 contact_force: [0.0001, 0.0001, 0.0001]
 lin_momentum: [0.00001, 0.00001, 0.00001]
-ang_momentum: [0.0000001, 0.0000001, 0.0000001]
+ang_momentum: [0.0000000, 0.0000000, 0.0000000]
 
 tol: 1e-2
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
index 02e3834e1b..e228a971ee 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
@@ -1,9 +1,9 @@
 n_knot_points: 45
 target_com_height: 0.8
 stance_width: 0.25
-swing_foot_minimum_height: 0.03
+swing_foot_minimum_height: 0.015
 duration_scaling:
-  [ 1.0, 1.0, 1.0 ]
+  [ 1.0, 3.0, 1.0 ]
 gait_sequence: [ 'stand', 'walk', 'walk' ]
 com_vel_values:
   [ 0, 0, 0,
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
index 70c73bc0ce..a8e23145ea 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -664,6 +664,7 @@ void KinematicCentroidalSolver::UpdateInitialStateConstraint(
 
 void KinematicCentroidalSolver::SetGains(
     const KinematicCentroidalGains& gains) {
+  gains_ = gains;
   std::map<std::string, int> positions_map =
       dairlib::multibody::MakeNameToPositionsMap(plant_);
   std::map<std::string, int> velocities_map =

From 891c52257c5ab12023ffddf49d952b812de14ff1 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Tue, 13 Dec 2022 16:10:04 -0500
Subject: [PATCH 64/76] Solving much better after some tuning

---
 .../cassie_kcmpc_trajopt.cc                   |  4 +--
 .../cassie_kinematic_centroidal_solver.cc     |  8 ++---
 .../gaits/stand.yaml                          |  2 +-
 .../kinematic_centroidal_mpc_gains.yaml       | 34 +++++++++----------
 .../motions/motion_walk.yaml                  |  4 +--
 .../kinematic_centroidal_solver.cc            |  4 +--
 6 files changed, 28 insertions(+), 28 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index 0c9392367e..6b3159959b 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -104,7 +104,7 @@ int DoMain(int argc, char* argv[]) {
   // Create MPC and set gains
   CassieKinematicCentroidalSolver mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 5000, 10,
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 6000, 80,
       sqrt(pow(traj_params.target_com_height,2) + pow(traj_params.stance_width,2)), traj_params.stance_width);
   mpc.SetGains(gains);
   mpc.SetMinimumFootClearance(traj_params.swing_foot_minimum_height);
@@ -162,7 +162,7 @@ int DoMain(int argc, char* argv[]) {
     options.SetOption(id, "dual_inf_tol", gains.tol);
     options.SetOption(id, "constr_viol_tol", gains.tol);
     options.SetOption(id, "compl_inf_tol", gains.tol);
-    options.SetOption(id, "max_iter", 200);
+    options.SetOption(id, "max_iter", 800);
     options.SetOption(id, "nlp_lower_bound_inf", -1e6);
     options.SetOption(id, "nlp_upper_bound_inf", 1e6);
     options.SetOption(id, "print_timing_statistics", "yes");
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index 5c731eee57..c8b83eb0b7 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -117,10 +117,10 @@ void CassieKinematicCentroidalSolver::AddPlanarSlipCost(int knot_point, double t
         slip_contact_vel_vars(knot_point,1));
   }
   if (force_ref_traj_) {
-    const Eigen::MatrixXd Q_contact_pos =Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2]).asDiagonal();
-    prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_pos,
-                                 Eigen::Vector2d::Zero(2),
-                                 slip_force_vars_[knot_point]);
+//    const Eigen::MatrixXd Q_contact_pos =Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2]).asDiagonal();
+//    prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_pos,
+//                                 Eigen::Vector2d::Zero(2),
+//                                 slip_force_vars_[knot_point]);
   }
 }
 
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
index cc525ff774..18b357a213 100644
--- a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
@@ -2,4 +2,4 @@ gait_pattern:
   - start_phase: 0.0
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 1.0
+period: 0.8
diff --git a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
index 679e570245..4ab2fb335f 100644
--- a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
@@ -7,29 +7,29 @@ generalized_positions:
   base_x: 0
   base_y: 0
   base_z: 0
-  hip_roll_: 0.01
-  hip_yaw_: 0.01
-  hip_pitch_: 0.001
-  knee_: 0.001
-  ankle_joint_: 0.001
-  toe_: 0.0000
+  hip_roll_: 0.001
+  hip_yaw_: 0.001
+  hip_pitch_: 0.00001
+  knee_: 0.00001
+  ankle_joint_: 0.00001
+  toe_: 0.000001
 generalized_velocities:
   base_wx: 0.5
   base_wy: 0.5
   base_wz: 0.5
-  base_vx: 0.1
-  base_vy: 0.1
+  base_vx: 0.2
+  base_vy: 0.2
   base_vz: 0.1
-  hip_roll_: 0.02
-  hip_yaw_: 0.02
-  hip_pitch_: 0.002
-  knee_: 0.002
-  ankle_joint_: 0.002
-  toe_: 0.004
+  hip_roll_: 0.002
+  hip_yaw_: 0.002
+  hip_pitch_: 0.00002
+  knee_: 0.00002
+  ankle_joint_: 0.00002
+  toe_: 0.00004
 
-contact_pos: [0.1, 16, 0.001]
-contact_vel: [0.02, 0.02, 0.02]
-contact_force: [0.0001, 0.0001, 0.0001]
+contact_pos: [0.3, 5, 0.001]
+contact_vel: [0.2, 0.2, 0.2]
+contact_force: [0.00001, 0.00001, 0.00001]
 lin_momentum: [0.00001, 0.00001, 0.00001]
 ang_momentum: [0.0000000, 0.0000000, 0.0000000]
 
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
index e228a971ee..aae8a88b1e 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
@@ -1,11 +1,11 @@
 n_knot_points: 45
 target_com_height: 0.8
 stance_width: 0.25
-swing_foot_minimum_height: 0.015
+swing_foot_minimum_height: 0.02
 duration_scaling:
   [ 1.0, 3.0, 1.0 ]
 gait_sequence: [ 'stand', 'walk', 'walk' ]
 com_vel_values:
   [ 0, 0, 0,
-    0, 0, 0,
+    0.5, 0.0, 0,
     0, 0, 0 ]
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
index a8e23145ea..fad5e70fc5 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -277,7 +277,7 @@ void KinematicCentroidalSolver::SetConstantMomentumReference(
 }
 
 double KinematicCentroidalSolver::GetKnotpointGain(int knot_point) const {
-  const double terminal_gain = is_last_knot(knot_point) ? 100 : 1;
+  const double terminal_gain = is_last_knot(knot_point) ? 50 : 1;
   const double collocation_gain =
       (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
   return terminal_gain * collocation_gain;
@@ -329,7 +329,7 @@ void KinematicCentroidalSolver::AddCosts() {
   }
 }
 
-void KinematicCentroidalSolver::UpdateCosts() {}
+void KinematicCentroidalSolver::UpdateCosts() { DRAKE_DEMAND(false);}
 
 void KinematicCentroidalSolver::SetZeroInitialGuess() {
   Eigen::VectorXd initialGuess =

From f54f8da84be50dd963cdb0f8c999229f9fabe146 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 14 Dec 2022 17:18:39 -0500
Subject: [PATCH 65/76] Fixed dynamics bug

---
 .../cassie_kcmpc_trajopt.cc                       |  2 +-
 .../cassie_kinematic_centroidal_solver.cc         | 10 +++++-----
 .../simple_models/planar_slip_constraints.cc      | 15 +++++++++------
 .../simple_models/planar_slip_constraints.h       | 11 ++++++++---
 4 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index 6b3159959b..6a69b6ffb1 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -104,7 +104,7 @@ int DoMain(int argc, char* argv[]) {
   // Create MPC and set gains
   CassieKinematicCentroidalSolver mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 6000, 80,
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 3000, 80,
       sqrt(pow(traj_params.target_com_height,2) + pow(traj_params.stance_width,2)), traj_params.stance_width);
   mpc.SetGains(gains);
   mpc.SetMinimumFootClearance(traj_params.swing_foot_minimum_height);
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index c8b83eb0b7..2bed5cc30d 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -117,10 +117,10 @@ void CassieKinematicCentroidalSolver::AddPlanarSlipCost(int knot_point, double t
         slip_contact_vel_vars(knot_point,1));
   }
   if (force_ref_traj_) {
-//    const Eigen::MatrixXd Q_contact_pos =Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2]).asDiagonal();
-//    prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_pos,
-//                                 Eigen::Vector2d::Zero(2),
-//                                 slip_force_vars_[knot_point]);
+    const Eigen::MatrixXd Q_contact_pos =Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2]).asDiagonal();
+    prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_pos,
+                                 Eigen::Vector2d::Zero(2),
+                                 slip_force_vars_[knot_point]);
   }
 }
 
@@ -166,7 +166,7 @@ void CassieKinematicCentroidalSolver::AddSlipDynamics(int knot_point) {
   if(!is_last_knot(knot_point)) {
     auto slip_com_dynamics =
         std::make_shared<PlanarSlipDynamicsConstraint<double>>(
-            r0_, k_, b_,m_, 2, slip_contact_sequence_[knot_point], dt_, knot_point);
+            r0_, k_, b_,m_, 2, slip_contact_sequence_[knot_point], slip_contact_sequence_[knot_point+1], dt_, knot_point);
 
     slip_dynamics_binding_.push_back(prog_->AddConstraint(slip_com_dynamics,
                          {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], slip_force_vars_[knot_point],
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
index c224090e6c..88ba037a71 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
@@ -121,7 +121,8 @@ PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
                                                               double b,
                                                               T m,
                                                               int n_feet,
-                                                              std::vector<bool> contact_mask,
+                                                              std::vector<bool> contact_mask0,
+                                                              std::vector<bool> contact_mask1,
                                                               double dt,
                                                               int knot_index):dairlib::solvers::NonlinearConstraint<T>(
     3 + 3, 2 * (3 + 3 + 3 * n_feet + n_feet),
@@ -134,7 +135,8 @@ PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
                                                                               b_(b),
                                                                               m_(m),
                                                                               n_feet_(n_feet),
-                                                                              contact_mask_(std::move(contact_mask)),
+                                                                              contact_mask0_(std::move(contact_mask0)),
+                                                                              contact_mask1_(std::move(contact_mask1)),
                                                                               dt_(dt) {}
 
 
@@ -162,8 +164,8 @@ void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(const Eigen::Ref<const
   const auto& x0 = x.head(6);
   const auto& x1 = x.segment(3 + 3 + n_feet_ * 3 + n_feet_, 6);
 
-  drake::Vector<T, 6> xdot0 = CalcTimeDerivativesWithForce(com0, vel0,contact_pos0, force0);
-  drake::Vector<T, 6> xdot1 = CalcTimeDerivativesWithForce(com1, vel1,contact_pos1, force1);
+  drake::Vector<T, 6> xdot0 = CalcTimeDerivativesWithForce(com0, vel0,contact_pos0, force0, contact_mask0_);
+  drake::Vector<T, 6> xdot1 = CalcTimeDerivativesWithForce(com1, vel1,contact_pos1, force1, contact_mask1_);
 
   // Predict state and return error
   const auto x1Predict = x0 + 0.5 * dt_ * (xdot0 + xdot1);
@@ -174,10 +176,11 @@ template<typename T>
 drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(const drake::VectorX<T> &com_position,
                                                                                 const drake::VectorX<T> &com_vel,
                                                                                 const drake::VectorX<T> &contact_loc,
-                                                                                const drake::VectorX<T> &slip_force) const {
+                                                                                const drake::VectorX<T> &slip_force,
+                                                                                const std::vector<bool> &contact_mask) const {
   drake::Vector3<T> ddcom = {0, 0, -9.81};
   for(int foot = 0; foot < n_feet_; foot ++){
-    if(contact_mask_[foot]){
+    if(contact_mask[foot]){
       drake::Vector3<T> com_rt_foot = com_position - contact_loc.segment(3 * foot, 3);
       const auto r = com_rt_foot.norm();
       const auto unit_vec = com_rt_foot/r;
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
index f5c931ebe7..5828f4c938 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
@@ -65,7 +65,10 @@ template <typename T>
 class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
 
  public:
-  PlanarSlipDynamicsConstraint(double r0, double k, double b, T m, int n_feet, std::vector<bool>  contact_mask, double dt, int knot_index);
+  PlanarSlipDynamicsConstraint(double r0, double k, double b, T m, int n_feet,
+                               std::vector<bool> contact_mask0,
+                               std::vector<bool> contact_mask1,
+                               double dt, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
@@ -75,14 +78,16 @@ class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstrain
       const drake::VectorX<T>& com_position,
       const drake::VectorX<T>& com_vel,
       const drake::VectorX<T>& contact_loc,
-      const drake::VectorX<T> &slip_force) const;
+      const drake::VectorX<T> &slip_force,
+      const std::vector<bool> &contact_mask) const;
 
   const double r0_;
   const double k_;
   const double b_;
   const T m_;
   const int n_feet_;
-  const std::vector<bool> contact_mask_;
+  const std::vector<bool> contact_mask0_;
+  const std::vector<bool> contact_mask1_;
   const double dt_;
 };
 

From acec98c1b9e75f25882e89b82bd71cfac8594b39 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 14 Dec 2022 17:29:37 -0500
Subject: [PATCH 66/76] Experimenting with autoformatter

---
 .../simple_models/planar_slip_constraints.cc  | 289 +++++++++---------
 1 file changed, 151 insertions(+), 138 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
index 88ba037a71..48ccef1e9d 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
@@ -1,25 +1,26 @@
+#include "planar_slip_constraints.h"
+
 #include <iostream>
 #include <utility>
-#include "planar_slip_constraints.h"
-#include "multibody/multibody_utils.h"
 
-PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<double> &plant,
-                                                                std::shared_ptr<PlanarSlipReducer> reducing_function,
-                                                                int n_slip_feet,
-                                                                int n_complex_feet,
-                                                                int knot_index)
-    :dairlib::solvers::NonlinearConstraint<double>(
-    3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet,
-    3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions()
-        + plant.num_velocities(),
-    Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
-    Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
-    "PlanarSlipReductionConstraint[" +
-        std::to_string(knot_index) + "]"),
-     reducing_function_(reducing_function),
-     slip_dim_(3 + 3 + 2 * 3 * n_slip_feet + n_slip_feet),
-     complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()) {}
+#include "multibody/multibody_utils.h"
 
+PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
+    int n_complex_feet, int knot_index)
+    : dairlib::solvers::NonlinearConstraint<double>(
+          3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet,
+          3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet + 6 + 3 +
+              3 * 3 * n_complex_feet + plant.num_positions() +
+              plant.num_velocities(),
+          Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
+          Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
+          "PlanarSlipReductionConstraint[" + std::to_string(knot_index) + "]"),
+      reducing_function_(reducing_function),
+      slip_dim_(3 + 3 + 2 * 3 * n_slip_feet + n_slip_feet),
+      complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
+                   plant.num_velocities()) {}
 
 /// Input is of the form:
 ///     slip_com
@@ -35,64 +36,66 @@ PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(const drake::multib
 ///     complex_contact_force
 ///     complex_gen_pos
 ///     complex_gen_vel
-void PlanarSlipReductionConstraint::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>> &x,
-                                                          drake::VectorX<double> *y) const {
+void PlanarSlipReductionConstraint::EvaluateConstraint(
+    const Eigen::Ref<const drake::VectorX<double>>& x,
+    drake::VectorX<double>* y) const {
   const auto& slip_state = x.head(slip_dim_);
   const auto& complex_state = x.tail(complex_dim_);
   *y = reducing_function_->Reduce(complex_state) - slip_state;
 }
 
-
-SlipGrfReductionConstrain::SlipGrfReductionConstrain(const drake::multibody::MultibodyPlant<double> &plant,
-                                                     std::shared_ptr<PlanarSlipReducer> reducing_function,
-                                                     int n_slip_feet,
-                                                     int n_complex_feet,
-                                                     int knot_index):dairlib::solvers::NonlinearConstraint<double>(
-    n_slip_feet,
-    3 + 3 * n_slip_feet + 3 * n_complex_feet + n_slip_feet + 3,
-    Eigen::VectorXd::Zero(n_slip_feet),
-    Eigen::VectorXd::Zero(n_slip_feet),
-    "SlipGrfReductionConstraint[" +
-        std::to_string(knot_index) + "]"),
-                                                                     reducing_function_(reducing_function),
-                                                                     n_slip_feet_(n_slip_feet),
-                                                                     n_complex_feet_(n_complex_feet) {}
+SlipGrfReductionConstrain::SlipGrfReductionConstrain(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
+    int n_complex_feet, int knot_index)
+    : dairlib::solvers::NonlinearConstraint<double>(
+          n_slip_feet,
+          3 + 3 * n_slip_feet + 3 * n_complex_feet + n_slip_feet + 3,
+          Eigen::VectorXd::Zero(n_slip_feet),
+          Eigen::VectorXd::Zero(n_slip_feet),
+          "SlipGrfReductionConstraint[" + std::to_string(knot_index) + "]"),
+      reducing_function_(reducing_function),
+      n_slip_feet_(n_slip_feet),
+      n_complex_feet_(n_complex_feet) {}
 /// Input is of the form:
 ///     complex_com
 ///     slip com velocity
 ///     slip_contact_pos
 ///     complex_grf
 ///     slip_contact_force
-void SlipGrfReductionConstrain::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>> &x,
-                                                   drake::VectorX<double> *y) const {
+void SlipGrfReductionConstrain::EvaluateConstraint(
+    const Eigen::Ref<const drake::VectorX<double>>& x,
+    drake::VectorX<double>* y) const {
   const auto& complex_com = x.head(3);
-  const auto& slip_vel = x.segment(3,3);
-  const auto& slip_contact_pos = x.segment(3 + 3,3*n_slip_feet_);
-  const auto& complex_grf = x.segment(3 + 3 + 3*n_slip_feet_,3*n_complex_feet_);
-  const auto& slip_contact_force = x.segment(3 + 3 + 3*n_slip_feet_ + 3*n_complex_feet_ ,n_slip_feet_);
-  *y = reducing_function_->ReduceGrf(complex_com, slip_vel, slip_contact_pos, complex_grf) - slip_contact_force;
+  const auto& slip_vel = x.segment(3, 3);
+  const auto& slip_contact_pos = x.segment(3 + 3, 3 * n_slip_feet_);
+  const auto& complex_grf =
+      x.segment(3 + 3 + 3 * n_slip_feet_, 3 * n_complex_feet_);
+  const auto& slip_contact_force =
+      x.segment(3 + 3 + 3 * n_slip_feet_ + 3 * n_complex_feet_, n_slip_feet_);
+  *y = reducing_function_->ReduceGrf(complex_com, slip_vel, slip_contact_pos,
+                                     complex_grf) -
+       slip_contact_force;
 }
 
-PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double> &plant,
-                                                         std::shared_ptr<PlanarSlipLifter> lifting_function,
-                                                         int n_slip_feet,
-                                                         int n_complex_feet,
-                                                         int knot_index)
+PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    std::shared_ptr<PlanarSlipLifter> lifting_function, int n_slip_feet,
+    int n_complex_feet, int knot_index)
     : dairlib::solvers::NonlinearConstraint<double>(
-    6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities(),
-    3 + 3 + 3 * 2 * n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + n_slip_feet
-        + plant.num_velocities(),
-    Eigen::VectorXd::Zero(
-        6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()),
-    Eigen::VectorXd::Zero(
-        6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()),
-    "PlanarSlipLiftingConstraint[" +
-        std::to_string(knot_index) + "]"),
+          6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
+              plant.num_velocities(),
+          3 + 3 + 3 * 2 * n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet +
+              plant.num_positions() + n_slip_feet + plant.num_velocities(),
+          Eigen::VectorXd::Zero(6 + 3 + 3 * 3 * n_complex_feet +
+                                plant.num_positions() + plant.num_velocities()),
+          Eigen::VectorXd::Zero(6 + 3 + 3 * 3 * n_complex_feet +
+                                plant.num_positions() + plant.num_velocities()),
+          "PlanarSlipLiftingConstraint[" + std::to_string(knot_index) + "]"),
       lifting_function_(std::move(lifting_function)),
       slip_dim_(2 + 2 + 2 * 2 * n_slip_feet + n_slip_feet),
-      complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + plant.num_velocities()){
-
-}
+      complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
+                   plant.num_velocities()) {}
 
 /// Input is of the form and should match the lifting function input and output:
 ///     slip_com
@@ -108,37 +111,31 @@ PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(const drake::multibody:
 ///     complex_contact_force
 ///     complex_gen_pos
 ///     complex_gen_vel
-void PlanarSlipLiftingConstraint::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>> &x,
-                                                     drake::VectorX<double> *y) const {
+void PlanarSlipLiftingConstraint::EvaluateConstraint(
+    const Eigen::Ref<const drake::VectorX<double>>& x,
+    drake::VectorX<double>* y) const {
   const auto& slip_state = x.head(slip_dim_);
   const auto& complex_state = x.tail(complex_dim_);
   *y = lifting_function_->Lift(slip_state) - complex_state;
 }
 
-template<typename T>
-PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
-                                                              double k,
-                                                              double b,
-                                                              T m,
-                                                              int n_feet,
-                                                              std::vector<bool> contact_mask0,
-                                                              std::vector<bool> contact_mask1,
-                                                              double dt,
-                                                              int knot_index):dairlib::solvers::NonlinearConstraint<T>(
-    3 + 3, 2 * (3 + 3 + 3 * n_feet + n_feet),
-    Eigen::VectorXd::Zero(6),
-    Eigen::VectorXd::Zero(6),
-    "PlanarSlipDynamicsConstraint[" +
-        std::to_string(knot_index) + "]"),
-                                                                              r0_(r0),
-                                                                              k_(k),
-                                                                              b_(b),
-                                                                              m_(m),
-                                                                              n_feet_(n_feet),
-                                                                              contact_mask0_(std::move(contact_mask0)),
-                                                                              contact_mask1_(std::move(contact_mask1)),
-                                                                              dt_(dt) {}
-
+template <typename T>
+PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(
+    double r0, double k, double b, T m, int n_feet,
+    std::vector<bool> contact_mask0, std::vector<bool> contact_mask1, double dt,
+    int knot_index)
+    : dairlib::solvers::NonlinearConstraint<T>(
+          3 + 3, 2 * (3 + 3 + 3 * n_feet + n_feet), Eigen::VectorXd::Zero(6),
+          Eigen::VectorXd::Zero(6),
+          "PlanarSlipDynamicsConstraint[" + std::to_string(knot_index) + "]"),
+      r0_(r0),
+      k_(k),
+      b_(b),
+      m_(m),
+      n_feet_(n_feet),
+      contact_mask0_(std::move(contact_mask0)),
+      contact_mask1_(std::move(contact_mask1)),
+      dt_(dt) {}
 
 /// The format of the input to the eval() function is in the order
 ///   - com0, center of mass at time k
@@ -149,93 +146,109 @@ PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(double r0,
 ///   - vel1, center of mass velocity at time k+1
 ///   - contact_pos1, active contact positions at time k+1
 ///   - force1, slip force in parallel with spring at time k+1
-template<typename T>
-void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>> &x,
-                                                         drake::VectorX<T> *y) const {
+template <typename T>
+void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(
+    const Eigen::Ref<const drake::VectorX<T>>& x, drake::VectorX<T>* y) const {
   const auto& com0 = x.head(3);
   const auto& vel0 = x.segment(3, 3);
   const auto& contact_pos0 = x.segment(3 + 3, n_feet_ * 3);
   const auto& force0 = x.segment(3 + 3 + n_feet_ * 3, n_feet_);
   const auto& com1 = x.segment(3 + 3 + n_feet_ * 3 + n_feet_, 3);
   const auto& vel1 = x.segment(3 + 3 + n_feet_ * 3 + 3 + n_feet_, 3);
-  const auto& contact_pos1 = x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_, n_feet_ * 3);
-  const auto& force1 = x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_ + n_feet_ * 3, n_feet_);
+  const auto& contact_pos1 =
+      x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_, n_feet_ * 3);
+  const auto& force1 =
+      x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_ + n_feet_ * 3, n_feet_);
 
   const auto& x0 = x.head(6);
   const auto& x1 = x.segment(3 + 3 + n_feet_ * 3 + n_feet_, 6);
 
-  drake::Vector<T, 6> xdot0 = CalcTimeDerivativesWithForce(com0, vel0,contact_pos0, force0, contact_mask0_);
-  drake::Vector<T, 6> xdot1 = CalcTimeDerivativesWithForce(com1, vel1,contact_pos1, force1, contact_mask1_);
+  drake::Vector<T, 6> xdot0 = CalcTimeDerivativesWithForce(
+      com0, vel0, contact_pos0, force0, contact_mask0_);
+  drake::Vector<T, 6> xdot1 = CalcTimeDerivativesWithForce(
+      com1, vel1, contact_pos1, force1, contact_mask1_);
 
   // Predict state and return error
   const auto x1Predict = x0 + 0.5 * dt_ * (xdot0 + xdot1);
   *y = x1 - x1Predict;
 }
 
-template<typename T>
-drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(const drake::VectorX<T> &com_position,
-                                                                                const drake::VectorX<T> &com_vel,
-                                                                                const drake::VectorX<T> &contact_loc,
-                                                                                const drake::VectorX<T> &slip_force,
-                                                                                const std::vector<bool> &contact_mask) const {
+template <typename T>
+drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
+    const drake::VectorX<T>& com_position, const drake::VectorX<T>& com_vel,
+    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};
-  for(int foot = 0; foot < n_feet_; foot ++){
-    if(contact_mask[foot]){
-      drake::Vector3<T> com_rt_foot = com_position - contact_loc.segment(3 * foot, 3);
+  for (int foot = 0; foot < n_feet_; foot++) {
+    if (contact_mask[foot]) {
+      drake::Vector3<T> com_rt_foot =
+          com_position - contact_loc.segment(3 * foot, 3);
       const auto r = com_rt_foot.norm();
-      const auto unit_vec = com_rt_foot/r;
+      const auto unit_vec = com_rt_foot / r;
       const auto dr = com_vel.dot(unit_vec);
       auto F = k_ * (r0_ - r) + slip_force[foot] - b_ * dr;
-      if(F < 0){
+      if (F < 0) {
         F = 0;
       }
       ddcom = ddcom + F * unit_vec / m_;
     }
   }
   drake::Vector6<T> derivative;
-  derivative << com_vel,ddcom;
+  derivative << com_vel, ddcom;
   return derivative;
 }
 
-QuadraticLiftedCost::QuadraticLiftedCost(std::shared_ptr<PlanarSlipLifter> lifting_function,
-                                            QuadraticLiftedCost::cost_element com_cost,
-                                            QuadraticLiftedCost::cost_element momentum_cost,
-                                            QuadraticLiftedCost::cost_element contact_cost,
-                                            QuadraticLiftedCost::cost_element grf_cost,
-                                            QuadraticLiftedCost::cost_element q_cost,
-                                            QuadraticLiftedCost::cost_element v_cost,
-                                            double terminal_gain,
-                                            int n_slip_feet,
-                                            int knot_point):dairlib::solvers::NonlinearCost<double>(
-    3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet, "LiftedCost[" +
-        std::to_string(knot_point) + "]"),
-                                                            lifting_function_(std::move(lifting_function)),
-                                                            com_cost_(std::move(com_cost)),
-                                                            momentum_cost_(std::move(momentum_cost)),
-                                                            contact_cost_(std::move(contact_cost)),
-                                                            grf_cost_(std::move(grf_cost)),
-                                                            q_cost_(std::move(q_cost)),
-                                                            v_cost_(std::move(v_cost)),
-                                                            terminal_gain_(terminal_gain) {}
-
-void QuadraticLiftedCost::EvaluateCost(const Eigen::Ref<const drake::VectorX<double>> &x, drake::VectorX<double> *y) const {
+QuadraticLiftedCost::QuadraticLiftedCost(
+    std::shared_ptr<PlanarSlipLifter> lifting_function,
+    QuadraticLiftedCost::cost_element com_cost,
+    QuadraticLiftedCost::cost_element momentum_cost,
+    QuadraticLiftedCost::cost_element contact_cost,
+    QuadraticLiftedCost::cost_element grf_cost,
+    QuadraticLiftedCost::cost_element q_cost,
+    QuadraticLiftedCost::cost_element v_cost, double terminal_gain,
+    int n_slip_feet, int knot_point)
+    : dairlib::solvers::NonlinearCost<double>(
+          3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet,
+          "LiftedCost[" + std::to_string(knot_point) + "]"),
+      lifting_function_(std::move(lifting_function)),
+      com_cost_(std::move(com_cost)),
+      momentum_cost_(std::move(momentum_cost)),
+      contact_cost_(std::move(contact_cost)),
+      grf_cost_(std::move(grf_cost)),
+      q_cost_(std::move(q_cost)),
+      v_cost_(std::move(v_cost)),
+      terminal_gain_(terminal_gain) {}
+
+void QuadraticLiftedCost::EvaluateCost(
+    const Eigen::Ref<const drake::VectorX<double>>& x,
+    drake::VectorX<double>* y) const {
   const auto lifted_state = lifting_function_->Lift(x);
 
   const auto& com = lifted_state.head(3);
-  const auto& momentum = lifted_state.segment(3,6);
-  const auto& contact_info = lifted_state.segment(3 + 6, contact_cost_.ref.size());
-  const auto& grf = lifted_state.segment(3 + 6 + contact_cost_.ref.size(), grf_cost_.ref.size());
-  const auto& q = lifted_state.segment(3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size(), q_cost_.ref.size());
-  const auto& v = lifted_state.segment(3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size() + q_cost_.ref.size(), v_cost_.ref.size());
-
-  *y = CalcCost(com_cost_, com) + CalcCost(momentum_cost_, momentum) + CalcCost(contact_cost_, contact_info)
-      + CalcCost(grf_cost_, grf) + CalcCost(q_cost_, q) + CalcCost(v_cost_, v);
+  const auto& momentum = lifted_state.segment(3, 6);
+  const auto& contact_info =
+      lifted_state.segment(3 + 6, contact_cost_.ref.size());
+  const auto& grf = lifted_state.segment(3 + 6 + contact_cost_.ref.size(),
+                                         grf_cost_.ref.size());
+  const auto& q = lifted_state.segment(
+      3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size(),
+      q_cost_.ref.size());
+  const auto& v =
+      lifted_state.segment(3 + 6 + contact_cost_.ref.size() +
+                               grf_cost_.ref.size() + q_cost_.ref.size(),
+                           v_cost_.ref.size());
+
+  *y = CalcCost(com_cost_, com) + CalcCost(momentum_cost_, momentum) +
+       CalcCost(contact_cost_, contact_info) + CalcCost(grf_cost_, grf) +
+       CalcCost(q_cost_, q) + CalcCost(v_cost_, v);
 }
 
-Eigen::Matrix<double, -1, 1, 0> QuadraticLiftedCost::CalcCost(const QuadraticLiftedCost::cost_element &cost,
-                                                                    const Eigen::Ref<const drake::VectorX<double>> &x)const {
-  auto error = x-cost.ref;
+Eigen::Matrix<double, -1, 1, 0> QuadraticLiftedCost::CalcCost(
+    const QuadraticLiftedCost::cost_element& cost,
+    const Eigen::Ref<const drake::VectorX<double>>& x) const {
+  auto error = x - cost.ref;
   return terminal_gain_ * error.transpose() * cost.Q * error;
 }
 
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS (class PlanarSlipDynamicsConstraint);
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class PlanarSlipDynamicsConstraint);

From 83c4c4bf7196fc4c09f7d77a45a8e78e26cdf415 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 14 Dec 2022 17:42:12 -0500
Subject: [PATCH 67/76] Ran autoformatter

---
 .../cassie_kcmpc_trajopt.cc                   |  23 +-
 .../cassie_kinematic_centroidal_solver.cc     | 282 +++++++++-------
 .../cassie_kinematic_centroidal_solver.h      | 119 +++----
 .../simple_models/planar_slip_constraints.h   |  92 +++---
 .../simple_models/planar_slip_lifter.cc       | 305 +++++++++++-------
 .../simple_models/planar_slip_lifter.h        |  94 +++---
 .../simple_models/planar_slip_lifting_test.cc | 131 ++++----
 .../simple_models/planar_slip_reducer.cc      | 115 ++++---
 .../simple_models/planar_slip_reducer.h       |  59 ++--
 multibody/kinematic/world_point_evaluator.h   |   4 +-
 .../kcmpc_reference_generator.cc              |   5 +-
 .../kinematic_centroidal_solver.cc            |  44 +--
 .../kinematic_centroidal_solver.h             |  44 ++-
 .../reference_generation_utils.cc             |  18 +-
 .../reference_generation_utils.h              | 105 +++---
 15 files changed, 809 insertions(+), 631 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index 6a69b6ffb1..c057348639 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -1,5 +1,7 @@
+#include <unistd.h>
+
 #include <iostream>
-#include<unistd.h>
+
 #include <drake/common/yaml/yaml_io.h>
 #include <drake/geometry/drake_visualizer.h>
 #include <drake/geometry/meshcat_visualizer.h>
@@ -91,7 +93,7 @@ int DoMain(int argc, char* argv[]) {
   gait_library["jump"] = jump;
   // Create reference
   std::vector<Gait> gait_samples;
-  for (auto gait : traj_params.gait_sequence){
+  for (auto gait : traj_params.gait_sequence) {
     gait_samples.push_back(gait_library.at(gait));
   }
   DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size());
@@ -104,19 +106,22 @@ int DoMain(int argc, char* argv[]) {
   // Create MPC and set gains
   CassieKinematicCentroidalSolver mpc(
       plant, traj_params.n_knot_points,
-      time_points.back() / (traj_params.n_knot_points - 1), 0.4, reference_state.head(plant.num_positions()), 3000, 80,
-      sqrt(pow(traj_params.target_com_height,2) + pow(traj_params.stance_width,2)), traj_params.stance_width);
+      time_points.back() / (traj_params.n_knot_points - 1), 0.4,
+      reference_state.head(plant.num_positions()), 3000, 80,
+      sqrt(pow(traj_params.target_com_height, 2) +
+           pow(traj_params.stance_width, 2)),
+      traj_params.stance_width);
   mpc.SetGains(gains);
   mpc.SetMinimumFootClearance(traj_params.swing_foot_minimum_height);
 
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
-  std::fill(complexity_schedule.begin(), complexity_schedule.end(),Complexity::KINEMATIC_CENTROIDAL);
-  for(int i = 1; i <traj_params.n_knot_points ; i++){
+  std::fill(complexity_schedule.begin(), complexity_schedule.end(),
+            Complexity::KINEMATIC_CENTROIDAL);
+  for (int i = 1; i < traj_params.n_knot_points; i++) {
     complexity_schedule[i] = Complexity::PLANAR_SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
 
-
   KcmpcReferenceGenerator generator(plant, plant_context.get(),
                                     CreateContactPoints(plant, 0));
 
@@ -225,6 +230,4 @@ int DoMain(int argc, char* argv[]) {
   }
 }
 
-int main(int argc, char* argv[]) {
-  DoMain(argc, argv);
-}
+int main(int argc, char* argv[]) { DoMain(argc, argv); }
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index 2bed5cc30d..432d325271 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -1,11 +1,14 @@
 #include "cassie_kinematic_centroidal_solver.h"
-#include "drake/multibody/parsing/parser.h"
+
 #include "common/find_resource.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h"
 
-std::vector<dairlib::multibody::WorldPointEvaluator<double>> CassieKinematicCentroidalSolver::CreateSlipContactPoints(const drake::multibody::MultibodyPlant<
-    double> &plant, double mu) {
+#include "drake/multibody/parsing/parser.h"
+
+std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+CassieKinematicCentroidalSolver::CreateSlipContactPoints(
+    const drake::multibody::MultibodyPlant<double>& plant, double mu) {
   auto left_toe_pair = dairlib::LeftToeFront(plant);
   auto right_toe_pair = dairlib::RightToeFront(plant);
   std::vector<int> active_inds{0, 1, 2};
@@ -30,31 +33,33 @@ void CassieKinematicCentroidalSolver::AddLoopClosure() {
   loop_closure_evaluators.add_evaluator(&r_loop_evaluator_);
   auto loop_closure =
       std::make_shared<dairlib::multibody::KinematicPositionConstraint<double>>(
-          Plant(),
-          loop_closure_evaluators,
-          Eigen::VectorXd::Zero(2),
+          Plant(), loop_closure_evaluators, Eigen::VectorXd::Zero(2),
           Eigen::VectorXd::Zero(2));
   for (int knot_point = 1; knot_point < num_knot_points(); knot_point++) {
-    AddKinematicConstraint(loop_closure, state_vars(knot_point).head(Plant().num_positions()));
+    AddKinematicConstraint(
+        loop_closure, state_vars(knot_point).head(Plant().num_positions()));
   }
-
 }
 
 void CassieKinematicCentroidalSolver::AddPlanarSlipConstraints(int knot_point) {
-  for(int contact_index = 0; contact_index < slip_contact_points_.size(); contact_index ++){
+  for (int contact_index = 0; contact_index < slip_contact_points_.size();
+       contact_index++) {
+    prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) -
+                          slip_com_vars_[knot_point])
+                             .squaredNorm() <= 1.5);
 
-    prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - slip_com_vars_[knot_point]).squaredNorm() <= 1.5);
-
-    if(slip_contact_sequence_[knot_point][contact_index]){
+    if (slip_contact_sequence_[knot_point][contact_index]) {
       // Foot isn't moving
       prog_->AddBoundingBoxConstraint(
           Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3),
           slip_contact_vel_vars(knot_point, contact_index));
       // Foot is on the ground
       prog_->AddBoundingBoxConstraint(
-          slip_ground_offset_, slip_ground_offset_, slip_contact_pos_vars(knot_point, contact_index)[2]);
-    }else{
-      prog_->AddBoundingBoxConstraint(0, 0, slip_force_vars_[knot_point][contact_index]);
+          slip_ground_offset_, slip_ground_offset_,
+          slip_contact_pos_vars(knot_point, contact_index)[2]);
+    } else {
+      prog_->AddBoundingBoxConstraint(
+          0, 0, slip_force_vars_[knot_point][contact_index]);
       // Feet are above the ground
       double lb = 0;
       // Check if at least one of the time points before or after is also in
@@ -62,17 +67,18 @@ void CassieKinematicCentroidalSolver::AddPlanarSlipConstraints(int knot_point) {
       // constraining the optimization problem
       if (!is_first_knot(knot_point) and !is_last_knot(knot_point) and
           (!slip_contact_sequence_[knot_point - 1][contact_index] or
-              !slip_contact_sequence_[knot_point + 1][contact_index])) {
+           !slip_contact_sequence_[knot_point + 1][contact_index])) {
         lb = swing_foot_minimum_height_;
       }
-      lb +=slip_ground_offset_;
+      lb += slip_ground_offset_;
       prog_->AddBoundingBoxConstraint(
           lb, 0.5, slip_contact_pos_vars(knot_point, contact_index)[2]);
     }
   }
 }
 
-void CassieKinematicCentroidalSolver::AddPlanarSlipCost(int knot_point, double terminal_gain) {
+void CassieKinematicCentroidalSolver::AddPlanarSlipCost(int knot_point,
+                                                        double terminal_gain) {
   const double t = dt_ * knot_point;
   if (com_ref_traj_) {
     prog_->AddQuadraticErrorCost(terminal_gain * Q_com_,
@@ -81,116 +87,141 @@ void CassieKinematicCentroidalSolver::AddPlanarSlipCost(int knot_point, double t
   }
 
   // Project linear momentum
-  if (mom_ref_traj_){
-    const Eigen::MatrixXd Q_vel =gains_.lin_momentum.asDiagonal() * m_;
-    prog_->AddQuadraticErrorCost(terminal_gain * Q_vel,
-                                 Eigen::VectorXd(mom_ref_traj_->value(t)).tail(3),
-                                 slip_vel_vars_[knot_point]);
+  if (mom_ref_traj_) {
+    const Eigen::MatrixXd Q_vel = gains_.lin_momentum.asDiagonal() * m_;
+    prog_->AddQuadraticErrorCost(
+        terminal_gain * Q_vel, Eigen::VectorXd(mom_ref_traj_->value(t)).tail(3),
+        slip_vel_vars_[knot_point]);
   }
   // Project com velocity
-  if(v_ref_traj_){
-    const Eigen::MatrixXd Q_vel =Q_v_.block(3,3,3,3);
-    prog_->AddQuadraticErrorCost(terminal_gain * Q_vel,
-                                 Eigen::VectorXd(v_ref_traj_->value(t)).segment(4,3),
-                                 slip_vel_vars_[knot_point]);
+  if (v_ref_traj_) {
+    const Eigen::MatrixXd Q_vel = Q_v_.block(3, 3, 3, 3);
+    prog_->AddQuadraticErrorCost(
+        terminal_gain * Q_vel,
+        Eigen::VectorXd(v_ref_traj_->value(t)).segment(4, 3),
+        slip_vel_vars_[knot_point]);
   }
 
   if (contact_ref_traj_) {
-    const Eigen::MatrixXd Q_contact_pos =gains_.contact_pos.asDiagonal();
+    const Eigen::MatrixXd Q_contact_pos = gains_.contact_pos.asDiagonal();
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_contact_pos,
-        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(0,3),
-        slip_contact_pos_vars(knot_point,0));
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(0, 3),
+        slip_contact_pos_vars(knot_point, 0));
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_contact_pos,
-        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(6,3),
-        slip_contact_pos_vars(knot_point,1));
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(6, 3),
+        slip_contact_pos_vars(knot_point, 1));
 
-    const Eigen::MatrixXd Q_contact_vel =gains_.contact_vel.asDiagonal();
+    const Eigen::MatrixXd Q_contact_vel = gains_.contact_vel.asDiagonal();
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_contact_vel,
-        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(12,3),
-        slip_contact_vel_vars(knot_point,0));
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(12, 3),
+        slip_contact_vel_vars(knot_point, 0));
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_contact_vel,
-        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(18,3),
-        slip_contact_vel_vars(knot_point,1));
+        Eigen::VectorXd(contact_ref_traj_->value(t)).segment(18, 3),
+        slip_contact_vel_vars(knot_point, 1));
   }
   if (force_ref_traj_) {
-    const Eigen::MatrixXd Q_contact_pos =Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2]).asDiagonal();
+    const Eigen::MatrixXd Q_contact_pos =
+        Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2])
+            .asDiagonal();
     prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_pos,
                                  Eigen::Vector2d::Zero(2),
                                  slip_force_vars_[knot_point]);
   }
 }
 
-void CassieKinematicCentroidalSolver::SetModeSequence(const std::vector<std::vector<bool>> &contact_sequence) {
+void CassieKinematicCentroidalSolver::SetModeSequence(
+    const std::vector<std::vector<bool>>& contact_sequence) {
   KinematicCentroidalSolver::SetModeSequence(contact_sequence);
   MapModeSequence();
 }
-void CassieKinematicCentroidalSolver::SetModeSequence(const drake::trajectories::PiecewisePolynomial<double> &contact_sequence) {
+void CassieKinematicCentroidalSolver::SetModeSequence(
+    const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) {
   KinematicCentroidalSolver::SetModeSequence(contact_sequence);
   MapModeSequence();
 }
 
 void CassieKinematicCentroidalSolver::MapModeSequence() {
   for (int knot_point = 0; knot_point < num_knot_points(); knot_point++) {
-    slip_contact_sequence_[knot_point] = complex_mode_to_slip_mode_.at(contact_sequence_[knot_point]);
+    slip_contact_sequence_[knot_point] =
+        complex_mode_to_slip_mode_.at(contact_sequence_[knot_point]);
   }
 }
-void CassieKinematicCentroidalSolver::AddSlipReductionConstraint(int knot_point) {
+void CassieKinematicCentroidalSolver::AddSlipReductionConstraint(
+    int knot_point) {
   prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point));
-  prog_->AddConstraint(slip_vel_vars_[knot_point] * m_ == momentum_vars(knot_point).tail(3));
-  prog_->AddConstraint(slip_contact_pos_vars(knot_point,0) == contact_pos_vars(knot_point,0));
-  prog_->AddConstraint(slip_contact_pos_vars(knot_point,1) == contact_pos_vars(knot_point,2));
-  prog_->AddConstraint(slip_contact_vel_vars(knot_point,0) == contact_vel_vars(knot_point,0));
-  prog_->AddConstraint(slip_contact_vel_vars(knot_point,1) == contact_vel_vars(knot_point,2));
-  auto grf_constraint =
-    std::make_shared<SlipGrfReductionConstrain>(
-        plant_, reducers[knot_point], 2, 4,knot_point);
-  prog_->AddConstraint(grf_constraint,
-                       {com_pos_vars(knot_point), slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], contact_force_[knot_point], slip_force_vars_[knot_point]});
+  prog_->AddConstraint(slip_vel_vars_[knot_point] * m_ ==
+                       momentum_vars(knot_point).tail(3));
+  prog_->AddConstraint(slip_contact_pos_vars(knot_point, 0) ==
+                       contact_pos_vars(knot_point, 0));
+  prog_->AddConstraint(slip_contact_pos_vars(knot_point, 1) ==
+                       contact_pos_vars(knot_point, 2));
+  prog_->AddConstraint(slip_contact_vel_vars(knot_point, 0) ==
+                       contact_vel_vars(knot_point, 0));
+  prog_->AddConstraint(slip_contact_vel_vars(knot_point, 1) ==
+                       contact_vel_vars(knot_point, 2));
+  auto grf_constraint = std::make_shared<SlipGrfReductionConstrain>(
+      plant_, reducers[knot_point], 2, 4, knot_point);
+  prog_->AddConstraint(
+      grf_constraint,
+      {com_pos_vars(knot_point), slip_vel_vars_[knot_point],
+       slip_contact_pos_vars_[knot_point], contact_force_[knot_point],
+       slip_force_vars_[knot_point]});
 }
 
 void CassieKinematicCentroidalSolver::AddSlipLiftingConstraint(int knot_point) {
-  auto lifting_constraint =
-      std::make_shared<PlanarSlipLiftingConstraint>(
-          plant_, lifters_[knot_point], 2, n_contact_points_,knot_point);
-  prog_->AddConstraint(lifting_constraint,
-                       {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point],slip_contact_vel_vars_[knot_point],slip_force_vars_[knot_point],
-                        com_pos_vars(knot_point), momentum_vars(knot_point), contact_pos_[knot_point], contact_vel_[knot_point], contact_force_[knot_point],
-                        state_vars(knot_point)});
+  auto lifting_constraint = std::make_shared<PlanarSlipLiftingConstraint>(
+      plant_, lifters_[knot_point], 2, n_contact_points_, knot_point);
+  prog_->AddConstraint(
+      lifting_constraint,
+      {slip_com_vars_[knot_point], slip_vel_vars_[knot_point],
+       slip_contact_pos_vars_[knot_point], slip_contact_vel_vars_[knot_point],
+       slip_force_vars_[knot_point], com_pos_vars(knot_point),
+       momentum_vars(knot_point), contact_pos_[knot_point],
+       contact_vel_[knot_point], contact_force_[knot_point],
+       state_vars(knot_point)});
 }
 
 void CassieKinematicCentroidalSolver::AddSlipDynamics(int knot_point) {
-  if(!is_last_knot(knot_point)) {
+  if (!is_last_knot(knot_point)) {
     auto slip_com_dynamics =
         std::make_shared<PlanarSlipDynamicsConstraint<double>>(
-            r0_, k_, b_,m_, 2, slip_contact_sequence_[knot_point], slip_contact_sequence_[knot_point+1], dt_, knot_point);
-
-    slip_dynamics_binding_.push_back(prog_->AddConstraint(slip_com_dynamics,
-                         {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], slip_contact_pos_vars_[knot_point], slip_force_vars_[knot_point],
-                          slip_com_vars_[knot_point+1], slip_vel_vars_[knot_point+1], slip_contact_pos_vars_[knot_point+1],slip_force_vars_[knot_point+1]}));
-    prog_->AddConstraint(
-        slip_contact_pos_vars_[knot_point + 1] ==
-            slip_contact_pos_vars_[knot_point] +
-                0.5 * dt_ *
-                    (slip_contact_vel_vars_[knot_point] +
-                        slip_contact_vel_vars_[knot_point + 1]));
+            r0_, k_, b_, m_, 2, slip_contact_sequence_[knot_point],
+            slip_contact_sequence_[knot_point + 1], dt_, knot_point);
 
+    slip_dynamics_binding_.push_back(prog_->AddConstraint(
+        slip_com_dynamics,
+        {slip_com_vars_[knot_point], slip_vel_vars_[knot_point],
+         slip_contact_pos_vars_[knot_point], slip_force_vars_[knot_point],
+         slip_com_vars_[knot_point + 1], slip_vel_vars_[knot_point + 1],
+         slip_contact_pos_vars_[knot_point + 1],
+         slip_force_vars_[knot_point + 1]}));
+    prog_->AddConstraint(slip_contact_pos_vars_[knot_point + 1] ==
+                         slip_contact_pos_vars_[knot_point] +
+                             0.5 * dt_ *
+                                 (slip_contact_vel_vars_[knot_point] +
+                                  slip_contact_vel_vars_[knot_point + 1]));
   }
 }
 
-drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalSolver::slip_contact_pos_vars(int knot_point_index,
-                                                                                               int slip_foot_index) {
-  return slip_contact_pos_vars_[knot_point_index].segment(3 * slip_foot_index, 3);
+drake::solvers::VectorXDecisionVariable
+CassieKinematicCentroidalSolver::slip_contact_pos_vars(int knot_point_index,
+                                                       int slip_foot_index) {
+  return slip_contact_pos_vars_[knot_point_index].segment(3 * slip_foot_index,
+                                                          3);
 }
-drake::solvers::VectorXDecisionVariable CassieKinematicCentroidalSolver::slip_contact_vel_vars(int knot_point_index,
-                                                                                               int slip_foot_index) {
-  return slip_contact_vel_vars_[knot_point_index].segment(3 * slip_foot_index, 3);
+drake::solvers::VectorXDecisionVariable
+CassieKinematicCentroidalSolver::slip_contact_vel_vars(int knot_point_index,
+                                                       int slip_foot_index) {
+  return slip_contact_vel_vars_[knot_point_index].segment(3 * slip_foot_index,
+                                                          3);
 }
 
-void CassieKinematicCentroidalSolver::SetComPositionGuess(const drake::trajectories::PiecewisePolynomial<double> &com_trajectory) {
+void CassieKinematicCentroidalSolver::SetComPositionGuess(
+    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],
                            com_trajectory.value(dt_ * knot_point));
@@ -198,40 +229,52 @@ void CassieKinematicCentroidalSolver::SetComPositionGuess(const drake::trajector
   KinematicCentroidalSolver::SetComPositionGuess(com_trajectory);
 }
 
-void CassieKinematicCentroidalSolver::SetRobotStateGuess(const drake::trajectories::PiecewisePolynomial<double> &q_traj,
-                                                         const drake::trajectories::PiecewisePolynomial<double> &v_traj) {
+void CassieKinematicCentroidalSolver::SetRobotStateGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& q_traj,
+    const drake::trajectories::PiecewisePolynomial<double>& v_traj) {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    dairlib::multibody::SetPositionsIfNew<double>(plant_, q_traj.value(dt_*knot_point),contexts_[knot_point].get());
-    dairlib::multibody::SetVelocitiesIfNew<double>(plant_, v_traj.value(dt_*knot_point),contexts_[knot_point].get());
-    for(int contact = 0; contact < slip_contact_points_.size(); contact++){
-      prog_->SetInitialGuess(slip_contact_pos_vars(knot_point, contact),
-                             slip_contact_points_[contact].EvalFull(*contexts_[knot_point]));
-      prog_->SetInitialGuess(slip_contact_vel_vars(knot_point, contact),
-                             slip_contact_points_[contact].EvalFullTimeDerivative(*contexts_[knot_point]));
+    dairlib::multibody::SetPositionsIfNew<double>(
+        plant_, q_traj.value(dt_ * knot_point), contexts_[knot_point].get());
+    dairlib::multibody::SetVelocitiesIfNew<double>(
+        plant_, v_traj.value(dt_ * knot_point), contexts_[knot_point].get());
+    for (int contact = 0; contact < slip_contact_points_.size(); contact++) {
+      prog_->SetInitialGuess(
+          slip_contact_pos_vars(knot_point, contact),
+          slip_contact_points_[contact].EvalFull(*contexts_[knot_point]));
+      prog_->SetInitialGuess(
+          slip_contact_vel_vars(knot_point, contact),
+          slip_contact_points_[contact].EvalFullTimeDerivative(
+              *contexts_[knot_point]));
     }
   }
   KinematicCentroidalSolver::SetRobotStateGuess(q_traj, v_traj);
 }
 
-void CassieKinematicCentroidalSolver::SetMomentumGuess(const drake::trajectories::PiecewisePolynomial<double> &momentum_trajectory) {
+void CassieKinematicCentroidalSolver::SetMomentumGuess(
+    const drake::trajectories::PiecewisePolynomial<double>&
+        momentum_trajectory) {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    prog_->SetInitialGuess(slip_vel_vars_[knot_point],
-                           drake::VectorX<double>(momentum_trajectory.value(dt_ * knot_point)).tail(3)/m_);
+    prog_->SetInitialGuess(
+        slip_vel_vars_[knot_point],
+        drake::VectorX<double>(momentum_trajectory.value(dt_ * knot_point))
+                .tail(3) /
+            m_);
   }
   KinematicCentroidalSolver::SetMomentumGuess(momentum_trajectory);
 }
 
-drake::VectorX<double> CassieKinematicCentroidalSolver::LiftSlipSolution(int knot_point) {
-  drake::VectorX<double> slip_state(3 + 3 + 6 * slip_contact_points_.size() + slip_contact_points_.size());
-  slip_state<<result_->GetSolution(slip_com_vars_[knot_point]),
+drake::VectorX<double> CassieKinematicCentroidalSolver::LiftSlipSolution(
+    int knot_point) {
+  drake::VectorX<double> slip_state(3 + 3 + 6 * slip_contact_points_.size() +
+                                    slip_contact_points_.size());
+  slip_state << result_->GetSolution(slip_com_vars_[knot_point]),
       result_->GetSolution(slip_vel_vars_[knot_point]),
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-  if(knot_point == 1){
-    auto grf_constraint =
-        std::make_shared<SlipGrfReductionConstrain>(
-            plant_, reducers[knot_point], 2, 4,knot_point);
+  if (knot_point == 1) {
+    auto grf_constraint = std::make_shared<SlipGrfReductionConstrain>(
+        plant_, reducers[knot_point], 2, 4, knot_point);
     drake::VectorX<double> grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3);
     drake::VectorX<double> grf_error(2);
     grf_input << result_->GetSolution(com_pos_vars(knot_point)),
@@ -240,37 +283,32 @@ drake::VectorX<double> CassieKinematicCentroidalSolver::LiftSlipSolution(int kno
         result_->GetSolution(contact_force_[knot_point]),
         result_->GetSolution(slip_force_vars_[knot_point]);
     grf_constraint->Eval(grf_input, &grf_error);
-    std::cout<<"Grf error"<<std::endl;
-    std::cout<<grf_error<<std::endl;
+    std::cout << "Grf error" << std::endl;
+    std::cout << grf_error << std::endl;
   }
-  return lifters_[knot_point]->Lift(slip_state).tail(n_q_+n_v_);
+  return lifters_[knot_point]->Lift(slip_state).tail(n_q_ + n_v_);
 }
 
-void CassieKinematicCentroidalSolver::SetForceGuess(const drake::trajectories::PiecewisePolynomial<double> &force_trajectory) {
-  for(const auto& force_vars : slip_force_vars_){
-    //TODO find better initial guess
-    prog_->SetInitialGuess(force_vars, 0 * drake::VectorX<double>::Ones(force_vars.size()));
+void CassieKinematicCentroidalSolver::SetForceGuess(
+    const drake::trajectories::PiecewisePolynomial<double>& force_trajectory) {
+  for (const auto& force_vars : slip_force_vars_) {
+    // TODO find better initial guess
+    prog_->SetInitialGuess(force_vars,
+                           0 * drake::VectorX<double>::Ones(force_vars.size()));
   }
   KinematicCentroidalSolver::SetForceGuess(force_trajectory);
 }
-void CassieKinematicCentroidalSolver::Build(const drake::solvers::SolverOptions &solver_options) {
+void CassieKinematicCentroidalSolver::Build(
+    const drake::solvers::SolverOptions& solver_options) {
   for (int knot = 0; knot < n_knot_points_; knot++) {
-    lifters_.emplace_back(new PlanarSlipLifter(plant_,
-                                               contexts_[knot].get(),
-                                               slip_contact_points_,
-                                               CreateContactPoints(plant_, 0),
-                                               {{0, {0, 1}}, {1, {2, 3}}},
-                                               nominal_stand_,
-                                               k_,
-                                               b_,
-                                               r0_, slip_contact_sequence_[knot]));
-    reducers.emplace_back(new PlanarSlipReducer(plant_,
-                                                contexts_[knot].get(),
-                                                slip_contact_points_,
-                                                CreateContactPoints(plant_, 0),
-                                                {{0, {0, 1}}, {1, {2, 3}}},
-                                                k_,
-                                                b_, r0_, slip_contact_sequence_[knot]));
+    lifters_.emplace_back(new PlanarSlipLifter(
+        plant_, contexts_[knot].get(), slip_contact_points_,
+        CreateContactPoints(plant_, 0), {{0, {0, 1}}, {1, {2, 3}}},
+        nominal_stand_, k_, b_, r0_, slip_contact_sequence_[knot]));
+    reducers.emplace_back(new PlanarSlipReducer(
+        plant_, contexts_[knot].get(), slip_contact_points_,
+        CreateContactPoints(plant_, 0), {{0, {0, 1}}, {1, {2, 3}}}, k_, b_, r0_,
+        slip_contact_sequence_[knot]));
   }
   KinematicCentroidalSolver::Build(solver_options);
 }
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index 36bb5698ab..503a42249b 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -1,94 +1,102 @@
 #pragma once
 #include <iostream>
-#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h"
-#include "drake/multibody/plant/multibody_plant.h"
+
 #include "examples/Cassie/cassie_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h"
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h"
 
+#include "drake/multibody/plant/multibody_plant.h"
+
 /*!
- * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop closure, joint limits, and cassie contact points
+ * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop
+ * closure, joint limits, and cassie contact points
  */
 class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
  public:
-  CassieKinematicCentroidalSolver(const drake::multibody::MultibodyPlant<double>& plant,
-                                  int n_knot_points,
-                                  double dt,
-                                  double mu,
-                                  const drake::VectorX<double>& nominal_stand,
-                                  double k = 1000,
-                                  double b = 20,
-                                  double r0 = 0.5,
-                                  double stance_width = 0.2) :
-      KinematicCentroidalSolver(plant, n_knot_points, dt, CreateContactPoints(plant, mu)),
-      l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
-      r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
-      loop_closure_evaluators(Plant()),
-      slip_contact_sequence_(n_knot_points),
-      k_(k),
-      r0_(r0),
-      b_(b),
-      nominal_stand_(nominal_stand){
+  CassieKinematicCentroidalSolver(
+      const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points,
+      double dt, double mu, const drake::VectorX<double>& nominal_stand,
+      double k = 1000, double b = 20, double r0 = 0.5,
+      double stance_width = 0.2)
+      : KinematicCentroidalSolver(plant, n_knot_points, dt,
+                                  CreateContactPoints(plant, mu)),
+        l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
+        r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
+        loop_closure_evaluators(Plant()),
+        slip_contact_sequence_(n_knot_points),
+        k_(k),
+        r0_(r0),
+        b_(b),
+        nominal_stand_(nominal_stand) {
     AddPlantJointLimits(dairlib::JointNames());
     AddLoopClosure();
 
-    slip_contact_points_ = CreateSlipContactPoints(plant,mu);
+    slip_contact_points_ = CreateSlipContactPoints(plant, mu);
     for (int knot = 0; knot < n_knot_points; knot++) {
-      slip_com_vars_.push_back(prog_->NewContinuousVariables(
-          3, "slip_com_" + std::to_string(knot)));
-      slip_vel_vars_.push_back(prog_->NewContinuousVariables(
-          3, "slip_vel_" + std::to_string(knot)));
+      slip_com_vars_.push_back(
+          prog_->NewContinuousVariables(3, "slip_com_" + std::to_string(knot)));
+      slip_vel_vars_.push_back(
+          prog_->NewContinuousVariables(3, "slip_vel_" + std::to_string(knot)));
       slip_contact_pos_vars_.push_back(prog_->NewContinuousVariables(
-          2*3, "slip_contact_pos_" + std::to_string(knot)));
+          2 * 3, "slip_contact_pos_" + std::to_string(knot)));
       slip_contact_vel_vars_.push_back(prog_->NewContinuousVariables(
-          2*3, "slip_contact_vel_" + std::to_string(knot)));
+          2 * 3, "slip_contact_vel_" + std::to_string(knot)));
       slip_force_vars_.push_back(prog_->NewContinuousVariables(
           2, "slip_force_" + std::to_string(knot)));
     }
     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(), stance_mode);
+    std::fill(slip_contact_sequence_.begin(), slip_contact_sequence_.end(),
+              stance_mode);
 
-    m_=plant_.CalcTotalMass(*contexts_[0]);
+    m_ = plant_.CalcTotalMass(*contexts_[0]);
   }
 
-  std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateSlipContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+  CreateSlipContactPoints(const drake::multibody::MultibodyPlant<double>& plant,
+                          double mu);
 
   /*!
- * @brief Set the mode sequence
- * @param contact_sequence vector where
- * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point`
- * is `contact_index` active
- */
-  void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence) override;
+   * @brief Set the mode sequence
+   * @param contact_sequence vector where
+   * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point`
+   * is `contact_index` active
+   */
+  void SetModeSequence(
+      const std::vector<std::vector<bool>>& contact_sequence) override;
 
   /*!
    * @brief Set the mode sequence via a trajectory. The value of the trajectory
    * at each time, cast to a bool is if a contact point is active or not
    * @param contact_sequence
    */
-  void SetModeSequence(
-      const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) override;
+  void SetModeSequence(const drake::trajectories::PiecewisePolynomial<double>&
+                           contact_sequence) override;
 
-  drake::solvers::VectorXDecisionVariable slip_contact_pos_vars(int knot_point_index, int slip_foot_index);
+  drake::solvers::VectorXDecisionVariable slip_contact_pos_vars(
+      int knot_point_index, int slip_foot_index);
 
-  drake::solvers::VectorXDecisionVariable slip_contact_vel_vars(int knot_point_index, int slip_foot_index);
+  drake::solvers::VectorXDecisionVariable slip_contact_vel_vars(
+      int knot_point_index, int slip_foot_index);
 
   void SetComPositionGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& com_trajectory) override;
+      const drake::trajectories::PiecewisePolynomial<double>& com_trajectory)
+      override;
 
   void SetRobotStateGuess(
       const drake::trajectories::PiecewisePolynomial<double>& q_traj,
       const drake::trajectories::PiecewisePolynomial<double>& v_traj) override;
 
-  void SetMomentumGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory) override;
+  void SetMomentumGuess(const drake::trajectories::PiecewisePolynomial<double>&
+                            momentum_trajectory) override;
 
-  void SetForceGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& force_trajectory) override;
+  void SetForceGuess(const drake::trajectories::PiecewisePolynomial<double>&
+                         force_trajectory) override;
 
   void Build(const drake::solvers::SolverOptions& solver_options) override;
+
  private:
   void MapModeSequence();
 
@@ -98,11 +106,11 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
 
   void AddSlipReductionConstraint(int knot_point) override;
 
-  void AddSlipLiftingConstraint(int knot_point)override;
+  void AddSlipLiftingConstraint(int knot_point) override;
 
-  void AddSlipDynamics(int knot_point)override;
+  void AddSlipDynamics(int knot_point) override;
 
-  drake::VectorX<double> LiftSlipSolution(int knot_point)override;
+  drake::VectorX<double> LiftSlipSolution(int knot_point) override;
 
   /*!
    * @brief Adds loop closure constraints to the mpc
@@ -130,13 +138,14 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
   std::vector<std::shared_ptr<PlanarSlipLifter>> lifters_;
   std::vector<std::shared_ptr<PlanarSlipReducer>> reducers;
 
-  std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
+  std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+      slip_contact_points_;
 
-  std::map<std::vector<bool>, std::vector<bool>> complex_mode_to_slip_mode_{{{true, true, true, true},{true, true}},
-                                                                            {{true, true, false, false},{true, false}},
-                                                                            {{false, false, true, true},{false, true}},
-                                                                            {{false, false, false, false},{false, false}}};
+  std::map<std::vector<bool>, std::vector<bool>> complex_mode_to_slip_mode_{
+      {{true, true, true, true}, {true, true}},
+      {{true, true, false, false}, {true, false}},
+      {{false, false, true, true}, {false, true}},
+      {{false, false, false, false}, {false, false}}};
   std::vector<drake::solvers::Binding<drake::solvers::Constraint>>
       slip_dynamics_binding_;
 };
-
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
index 5828f4c938..d6a20b29fe 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
@@ -1,23 +1,25 @@
-#pragma  once
+#pragma once
 
 #include <drake/multibody/plant/multibody_plant.h>
+
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
+#include "multibody/kinematic/kinematic_evaluator_set.h"
+#include "multibody/kinematic/world_point_evaluator.h"
+#include "solvers/nonlinear_constraint.h"
+#include "solvers/nonlinear_cost.h"
+
 #include "drake/common/drake_copyable.h"
 #include "drake/common/symbolic.h"
 #include "drake/solvers/constraint.h"
-#include "solvers/nonlinear_constraint.h"
-#include "solvers/nonlinear_cost.h"
-#include "multibody/kinematic/kinematic_evaluator_set.h"
-#include "multibody/kinematic/world_point_evaluator.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
 
-class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstraint<double> {
+class PlanarSlipReductionConstraint
+    : public dairlib::solvers::NonlinearConstraint<double> {
  public:
-  PlanarSlipReductionConstraint(const drake::multibody::MultibodyPlant<double> &plant,
-                                std::shared_ptr<PlanarSlipReducer> reducing_function,
-                                int n_slip_feet,
-                                int n_complex_feet,
-                                int knot_index);
+  PlanarSlipReductionConstraint(
+      const drake::multibody::MultibodyPlant<double>& plant,
+      std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
+      int n_complex_feet, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
@@ -28,13 +30,13 @@ class PlanarSlipReductionConstraint : public dairlib::solvers::NonlinearConstrai
   const int complex_dim_;
 };
 
-class SlipGrfReductionConstrain : public dairlib::solvers::NonlinearConstraint<double> {
+class SlipGrfReductionConstrain
+    : public dairlib::solvers::NonlinearConstraint<double> {
  public:
-  SlipGrfReductionConstrain(const drake::multibody::MultibodyPlant<double> &plant,
-                                std::shared_ptr<PlanarSlipReducer> reducing_function,
-                                int n_slip_feet,
-                                int n_complex_feet,
-                                int knot_index);
+  SlipGrfReductionConstrain(
+      const drake::multibody::MultibodyPlant<double>& plant,
+      std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
+      int n_complex_feet, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
@@ -45,12 +47,13 @@ class SlipGrfReductionConstrain : public dairlib::solvers::NonlinearConstraint<d
   const int n_complex_feet_;
 };
 
-class PlanarSlipLiftingConstraint : public dairlib::solvers::NonlinearConstraint<double> {
+class PlanarSlipLiftingConstraint
+    : public dairlib::solvers::NonlinearConstraint<double> {
  public:
-  PlanarSlipLiftingConstraint(const drake::multibody::MultibodyPlant<double>& plant,
-                              std::shared_ptr<PlanarSlipLifter> lifting_function,
-                              int n_slip_feet, int n_complex_feet,
-                              int knot_index);
+  PlanarSlipLiftingConstraint(
+      const drake::multibody::MultibodyPlant<double>& plant,
+      std::shared_ptr<PlanarSlipLifter> lifting_function, int n_slip_feet,
+      int n_complex_feet, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
@@ -62,24 +65,22 @@ class PlanarSlipLiftingConstraint : public dairlib::solvers::NonlinearConstraint
 };
 
 template <typename T>
-class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
-
+class PlanarSlipDynamicsConstraint
+    : public dairlib::solvers::NonlinearConstraint<T> {
  public:
   PlanarSlipDynamicsConstraint(double r0, double k, double b, T m, int n_feet,
                                std::vector<bool> contact_mask0,
-                               std::vector<bool> contact_mask1,
-                               double dt, int knot_index);
+                               std::vector<bool> contact_mask1, double dt,
+                               int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
                           drake::VectorX<T>* y) const override;
 
   drake::VectorX<T> CalcTimeDerivativesWithForce(
-      const drake::VectorX<T>& com_position,
-      const drake::VectorX<T>& com_vel,
-      const drake::VectorX<T>& contact_loc,
-      const drake::VectorX<T> &slip_force,
-      const std::vector<bool> &contact_mask) const;
+      const drake::VectorX<T>& com_position, const drake::VectorX<T>& com_vel,
+      const drake::VectorX<T>& contact_loc, const drake::VectorX<T>& slip_force,
+      const std::vector<bool>& contact_mask) const;
 
   const double r0_;
   const double k_;
@@ -100,30 +101,25 @@ class PlanarSlipDynamicsConstraint : public dairlib::solvers::NonlinearConstrain
 ///     complex_gen_pos
 ///     complex_gen_vel
 class QuadraticLiftedCost : public dairlib::solvers::NonlinearCost<double> {
-
-  struct cost_element{
+  struct cost_element {
     const Eigen::MatrixXd& Q;
     Eigen::MatrixXd ref;
   };
 
  public:
   QuadraticLiftedCost(std::shared_ptr<PlanarSlipLifter> lifting_function,
-                      cost_element  com_cost,
-                      cost_element  momentum_cost,
-                      cost_element  contact_cost,
-                      cost_element  grf_cost,
-                      cost_element  q_cost,
-                      cost_element  v_cost,
-                      double terminal_gain,
-                      int n_slip_feet,
-                      int knot_point);
+                      cost_element com_cost, cost_element momentum_cost,
+                      cost_element contact_cost, cost_element grf_cost,
+                      cost_element q_cost, cost_element v_cost,
+                      double terminal_gain, int n_slip_feet, int knot_point);
 
  private:
   void EvaluateCost(const Eigen::Ref<const drake::VectorX<double>>& x,
-                          drake::VectorX<double>* y) const override;
-
-  Eigen::Matrix<double, -1, 1, 0> CalcCost(const cost_element& cost, const Eigen::Ref<const drake::VectorX<double>>& x) const;
+                    drake::VectorX<double>* y) const override;
 
+  Eigen::Matrix<double, -1, 1, 0> CalcCost(
+      const cost_element& cost,
+      const Eigen::Ref<const drake::VectorX<double>>& x) const;
 
   std::shared_ptr<PlanarSlipLifter> lifting_function_;
   const cost_element com_cost_;
@@ -134,5 +130,3 @@ class QuadraticLiftedCost : public dairlib::solvers::NonlinearCost<double> {
   const cost_element v_cost_;
   const double terminal_gain_;
 };
-
-
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc
index ba9bf860ad..61888fb7fc 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc
@@ -1,44 +1,51 @@
-#include <drake/solvers/solve.h>
-#include <iostream>
 #include "planar_slip_lifter.h"
+
+#include <iostream>
+
+#include <drake/solvers/solve.h>
+
 #include "multibody/multibody_utils.h"
 
-PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double> &plant,
-                                   drake::systems::Context<double> *context,
-                                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
-                                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
-                                   const std::map<int,
-                                                  std::vector<int>> &simple_foot_index_to_complex_foot_index,
-                                   const drake::VectorX<double> &nominal_stand,
-                                   double k,
-                                   double b,
-                                   double r0,
-                                   const std::vector<bool>& contact_mask) :
-    plant_(plant),
-    context_(context),
-    ik_(plant, context),
-    m_(plant.CalcTotalMass(*context)),
-    k_(k),
-    b_(b),
-    r0_(r0),
-    slip_contact_points_(slip_contact_points),
-    complex_contact_points_(complex_contact_points),
-    simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
-    n_q_(plant.num_positions()),
-    n_v_(plant.num_velocities()),
-    slip_contact_mask_(contact_mask){
+PlanarSlipLifter::PlanarSlipLifter(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    drake::systems::Context<double>* context,
+    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+        slip_contact_points,
+    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+        complex_contact_points,
+    const std::map<int, std::vector<int>>&
+        simple_foot_index_to_complex_foot_index,
+    const drake::VectorX<double>& nominal_stand, double k, double b, double r0,
+    const std::vector<bool>& contact_mask)
+    : plant_(plant),
+      context_(context),
+      ik_(plant, context),
+      m_(plant.CalcTotalMass(*context)),
+      k_(k),
+      b_(b),
+      r0_(r0),
+      slip_contact_points_(slip_contact_points),
+      complex_contact_points_(complex_contact_points),
+      simple_foot_index_to_complex_foot_index_(
+          simple_foot_index_to_complex_foot_index),
+      n_q_(plant.num_positions()),
+      n_v_(plant.num_velocities()),
+      slip_contact_mask_(contact_mask) {
   ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), nominal_stand);
   com_vars_ = ik_.get_mutable_prog()->NewContinuousVariables(3);
-  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(&plant, std::nullopt, plant.world_frame(), context);
-  ik_.get_mutable_prog()->AddConstraint(constraint, {ik_.q(),com_vars_});
+  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(
+      &plant, std::nullopt, plant.world_frame(), context);
+  ik_.get_mutable_prog()->AddConstraint(constraint, {ik_.q(), com_vars_});
 
   const auto& world_frame = plant.world_frame();
   const auto& pelvis_frame = plant.GetFrameByName("pelvis");
 
-  ik_.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
-                               world_frame, drake::math::RotationMatrix<double>(), 1e-4);
+  ik_.AddOrientationConstraint(
+      pelvis_frame, drake::math::RotationMatrix<double>(), world_frame,
+      drake::math::RotationMatrix<double>(), 1e-4);
 
-  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
+  std::map<std::string, int> positions_map =
+      dairlib::multibody::MakeNameToPositionsMap(plant);
   ik_.get_mutable_prog()->AddLinearConstraint(
       (ik_.q())(positions_map.at("hip_yaw_left")) == 0);
   ik_.get_mutable_prog()->AddLinearConstraint(
@@ -48,95 +55,112 @@ PlanarSlipLifter::PlanarSlipLifter(const drake::multibody::MultibodyPlant<double
   ik_.get_mutable_prog()->AddLinearConstraint(
       (ik_.q())(positions_map.at("knee_left")) +
           (ik_.q())(positions_map.at("ankle_joint_left")) ==
-          M_PI * 13 / 180.0);
+      M_PI * 13 / 180.0);
   ik_.get_mutable_prog()->AddLinearConstraint(
       (ik_.q())(positions_map.at("knee_right")) +
           (ik_.q())(positions_map.at("ankle_joint_right")) ==
-          M_PI * 13 / 180.0);
-
+      M_PI * 13 / 180.0);
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(const drake::Vector3<double> &com_position,
-                                                               const drake::VectorX<double> &slip_feet_positions) const {
-  DRAKE_DEMAND(slip_feet_positions.size() == 3*slip_contact_points_.size());
-  //Add com position constraint
-  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), com_vars_);
-  //Add feet position constraint
-  std::vector<drake::solvers::Binding<drake::solvers::Constraint>> foot_constraints;
-  for(int i = 0; i < slip_contact_points_.size(); i++){
+drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(
+    const drake::Vector3<double>& com_position,
+    const drake::VectorX<double>& slip_feet_positions) const {
+  DRAKE_DEMAND(slip_feet_positions.size() == 3 * slip_contact_points_.size());
+  // Add com position constraint
+  const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint(
+      Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), com_vars_);
+  // Add feet position constraint
+  std::vector<drake::solvers::Binding<drake::solvers::Constraint>>
+      foot_constraints;
+  for (int i = 0; i < slip_contact_points_.size(); i++) {
     const auto& slip_spatial_foot_pos = slip_feet_positions.segment(3 * i, 3);
-    const drake::Vector3<double> slip_foot_rt_com = slip_spatial_foot_pos - com_position;
-    foot_constraints.push_back(ik_.AddPositionConstraint(slip_contact_points_[i].get_frame(), slip_contact_points_[i].get_pt_A(), plant_.world_frame(),
-                                                         std::nullopt,
-                                                         slip_foot_rt_com,
-                                                         slip_foot_rt_com));
-    for(const auto complex_index : simple_foot_index_to_complex_foot_index_.at(i)){
+    const drake::Vector3<double> slip_foot_rt_com =
+        slip_spatial_foot_pos - com_position;
+    foot_constraints.push_back(ik_.AddPositionConstraint(
+        slip_contact_points_[i].get_frame(), slip_contact_points_[i].get_pt_A(),
+        plant_.world_frame(), std::nullopt, slip_foot_rt_com,
+        slip_foot_rt_com));
+    for (const auto complex_index :
+         simple_foot_index_to_complex_foot_index_.at(i)) {
       const drake::Vector3<double> lb{-100, -100, slip_foot_rt_com[2]};
       const drake::Vector3<double> ub{100, 100, slip_foot_rt_com[2]};
-      foot_constraints.push_back(ik_.AddPositionConstraint(complex_contact_points_[complex_index].get_frame(), complex_contact_points_[complex_index].get_pt_A(), plant_.world_frame(),
-                                                           std::nullopt,
-                                                           lb,
-                                                           ub));
+      foot_constraints.push_back(ik_.AddPositionConstraint(
+          complex_contact_points_[complex_index].get_frame(),
+          complex_contact_points_[complex_index].get_pt_A(),
+          plant_.world_frame(), std::nullopt, lb, ub));
     }
   }
-  //Set initial guess for com
-  ik_.get_mutable_prog()->SetInitialGuess(com_vars_,Eigen::VectorXd::Zero(3));
-  //Solve
+  // Set initial guess for com
+  ik_.get_mutable_prog()->SetInitialGuess(com_vars_, Eigen::VectorXd::Zero(3));
+  // Solve
   const auto result = drake::solvers::Solve(ik_.prog());
   const auto q_sol = result.GetSolution(ik_.q());
-  //Normalize quaternion
+  // Normalize quaternion
   drake::VectorX<double> q_sol_normd(n_q_);
   q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q_ - 4);
 
-  q_sol_normd.segment(4,3) = q_sol_normd.segment(4,3) + com_position;
-  //Set initial guess for next time
-//  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
-  //Remove added constraints
+  q_sol_normd.segment(4, 3) = q_sol_normd.segment(4, 3) + com_position;
+  // Set initial guess for next time
+  //  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
+  // Remove added constraints
   ik_.get_mutable_prog()->RemoveConstraint(com_constraint);
-  for(const auto& constraint : foot_constraints){
+  for (const auto& constraint : foot_constraints) {
     ik_.get_mutable_prog()->RemoveConstraint(constraint);
   }
   return q_sol_normd;
 }
-drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::VectorX<double>& generalized_pos,
-                                                               const drake::Vector3<double>& linear_momentum,
-                                                               const drake::Vector3<double>& com_pos,
-                                                               const drake::VectorX<double>& slip_feet_velocities)const {
-  DRAKE_DEMAND(slip_feet_velocities.size() == 3*slip_contact_points_.size());
+drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(
+    const drake::VectorX<double>& generalized_pos,
+    const drake::Vector3<double>& linear_momentum,
+    const drake::Vector3<double>& com_pos,
+    const drake::VectorX<double>& slip_feet_velocities) const {
+  DRAKE_DEMAND(slip_feet_velocities.size() == 3 * slip_contact_points_.size());
   // Preallocate linear constraint
-  drake::MatrixX<double> A(3 + 2 * 3 * slip_contact_points_.size() ,n_v_); // 3 rows for linear momentum, 3 rows for each slip foot, 3 rows for each slip foot
+  drake::MatrixX<double> A(3 + 2 * 3 * slip_contact_points_.size(),
+                           n_v_);  // 3 rows for linear momentum, 3 rows for
+                                   // each slip foot, 3 rows for each slip foot
   drake::VectorX<double> b(3 + 2 * 3 * slip_contact_points_.size());
 
-  // order of constraints are: slip foot velocity,  zero toe rotation, linear momentum,
-  // Zero toe rotation accomplished by constraining per foot complex contact velocities to be equal
+  // order of constraints are: slip foot velocity,  zero toe rotation, linear
+  // momentum, Zero toe rotation accomplished by constraining per foot complex
+  // contact velocities to be equal
 
   // set b for linear momentum
   b.tail(3) = linear_momentum;
 
-  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
-  for(int i = 0; i < slip_contact_points_.size(); i++){
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos,
+                                                context_);
+  for (int i = 0; i < slip_contact_points_.size(); i++) {
     // Set A and b for slip foot velocity constraint
-    b.segment(3 * i, 3) =slip_feet_velocities.segment(3 * i, 3);
-    A.middleRows(3 * i, 3) = slip_contact_points_[i].EvalFullJacobian(*context_);
+    b.segment(3 * i, 3) = slip_feet_velocities.segment(3 * i, 3);
+    A.middleRows(3 * i, 3) =
+        slip_contact_points_[i].EvalFullJacobian(*context_);
     // Set A for zero toe rotation
     const auto contact_it = simple_foot_index_to_complex_foot_index_.find(i);
     A.middleRows(3 * slip_contact_points_.size() + 3 * i, 3) =
-        complex_contact_points_[contact_it->second[0]].EvalFullJacobian(*context_)
-            - complex_contact_points_[contact_it->second[1]].EvalFullJacobian(*context_);
+        complex_contact_points_[contact_it->second[0]].EvalFullJacobian(
+            *context_) -
+        complex_contact_points_[contact_it->second[1]].EvalFullJacobian(
+            *context_);
   }
 
   // Finite difference to calculate momentum jacobian
   // TODO replace this with analytical gradient
   drake::VectorX<double> x_val = drake::VectorX<double>::Zero(n_v_);
   drake::VectorX<double> yi(6);
-  dairlib::multibody::SetVelocitiesIfNew<double>(plant_,x_val,context_);
-  auto y0 = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos).translational();
+  dairlib::multibody::SetVelocitiesIfNew<double>(plant_, x_val, context_);
+  auto y0 = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos)
+                .translational();
   const double eps = 1e-7;
   for (int i = 0; i < n_v_; i++) {
     x_val(i) += eps;
-    dairlib::multibody::SetVelocitiesIfNew<double>(plant_,x_val,context_);
+    dairlib::multibody::SetVelocitiesIfNew<double>(plant_, x_val, context_);
     x_val(i) -= eps;
-    A.col(i).tail(3) = (plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos).translational() - y0) / eps;
+    A.col(i).tail(3) =
+        (plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos)
+             .translational() -
+         y0) /
+        eps;
   }
 
   // solve
@@ -144,60 +168,80 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(const drake::Ve
   // Set base angular velocity to zero
   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).bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+  rv.tail(n_v_ - 3) = A.rightCols(n_v_ - 3)
+                          .bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV)
+                          .solve(b);
   return rv;
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftContactPos(const drake::VectorX<double> &generalized_position)const {
-  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_position, context_);
+drake::VectorX<double> PlanarSlipLifter::LiftContactPos(
+    const drake::VectorX<double>& generalized_position) const {
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_position,
+                                                context_);
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
-  for(int i = 0; i < complex_contact_points_.size(); i++){
+  for (int i = 0; i < complex_contact_points_.size(); i++) {
     rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFull(*context_);
   }
   return rv;
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftContactVel(const drake::VectorX<double> &generalized_pos,
-                                                        const drake::VectorX<double> &generalized_vel)const {
-  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
-  dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel, context_);
+drake::VectorX<double> PlanarSlipLifter::LiftContactVel(
+    const drake::VectorX<double>& generalized_pos,
+    const drake::VectorX<double>& generalized_vel) const {
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos,
+                                                context_);
+  dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel,
+                                                 context_);
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
-  for(int i = 0; i < complex_contact_points_.size(); i++){
-    rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFullTimeDerivative(*context_);
+  for (int i = 0; i < complex_contact_points_.size(); i++) {
+    rv.segment(3 * i, 3) =
+        complex_contact_points_[i].EvalFullTimeDerivative(*context_);
   }
   return rv;
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &com_pos,
-                                                 const drake::VectorX<double> &com_vel,
-                                                 const drake::VectorX<double> &slip_feet_pos,
-                                                 const drake::VectorX<double> &slip_force,
-                                                 const drake::VectorX<double> &complex_contact_point_pos) const {
+drake::VectorX<double> PlanarSlipLifter::LiftGrf(
+    const drake::VectorX<double>& com_pos,
+    const drake::VectorX<double>& com_vel,
+    const drake::VectorX<double>& slip_feet_pos,
+    const drake::VectorX<double>& slip_force,
+    const drake::VectorX<double>& complex_contact_point_pos) const {
   drake::VectorX<double> rv(complex_contact_points_.size() * 3);
   // Loop through the slip feet
-  for(int simple_index = 0; simple_index < slip_contact_points_.size(); simple_index ++){
+  for (int simple_index = 0; simple_index < slip_contact_points_.size();
+       simple_index++) {
     // Calculate the slip grf
     double r = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).norm();
-    double dr = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).normalized().dot(com_vel);
-    double slip_grf_mag = slip_contact_mask_[simple_index] ? slip_force[simple_index] + k_ * (r0_ - r) - b_ * dr : 0;
-    if(slip_grf_mag  < 0){
+    double dr = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos)
+                    .normalized()
+                    .dot(com_vel);
+    double slip_grf_mag =
+        slip_contact_mask_[simple_index]
+            ? slip_force[simple_index] + k_ * (r0_ - r) - b_ * dr
+            : 0;
+    if (slip_grf_mag < 0) {
       slip_grf_mag = 0;
     }
-    // Find the average location for all of the complex contact points that make up the SLIP foot
+    // Find the average location for all of the complex contact points that make
+    // up the SLIP foot
     drake::Vector3<double> average_pos = drake::VectorX<double>::Zero(3);
 
-    const auto& complex_feet_list = simple_foot_index_to_complex_foot_index_.at(simple_index);
-    for(const auto& complex_index : complex_feet_list){
-      average_pos = average_pos + complex_contact_point_pos.segment(3 * complex_index, 3);
+    const auto& complex_feet_list =
+        simple_foot_index_to_complex_foot_index_.at(simple_index);
+    for (const auto& complex_index : complex_feet_list) {
+      average_pos =
+          average_pos + complex_contact_point_pos.segment(3 * complex_index, 3);
     }
-    average_pos = average_pos/complex_feet_list.size();
+    average_pos = average_pos / complex_feet_list.size();
 
-    // Direction of all the grf for this slip foot must be parallel to not create internal forces
-    // direction is the vector from average contact point to com, so no moment from sum of grf
+    // Direction of all the grf for this slip foot must be parallel to not
+    // create internal forces direction is the vector from average contact point
+    // to com, so no moment from sum of grf
     const auto dir = (com_pos - average_pos).normalized();
     // Distribute grf magnitude across all of the complex contact points
-    for(const auto& complex_index : complex_feet_list){
-      rv.segment(3 * complex_index, 3) = dir * slip_grf_mag/complex_feet_list.size();
+    for (const auto& complex_index : complex_feet_list) {
+      rv.segment(3 * complex_index, 3) =
+          dir * slip_grf_mag / complex_feet_list.size();
     }
   }
   return rv;
@@ -218,32 +262,45 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(const drake::VectorX<double> &c
 ///     complex_contact_force
 ///     complex_gen_pos
 ///     complex_gen_vel
-void PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
-                               drake::VectorX<double> *complex_state)const {
+void PlanarSlipLifter::Lift(
+    const Eigen::Ref<const drake::VectorX<double>>& slip_state,
+    drake::VectorX<double>* complex_state) const {
   const auto& slip_com = slip_state.head(kSLIP_DIM);
   const auto& slip_vel = slip_state.segment(kSLIP_DIM, kSLIP_DIM);
-  const auto& slip_contact_pos = slip_state.segment(kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_contact_points_.size());
-  const auto& slip_contact_vel = slip_state.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size(), kSLIP_DIM * slip_contact_points_.size());
-  const auto& slip_force = slip_state.segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size() + kSLIP_DIM * slip_contact_points_.size(), slip_contact_points_.size());
+  const auto& slip_contact_pos = slip_state.segment(
+      kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_contact_points_.size());
+  const auto& slip_contact_vel = slip_state.segment(
+      kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size(),
+      kSLIP_DIM * slip_contact_points_.size());
+  const auto& slip_force = slip_state.segment(
+      kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size() +
+          kSLIP_DIM * slip_contact_points_.size(),
+      slip_contact_points_.size());
 
   const drake::Vector3<double> lin_mom = slip_vel * m_;
-  const auto& generalized_pos = LiftGeneralizedPosition(slip_com, slip_contact_pos);
-  const auto& generalized_vel = LiftGeneralizedVelocity(generalized_pos, lin_mom, slip_com, slip_contact_vel);
+  const auto& generalized_pos =
+      LiftGeneralizedPosition(slip_com, slip_contact_pos);
+  const auto& generalized_vel = LiftGeneralizedVelocity(
+      generalized_pos, lin_mom, slip_com, slip_contact_vel);
 
-  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos, context_);
-  dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel, context_);
+  dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos,
+                                                context_);
+  dairlib::multibody::SetVelocitiesIfNew<double>(plant_, generalized_vel,
+                                                 context_);
   const auto& complex_contact_pos = LiftContactPos(generalized_pos);
 
-  (*complex_state) << slip_com, plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, slip_com)
-      .get_coeffs(), complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel), LiftGrf(slip_com,
-                                                                                                    slip_vel,
-                                                                                                    slip_contact_pos,
-                                                                                                    slip_force,
-                                                                                                    complex_contact_pos),
+  (*complex_state) << slip_com,
+      plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, slip_com)
+          .get_coeffs(),
+      complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel),
+      LiftGrf(slip_com, slip_vel, slip_contact_pos, slip_force,
+              complex_contact_pos),
       generalized_pos, generalized_vel;
 }
-drake::VectorX<double> PlanarSlipLifter::Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state) const {
-  drake::VectorX<double> complex_state(6 + 3 + 3 * 3 * complex_contact_points_.size() + n_q_ + n_v_);
+drake::VectorX<double> PlanarSlipLifter::Lift(
+    const Eigen::Ref<const drake::VectorX<double>>& slip_state) const {
+  drake::VectorX<double> complex_state(
+      6 + 3 + 3 * 3 * complex_contact_points_.size() + n_q_ + n_v_);
   Lift(slip_state, &complex_state);
   return complex_state;
 }
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h
index 8856911778..e5a47de131 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h
@@ -1,48 +1,56 @@
 #pragma once
+#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 #include <drake/multibody/plant/multibody_plant.h>
+
+#include "multibody/kinematic/kinematic_evaluator_set.h"
+#include "multibody/kinematic/world_point_evaluator.h"
+#include "solvers/nonlinear_constraint.h"
+
 #include "drake/common/drake_copyable.h"
 #include "drake/common/symbolic.h"
 #include "drake/solvers/constraint.h"
 #include "drake/systems/trajectory_optimization/multiple_shooting.h"
-#include "solvers/nonlinear_constraint.h"
-#include "multibody/kinematic/kinematic_evaluator_set.h"
-#include "multibody/kinematic/world_point_evaluator.h"
-#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
-#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 
 class PlanarSlipLifter {
  public:
-  PlanarSlipLifter(const drake::multibody::MultibodyPlant<double> &plant,
-                   drake::systems::Context<double> *context,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
-                   const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
-                   const std::map<int,
-                                  std::vector<int>> &simple_foot_index_to_complex_foot_index,
-                   const drake::VectorX<double> &nominal_stand,
-                   double k,
-                   double b,
-                   double r0,
-                   const std::vector<bool>& contact_mask);
+  PlanarSlipLifter(
+      const drake::multibody::MultibodyPlant<double>& plant,
+      drake::systems::Context<double>* context,
+      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+          slip_contact_points,
+      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+          complex_contact_points,
+      const std::map<int, std::vector<int>>&
+          simple_foot_index_to_complex_foot_index,
+      const drake::VectorX<double>& nominal_stand, double k, double b,
+      double r0, const std::vector<bool>& contact_mask);
 
-  void Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state,
-            drake::VectorX<double> *complex_state) const;
+  void Lift(const Eigen::Ref<const drake::VectorX<double>>& slip_state,
+            drake::VectorX<double>* complex_state) const;
 
-  drake::VectorX<double> Lift(const Eigen::Ref<const drake::VectorX<double>> &slip_state) const;
+  drake::VectorX<double> Lift(
+      const Eigen::Ref<const drake::VectorX<double>>& slip_state) const;
 
  private:
   /*!
-   * @brief given the center of mass position and slip feet position solve for the generalized position of the full robot
+   * @brief given the center of mass position and slip feet position solve for
+   * the generalized position of the full robot
    * @note assumes identity orientation
    * @note implements numerical ik (can be slow)
    * @param com_position center of mass position in the world
-   * @param slip_feet_positions [2*n_slip_feet] locations of the planar slip feet
+   * @param slip_feet_positions [2*n_slip_feet] locations of the planar slip
+   * feet
    * @return the generalized positions
    */
-  drake::VectorX<double> LiftGeneralizedPosition(const drake::Vector3<double>& com_position, const drake::VectorX<double>& slip_feet_positions) const;
+  drake::VectorX<double> LiftGeneralizedPosition(
+      const drake::Vector3<double>& com_position,
+      const drake::VectorX<double>& slip_feet_positions) const;
 
   /*!
-   * @brief Given a generalized position calculate the generalized velocity that is the least squares solution to tracking the
-   * linear momentum and foot velocity with 0 floating base angular velocity.
+   * @brief Given a generalized position calculate the generalized velocity that
+   * is the least squares solution to tracking the linear momentum and foot
+   * velocity with 0 floating base angular velocity.
    * @note Jacobian for momentum is calculated numerically
    * @param generalized_pos
    * @param linear_momentum
@@ -50,21 +58,25 @@ class PlanarSlipLifter {
    * @param slip_feet_velocities [2*n_slip_feet] the foot velocity
    * @return generalized velocities
    */
-  drake::VectorX<double> LiftGeneralizedVelocity(const drake::VectorX<double>& generalized_pos,
-                                            const drake::Vector3<double>& linear_momentum,
-                                            const drake::Vector3<double>& com_pos,
-                                            const drake::VectorX<double>& slip_feet_velocities) const;
-
-  drake::VectorX<double> LiftContactPos(const drake::VectorX<double>& generalized_position) const;
+  drake::VectorX<double> LiftGeneralizedVelocity(
+      const drake::VectorX<double>& generalized_pos,
+      const drake::Vector3<double>& linear_momentum,
+      const drake::Vector3<double>& com_pos,
+      const drake::VectorX<double>& slip_feet_velocities) const;
 
-  drake::VectorX<double> LiftContactVel(const drake::VectorX<double>& generalized_pos, const drake::VectorX<double>& generalized_vel) const;
+  drake::VectorX<double> LiftContactPos(
+      const drake::VectorX<double>& generalized_position) const;
 
-  drake::VectorX<double> LiftGrf(const drake::VectorX<double> &com_pos,
-                                 const drake::VectorX<double> &com_vel,
-                                 const drake::VectorX<double> &slip_feet_pos,
-                                 const drake::VectorX<double> &slip_force,
-                                 const drake::VectorX<double> &complex_contact_point_pos) const;
+  drake::VectorX<double> LiftContactVel(
+      const drake::VectorX<double>& generalized_pos,
+      const drake::VectorX<double>& generalized_vel) const;
 
+  drake::VectorX<double> LiftGrf(
+      const drake::VectorX<double>& com_pos,
+      const drake::VectorX<double>& com_vel,
+      const drake::VectorX<double>& slip_feet_pos,
+      const drake::VectorX<double>& slip_force,
+      const drake::VectorX<double>& complex_contact_point_pos) const;
 
   const drake::multibody::MultibodyPlant<double>& plant_;
   mutable drake::systems::Context<double>* context_;
@@ -74,9 +86,12 @@ class PlanarSlipLifter {
   const double b_;
   const double r0_;
   const std::vector<double> stance_widths_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> complex_contact_points_;
-  const std::map<int, std::vector<int>> simple_foot_index_to_complex_foot_index_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+      slip_contact_points_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+      complex_contact_points_;
+  const std::map<int, std::vector<int>>
+      simple_foot_index_to_complex_foot_index_;
   const int n_q_;
   const int n_v_;
   const std::vector<bool> slip_contact_mask_;
@@ -84,4 +99,3 @@ class PlanarSlipLifter {
 
   const int kSLIP_DIM = 3;
 };
-
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
index d0b3e42d82..daf328dd9a 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
@@ -1,27 +1,29 @@
-#include <drake/systems/framework/diagram_builder.h>
+#include <iostream>
+
+#include <drake/common/yaml/yaml_io.h>
+#include <drake/geometry/drake_visualizer.h>
 #include <drake/geometry/scene_graph.h>
 #include <drake/multibody/parsing/parser.h>
 #include <drake/multibody/plant/multibody_plant.h>
-#include <iostream>
+#include <drake/solvers/solve.h>
+#include <drake/systems/analysis/simulator.h>
+#include <drake/systems/framework/diagram_builder.h>
 #include <drake/systems/primitives/trajectory_source.h>
 #include <drake/systems/rendering/multibody_position_to_geometry_pose.h>
-#include <drake/systems/analysis/simulator.h>
-#include <drake/geometry/drake_visualizer.h>
-#include <drake/solvers/solve.h>
-#include <drake/common/yaml/yaml_io.h>
+
 #include "common/find_resource.h"
-#include "systems/primitives/subvector_pass_through.h"
-#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
 #include "examples/Cassie/cassie_utils.h"
-#include "multibody/visualization_utils.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
+#include "multibody/visualization_utils.h"
+#include "systems/primitives/subvector_pass_through.h"
 
+using drake::geometry::DrakeVisualizer;
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
 using drake::multibody::Parser;
-using drake::geometry::DrakeVisualizer;
 
 int main(int argc, char* argv[]) {
   // Create fix-spring Cassie MBP
@@ -35,13 +37,14 @@ int main(int argc, char* argv[]) {
   Parser parser(&plant);
   Parser parser_vis(&plant_vis, &scene_graph);
 
-  std::string full_name =
-      dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+  std::string full_name = dairlib::FindResourceOrThrow(
+      "examples/Cassie/urdf/cassie_fixed_springs.urdf");
   parser.AddModelFromFile(full_name);
   parser_vis.AddModelFromFile(full_name);
   plant.Finalize();
   plant_vis.Finalize();
-  Eigen::VectorXd reference_state = GenerateNominalStand(plant, 0.8, 0.2, false);
+  Eigen::VectorXd reference_state =
+      GenerateNominalStand(plant, 0.8, 0.2, false);
 
   auto context = plant.CreateDefaultContext();
 
@@ -68,45 +71,41 @@ int main(int argc, char* argv[]) {
       plant, right_heel_pair.first, right_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
 
-//  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-//      plant, {0,0,0}, left_heel_pair.second,
-//      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-//
-//  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-//      plant, {0,0,0}, right_heel_pair.second,
-//      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  //  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+  //      plant, {0,0,0}, left_heel_pair.second,
+  //      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
+  //
+  //  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
+  //      plant, {0,0,0}, right_heel_pair.second,
+  //      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
 
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant, reference_state, context.get());
-//  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(
+      plant, reference_state, context.get());
+  //  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
 
   std::vector<bool> contact_mask = {true, true};
-  PlanarSlipLifter lifter(plant,
-                          context.get(),
-                          {left_toe_eval, right_toe_eval},
-                          {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
-                          {{0, {0, 1}}, {1, {2, 3}}},
-                          reference_state.head(plant.num_positions()),
-                          2000,
-                          0,
-                          0.5, contact_mask);
-  PlanarSlipReducer reducer(plant,
-                            context.get(),
-                            {left_toe_eval, right_toe_eval},
-                            {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
-                            {{0, {0, 1}}, {1, {2, 3}}},
-                            2000,
-                            0,
-                            0.5, contact_mask);
-
-  Eigen::Vector3d slip_com = {0.2,0,0.7};
-  Eigen::Vector3d slip_vel = {0.1,0,0.1};
-  Eigen::VectorXd slip_feet(6); slip_feet  << 0.0, 0.2, 0.0,0.0,-0.2, 0.0;
-  Eigen::VectorXd slip_foot_vel(6); slip_foot_vel  << 0.11,0.12,0.0,0.15,0.18,0.0;
-  Eigen::Vector2d slip_force = {0.5,0.3};
-
-  Eigen::VectorXd slip_state(3+3+6+6+2);
-  slip_state << slip_com,slip_vel,slip_feet,slip_foot_vel,slip_force;
-  Eigen::VectorXd complex_state(6 + 3 + 3 * 3 * 4 + plant.num_positions() + plant.num_velocities());
+  PlanarSlipLifter lifter(
+      plant, context.get(), {left_toe_eval, right_toe_eval},
+      {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
+      {{0, {0, 1}}, {1, {2, 3}}}, reference_state.head(plant.num_positions()),
+      2000, 0, 0.5, contact_mask);
+  PlanarSlipReducer reducer(
+      plant, context.get(), {left_toe_eval, right_toe_eval},
+      {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
+      {{0, {0, 1}}, {1, {2, 3}}}, 2000, 0, 0.5, contact_mask);
+
+  Eigen::Vector3d slip_com = {0.2, 0, 0.7};
+  Eigen::Vector3d slip_vel = {0.1, 0, 0.1};
+  Eigen::VectorXd slip_feet(6);
+  slip_feet << 0.0, 0.2, 0.0, 0.0, -0.2, 0.0;
+  Eigen::VectorXd slip_foot_vel(6);
+  slip_foot_vel << 0.11, 0.12, 0.0, 0.15, 0.18, 0.0;
+  Eigen::Vector2d slip_force = {0.5, 0.3};
+
+  Eigen::VectorXd slip_state(3 + 3 + 6 + 6 + 2);
+  slip_state << slip_com, slip_vel, slip_feet, slip_foot_vel, slip_force;
+  Eigen::VectorXd complex_state(6 + 3 + 3 * 3 * 4 + plant.num_positions() +
+                                plant.num_velocities());
 
   auto start = std::chrono::high_resolution_clock::now();
   lifter.Lift(slip_state, &complex_state);
@@ -119,37 +118,44 @@ int main(int argc, char* argv[]) {
   finish = std::chrono::high_resolution_clock::now();
   elapsed = finish - start;
   std::cout << "Solve time 2:" << elapsed.count() << std::endl;
-  auto constraint = PlanarSlipReductionConstraint(plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
+  auto constraint = PlanarSlipReductionConstraint(
+      plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
 
   drake::VectorX<double> error(slip_state.size());
   drake::VectorX<double> input(slip_state.size() + complex_state.size());
-  input << slip_state,complex_state;
+  input << slip_state, complex_state;
   constraint.DoEval(input, &error);
-  std::cout<<"Max Error in inverse test: "<< error.cwiseAbs().maxCoeff() << std::endl;
-//  std::cout<<error<<std::endl;
+  std::cout << "Max Error in inverse test: " << error.cwiseAbs().maxCoeff()
+            << std::endl;
+  //  std::cout<<error<<std::endl;
 
-  auto grf_constraint = SlipGrfReductionConstrain(plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
+  auto grf_constraint = SlipGrfReductionConstrain(
+      plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
   drake::VectorX<double> grf_error(2);
   drake::VectorX<double> grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3);
-//  grf_input << slip_com, slip_vel, slip_feet, complex_state.segment(3 + 6 + 12 + 12,12),slip_force;
+  //  grf_input << slip_com, slip_vel, slip_feet, complex_state.segment(3 + 6 +
+  //  12 + 12,12),slip_force;
 
   grf_constraint.DoEval(grf_input, &grf_error);
-  std::cout<<"Max Error in grf inverse test: "<< grf_error.cwiseAbs().maxCoeff() << std::endl;
+  std::cout << "Max Error in grf inverse test: "
+            << grf_error.cwiseAbs().maxCoeff() << std::endl;
 
-  if(true){
+  if (true) {
     // Build temporary diagram for visualization
     drake::systems::DiagramBuilder<double> builder_ik;
-    drake::geometry::SceneGraph<double>& scene_graph_ik = *builder_ik.AddSystem<drake::geometry::SceneGraph>();
+    drake::geometry::SceneGraph<double>& scene_graph_ik =
+        *builder_ik.AddSystem<drake::geometry::SceneGraph>();
     scene_graph_ik.set_name("scene_graph_ik");
     MultibodyPlant<double> plant_ik(0.0);
     Parser parser(&plant_ik, &scene_graph_ik);
-    std::string full_name =
-        dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+    std::string full_name = dairlib::FindResourceOrThrow(
+        "examples/Cassie/urdf/cassie_fixed_springs.urdf");
     parser.AddModelFromFile(full_name);
     plant_ik.Finalize();
 
     // Visualize
-    const auto& x_const = complex_state.tail(plant.num_positions() + plant.num_velocities());
+    const auto& x_const =
+        complex_state.tail(plant.num_positions() + plant.num_velocities());
     drake::trajectories::PiecewisePolynomial<double> pp_xtraj(x_const);
 
     dairlib::multibody::ConnectTrajectoryVisualizer(&plant_ik, &builder_ik,
@@ -161,4 +167,3 @@ int main(int argc, char* argv[]) {
     simulator.AdvanceTo(0.1);
   }
 }
-
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc
index a604982816..4ad55af827 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc
@@ -1,28 +1,32 @@
-#include <iostream>
 #include "planar_slip_reducer.h"
+
+#include <iostream>
+
 #include "multibody/multibody_utils.h"
 
-PlanarSlipReducer::PlanarSlipReducer(const drake::multibody::MultibodyPlant<double> &plant,
-                                     drake::systems::Context<double> *context,
-                                     const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
-                                     const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
-                                     const std::map<int,
-                                                    std::vector<int>> &simple_foot_index_to_complex_foot_index,
-                                     double k,
-                                     double b,
-                                     double r0,
-                                     const std::vector<bool> &contact_mask) : plant_(plant),
-                                                                              context_(context),
-                                                                              k_(k),
-                                                                              b_(b),
-                                                                              r0_(r0),
-                                                                              m_(plant.CalcTotalMass(*context)),
-                                                                              slip_contact_points_(slip_contact_points),
-                                                                              complex_contact_points_(complex_contact_points),
-                                                                              simple_foot_index_to_complex_foot_index_(simple_foot_index_to_complex_foot_index),
-                                                                              n_q_(plant.num_positions()),
-                                                                              n_v_(plant.num_velocities()),
-                                                                              slip_contact_mask_(contact_mask){}
+PlanarSlipReducer::PlanarSlipReducer(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    drake::systems::Context<double>* context,
+    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+        slip_contact_points,
+    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+        complex_contact_points,
+    const std::map<int, std::vector<int>>&
+        simple_foot_index_to_complex_foot_index,
+    double k, double b, double r0, const std::vector<bool>& contact_mask)
+    : plant_(plant),
+      context_(context),
+      k_(k),
+      b_(b),
+      r0_(r0),
+      m_(plant.CalcTotalMass(*context)),
+      slip_contact_points_(slip_contact_points),
+      complex_contact_points_(complex_contact_points),
+      simple_foot_index_to_complex_foot_index_(
+          simple_foot_index_to_complex_foot_index),
+      n_q_(plant.num_positions()),
+      n_v_(plant.num_velocities()),
+      slip_contact_mask_(contact_mask) {}
 /// Input is of the form:
 ///     complex_com
 ///     complex_ang_momentum
@@ -38,53 +42,66 @@ PlanarSlipReducer::PlanarSlipReducer(const drake::multibody::MultibodyPlant<doub
 ///     slip_contact_pos
 ///     slip_contact_vel
 ///     slip_force
-void PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state,
-                               drake::VectorX<double> *slip_state) const {
+void PlanarSlipReducer::Reduce(
+    const Eigen::Ref<const drake::VectorX<double>>& complex_state,
+    drake::VectorX<double>* slip_state) const {
   const auto& complex_com = complex_state.segment(0, 3);
   const auto& complex_ang_momentum = complex_state.segment(3, 3);
-  const auto& complex_lin_momentum = complex_state.segment( 3 + 3, 3);
-  const auto& complex_grf = complex_state.segment( 3 + 3 + 3 + 2 * 3 * complex_contact_points_.size(), 3 * complex_contact_points_.size());
-  const auto& complex_gen_state = complex_state.tail(n_q_+n_v_);
+  const auto& complex_lin_momentum = complex_state.segment(3 + 3, 3);
+  const auto& complex_grf =
+      complex_state.segment(3 + 3 + 3 + 2 * 3 * complex_contact_points_.size(),
+                            3 * complex_contact_points_.size());
+  const auto& complex_gen_state = complex_state.tail(n_q_ + n_v_);
 
-  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(plant_, complex_gen_state, context_);
+  dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(
+      plant_, complex_gen_state, context_);
   slip_state->head(kSLIP_DIM) = complex_com;
-  slip_state->segment(kSLIP_DIM, kSLIP_DIM) = complex_lin_momentum/m_;
-  for(int i = 0; i<slip_contact_points_.size(); i++){
-    slip_state->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_points_[i].EvalFull(*context_);
-    slip_state->segment(kSLIP_DIM+kSLIP_DIM+kSLIP_DIM * slip_contact_points_.size()+kSLIP_DIM * i, kSLIP_DIM) = slip_contact_points_[i].EvalFullTimeDerivative(*context_);
+  slip_state->segment(kSLIP_DIM, kSLIP_DIM) = complex_lin_momentum / m_;
+  for (int i = 0; i < slip_contact_points_.size(); i++) {
+    slip_state->segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * i, kSLIP_DIM) =
+        slip_contact_points_[i].EvalFull(*context_);
+    slip_state->segment(
+        kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size() +
+            kSLIP_DIM * i,
+        kSLIP_DIM) = slip_contact_points_[i].EvalFullTimeDerivative(*context_);
   }
-  slip_state->tail(slip_contact_points_.size()) = ReduceGrf(complex_com,
-                                                            complex_lin_momentum/m_,
-                                                            slip_state->segment(kSLIP_DIM + kSLIP_DIM,
-                                                                                slip_contact_points_.size()
-                                                                                    * kSLIP_DIM),
-                                                            complex_grf);
+  slip_state->tail(slip_contact_points_.size()) =
+      ReduceGrf(complex_com, complex_lin_momentum / m_,
+                slip_state->segment(kSLIP_DIM + kSLIP_DIM,
+                                    slip_contact_points_.size() * kSLIP_DIM),
+                complex_grf);
 }
 
-drake::VectorX<double> PlanarSlipReducer::Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state) const {
-  drake::VectorX<double> slip_state(kSLIP_DIM + kSLIP_DIM + 2 * kSLIP_DIM * slip_contact_points_.size() + slip_contact_points_.size());
+drake::VectorX<double> PlanarSlipReducer::Reduce(
+    const Eigen::Ref<const drake::VectorX<double>>& complex_state) const {
+  drake::VectorX<double> slip_state(
+      kSLIP_DIM + kSLIP_DIM + 2 * kSLIP_DIM * slip_contact_points_.size() +
+      slip_contact_points_.size());
   Reduce(complex_state, &slip_state);
   return slip_state;
 }
 
-drake::VectorX<double> PlanarSlipReducer::ReduceGrf(const Eigen::Ref<const drake::VectorX<double>> &complex_com,
-                                                    const Eigen::Ref<const drake::VectorX<double>> &com_vel,
-                                                    const Eigen::Ref<const drake::VectorX<double>> &slip_contact_pos,
-                                                    const Eigen::Ref<const drake::VectorX<double>> &complex_grf) const {
+drake::VectorX<double> PlanarSlipReducer::ReduceGrf(
+    const Eigen::Ref<const drake::VectorX<double>>& complex_com,
+    const Eigen::Ref<const drake::VectorX<double>>& com_vel,
+    const Eigen::Ref<const drake::VectorX<double>>& slip_contact_pos,
+    const Eigen::Ref<const drake::VectorX<double>>& complex_grf) const {
   DRAKE_DEMAND(complex_com.size() == 3);
   DRAKE_DEMAND(slip_contact_pos.size() == 3 * slip_contact_points_.size());
   Eigen::VectorXd slip_force(slip_contact_points_.size());
-  for(int i = 0; i<slip_contact_points_.size(); i++){
+  for (int i = 0; i < slip_contact_points_.size(); i++) {
     const double r = (complex_com - slip_contact_pos.segment(3 * i, 3)).norm();
-    const Eigen::VectorXd unit_vec = (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized();
-    const double dr =  unit_vec.dot(com_vel);
+    const Eigen::VectorXd unit_vec =
+        (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized();
+    const double dr = unit_vec.dot(com_vel);
     const double spring_force = k_ * (r0_ - r) - b_ * dr;
     auto complex_feet_it = simple_foot_index_to_complex_foot_index_.find(i);
     Eigen::Vector3d complex_force = Eigen::Vector3d::Zero(3);
-    for(const auto complex_index : complex_feet_it->second){
+    for (const auto complex_index : complex_feet_it->second) {
       complex_force = complex_force + complex_grf.segment(3 * complex_index, 3);
     }
-    const double mag = complex_force.dot(unit_vec) > 0 ? complex_force.norm() : -complex_force.norm();
+    const double mag = complex_force.dot(unit_vec) > 0 ? complex_force.norm()
+                                                       : -complex_force.norm();
     slip_force.coeffRef(i) = slip_contact_mask_[i] ? mag - spring_force : 0;
   }
   return slip_force;
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h
index 014c7060d3..a9dd01655d 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h
@@ -1,37 +1,42 @@
 #pragma once
+#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 #include <drake/multibody/plant/multibody_plant.h>
+
+#include "multibody/kinematic/kinematic_evaluator_set.h"
+#include "multibody/kinematic/world_point_evaluator.h"
+#include "solvers/nonlinear_constraint.h"
+
 #include "drake/common/drake_copyable.h"
 #include "drake/common/symbolic.h"
 #include "drake/solvers/constraint.h"
 #include "drake/systems/trajectory_optimization/multiple_shooting.h"
-#include "solvers/nonlinear_constraint.h"
-#include "multibody/kinematic/kinematic_evaluator_set.h"
-#include "multibody/kinematic/world_point_evaluator.h"
-#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
-#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 
 class PlanarSlipReducer {
  public:
-  PlanarSlipReducer(const drake::multibody::MultibodyPlant<double> &plant,
-                    drake::systems::Context<double> *context,
-                    const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &slip_contact_points,
-                    const std::vector<dairlib::multibody::WorldPointEvaluator<double>> &complex_contact_points,
-                    const std::map<int,
-                                   std::vector<int>> &simple_foot_index_to_complex_foot_index,
-                    double k,
-                    double b,
-                    double r0,
-                    const std::vector<bool> &contact_mask);
+  PlanarSlipReducer(
+      const drake::multibody::MultibodyPlant<double>& plant,
+      drake::systems::Context<double>* context,
+      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+          slip_contact_points,
+      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+          complex_contact_points,
+      const std::map<int, std::vector<int>>&
+          simple_foot_index_to_complex_foot_index,
+      double k, double b, double r0, const std::vector<bool>& contact_mask);
+
+  void Reduce(const Eigen::Ref<const drake::VectorX<double>>& complex_state,
+              drake::VectorX<double>* slip_state) const;
 
-  void Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state,
-            drake::VectorX<double> *slip_state) const;
+  drake::VectorX<double> Reduce(
+      const Eigen::Ref<const drake::VectorX<double>>& complex_state) const;
 
-  drake::VectorX<double> Reduce(const Eigen::Ref<const drake::VectorX<double>> &complex_state) const;
+  drake::VectorX<double> ReduceGrf(
+      const Eigen::Ref<const drake::VectorX<double>>& complex_com,
+      const Eigen::Ref<const drake::VectorX<double>>& com_vel,
+      const Eigen::Ref<const drake::VectorX<double>>& slip_contact_pos,
+      const Eigen::Ref<const drake::VectorX<double>>& complex_grf) const;
 
-  drake::VectorX<double> ReduceGrf(const Eigen::Ref<const drake::VectorX<double>> &complex_com,
-                                   const Eigen::Ref<const drake::VectorX<double>> &com_vel,
-                                   const Eigen::Ref<const drake::VectorX<double>> &slip_contact_pos,
-                                   const Eigen::Ref<const drake::VectorX<double>> &complex_grf) const;
  private:
   const drake::multibody::MultibodyPlant<double>& plant_;
   mutable drake::systems::Context<double>* context_;
@@ -39,12 +44,14 @@ class PlanarSlipReducer {
   const double b_;
   const double r0_;
   const double m_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> slip_contact_points_;
-  const std::vector<dairlib::multibody::WorldPointEvaluator<double>> complex_contact_points_;
-  const std::map<int, std::vector<int>> simple_foot_index_to_complex_foot_index_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+      slip_contact_points_;
+  const std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+      complex_contact_points_;
+  const std::map<int, std::vector<int>>
+      simple_foot_index_to_complex_foot_index_;
   const int n_q_;
   const int n_v_;
   const std::vector<bool> slip_contact_mask_;
   const int kSLIP_DIM = 3;
 };
-
diff --git a/multibody/kinematic/world_point_evaluator.h b/multibody/kinematic/world_point_evaluator.h
index 0421b363ff..58f7cb3b59 100644
--- a/multibody/kinematic/world_point_evaluator.h
+++ b/multibody/kinematic/world_point_evaluator.h
@@ -93,8 +93,8 @@ class WorldPointEvaluator : public KinematicEvaluator<T> {
   /// evaluator's output.
   void set_frictional() { is_frictional_ = true; };
 
-  const drake::multibody::Frame<T> & get_frame() const {return frame_A_;};
-  const Eigen::Vector3d&  get_pt_A() const {return pt_A_;};
+  const drake::multibody::Frame<T>& get_frame() const { return frame_A_; };
+  const Eigen::Vector3d& get_pt_A() const { return pt_A_; };
 
  private:
   const Eigen::Vector3d pt_A_;
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc
index 60e421e72f..58c4bc71bc 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc
@@ -19,9 +19,8 @@ void KcmpcReferenceGenerator::Generate() {
   Eigen::Vector3d com = q_ref_.segment(4, 3) - p_ScmBase_W_;
   com_trajectory_ = GenerateComTrajectory(com, com_vel_knot_points_.samples,
                                           com_vel_knot_points_.times);
-  momentum_trajectory_ = GenerateMomentumTrajectory(com_vel_knot_points_.samples,
-                                          com_vel_knot_points_.times,
-                                          m_);
+  momentum_trajectory_ = GenerateMomentumTrajectory(
+      com_vel_knot_points_.samples, com_vel_knot_points_.times, m_);
   q_trajectory_ = GenerateGeneralizedPosTrajectory(q_ref_, p_ScmBase_W_,
                                                    com_trajectory_, 4);
   v_trajectory_ = GenerateGeneralizedVelTrajectory(com_trajectory_,
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
index fad5e70fc5..4ed7270e1d 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -36,7 +36,7 @@ KinematicCentroidalSolver::KinematicCentroidalSolver(
           Eigen::MatrixXd::Zero(3 * n_contact_points_, 3 * n_contact_points_)),
       contact_sequence_(n_knot_points),
       contexts_(n_knot_points),
-      complexity_schedule_(n_knot_points){
+      complexity_schedule_(n_knot_points) {
   n_joint_q_ = n_q_ - kCentroidalPosDim;
   n_joint_v_ = n_v_ - kCentroidalVelDim;
   prog_ = std::make_unique<drake::solvers::MathematicalProgram>();
@@ -66,7 +66,8 @@ KinematicCentroidalSolver::KinematicCentroidalSolver(
   std::vector<bool> stance_mode(n_contact_points_);
   std::fill(stance_mode.begin(), stance_mode.end(), true);
   std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
-  std::fill(complexity_schedule_.begin(), complexity_schedule_.end(), Complexity::KINEMATIC_CENTROIDAL);
+  std::fill(complexity_schedule_.begin(), complexity_schedule_.end(),
+            Complexity::KINEMATIC_CENTROIDAL);
 }
 
 void KinematicCentroidalSolver::SetGenPosReference(
@@ -100,7 +101,7 @@ void KinematicCentroidalSolver::SetMomentumReference(
 }
 
 void KinematicCentroidalSolver::AddCentroidalDynamics(int knot_point) {
-  if(!(is_last_knot(knot_point))){
+  if (!(is_last_knot(knot_point))) {
     auto constraint = std::make_shared<CentroidalDynamicsConstraint<double>>(
         plant_, contexts_[knot_point].get(), n_contact_points_, dt_,
         knot_point);
@@ -114,7 +115,7 @@ void KinematicCentroidalSolver::AddCentroidalDynamics(int knot_point) {
 }
 
 void KinematicCentroidalSolver::AddKinematicsIntegrator(int knot_point) {
-  if(!is_last_knot(knot_point)) {
+  if (!is_last_knot(knot_point)) {
     // Integrate generalized velocities to get generalized positions
     auto constraint = std::make_shared<KinematicIntegratorConstraint<double>>(
         plant_, contexts_[knot_point].get(), contexts_[knot_point + 1].get(),
@@ -166,7 +167,8 @@ void KinematicCentroidalSolver::AddContactConstraints(int knot_point) {
   }
 }
 
-void KinematicCentroidalSolver::AddCentroidalKinematicConsistency(int knot_point) {
+void KinematicCentroidalSolver::AddCentroidalKinematicConsistency(
+    int knot_point) {
   // Ensure linear and angular momentum line up
   auto centroidal_momentum =
       std::make_shared<CentroidalMomentumConstraint<double>>(
@@ -186,9 +188,8 @@ void KinematicCentroidalSolver::AddCentroidalKinematicConsistency(int knot_point
                           contact_pos_vars(knot_point, contact_index)});
   }
   // Constrain com position
-  auto com_position =
-      std::make_shared<CenterofMassPositionConstraint<double>>(
-          plant_, contexts_[knot_point].get(), knot_point);
+  auto com_position = std::make_shared<CenterofMassPositionConstraint<double>>(
+      plant_, contexts_[knot_point].get(), knot_point);
   prog_->AddConstraint(com_position,
                        {com_pos_vars(knot_point), state_vars(knot_point)});
 }
@@ -205,7 +206,8 @@ void KinematicCentroidalSolver::AddFrictionConeConstraints(int knot_point) {
   }
 }
 
-void KinematicCentroidalSolver::AddFlightContactForceConstraints(int knot_point) {
+void KinematicCentroidalSolver::AddFlightContactForceConstraints(
+    int knot_point) {
   for (int contact_index = 0; contact_index < n_contact_points_;
        contact_index++) {
     // Feet in flight produce no force
@@ -232,26 +234,27 @@ KinematicCentroidalSolver::joint_vel_vars(int knotpoint_index) const {
 
 void KinematicCentroidalSolver::Build(
     const drake::solvers::SolverOptions& solver_options) {
-  for(int knot_point = 0; knot_point < n_knot_points_; knot_point ++){
+  for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     switch (complexity_schedule_[knot_point]) {
       case KINEMATIC_CENTROIDAL:
-        //Add complex constraints
+        // Add complex constraints
         AddContactConstraints(knot_point);
         AddCentroidalKinematicConsistency(knot_point);
         AddFrictionConeConstraints(knot_point);
         AddFlightContactForceConstraints(knot_point);
-        //Add complex dynamics
+        // Add complex dynamics
         AddCentroidalDynamics(knot_point);
         AddKinematicsIntegrator(knot_point);
-        if(!is_last_knot(knot_point) and complexity_schedule_[knot_point+1] == PLANAR_SLIP){
+        if (!is_last_knot(knot_point) and
+            complexity_schedule_[knot_point + 1] == PLANAR_SLIP) {
           AddSlipReductionConstraint(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
         }
         break;
       case PLANAR_SLIP:
         AddPlanarSlipConstraints(knot_point);
-        if(!is_last_knot(knot_point)){
-          switch (complexity_schedule_[knot_point+1]) {
+        if (!is_last_knot(knot_point)) {
+          switch (complexity_schedule_[knot_point + 1]) {
             case KINEMATIC_CENTROIDAL:
               AddCentroidalDynamics(knot_point);
               AddKinematicsIntegrator(knot_point);
@@ -312,8 +315,7 @@ void KinematicCentroidalSolver::AddCosts() {
         }
         if (contact_ref_traj_) {
           prog_->AddQuadraticErrorCost(
-              knot_point_gain * Q_contact_,
-              contact_ref_traj_->value(t),
+              knot_point_gain * Q_contact_, contact_ref_traj_->value(t),
               {contact_pos_[knot_point], contact_vel_[knot_point]});
         }
         if (force_ref_traj_) {
@@ -329,7 +331,7 @@ void KinematicCentroidalSolver::AddCosts() {
   }
 }
 
-void KinematicCentroidalSolver::UpdateCosts() { DRAKE_DEMAND(false);}
+void KinematicCentroidalSolver::UpdateCosts() { DRAKE_DEMAND(false); }
 
 void KinematicCentroidalSolver::SetZeroInitialGuess() {
   Eigen::VectorXd initialGuess =
@@ -625,7 +627,8 @@ void KinematicCentroidalSolver::SetForceGuess(
 }
 
 void KinematicCentroidalSolver::SetMomentumGuess(
-    const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory) {
+    const drake::trajectories::PiecewisePolynomial<double>&
+        momentum_trajectory) {
   DRAKE_DEMAND(momentum_trajectory.rows() == 6);
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
     prog_->SetInitialGuess(mom_vars_[knot_point],
@@ -640,7 +643,8 @@ void KinematicCentroidalSolver::SetModeSequence(
 void KinematicCentroidalSolver::SetModeSequence(
     const drake::trajectories::PiecewisePolynomial<double>& contact_sequence) {
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    for (int contact_index = 0; contact_index < n_contact_points_; contact_index++) {
+    for (int contact_index = 0; contact_index < n_contact_points_;
+         contact_index++) {
       contact_sequence_[knot_point][contact_index] =
           contact_sequence.value(dt_ * knot_point).coeff(contact_index);
     }
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
index bdc2f853bc..bc296ca003 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
@@ -15,10 +15,7 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/systems/framework/context.h"
 
-enum Complexity{
-  KINEMATIC_CENTROIDAL,
-  PLANAR_SLIP
-};
+enum Complexity { KINEMATIC_CENTROIDAL, PLANAR_SLIP };
 
 /*!
  * @brief Class for solving nonlinear kinematic centroidal mpc. Implementation
@@ -271,14 +268,16 @@ class KinematicCentroidalSolver {
   virtual void SetComPositionGuess(
       const drake::trajectories::PiecewisePolynomial<double>& com_trajectory);
 
-  virtual void SetContactGuess(const drake::trajectories::PiecewisePolynomial<double>&
-                           contact_trajectory);
+  virtual void SetContactGuess(
+      const drake::trajectories::PiecewisePolynomial<double>&
+          contact_trajectory);
 
   virtual void SetForceGuess(
       const drake::trajectories::PiecewisePolynomial<double>& force_trajectory);
 
   virtual void SetMomentumGuess(
-      const drake::trajectories::PiecewisePolynomial<double>& momentum_trajectory);
+      const drake::trajectories::PiecewisePolynomial<double>&
+          momentum_trajectory);
 
   void CreateVisualizationCallback(const std::string& model_file, double alpha,
                                    const std::string& weld_frame_to_world = "");
@@ -327,7 +326,8 @@ class KinematicCentroidalSolver {
    * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point`
    * is `contact_index` active
    */
-  virtual void SetModeSequence(const std::vector<std::vector<bool>>& contact_sequence);
+  virtual void SetModeSequence(
+      const std::vector<std::vector<bool>>& contact_sequence);
 
   /*!
    * @brief Set the mode sequence via a trajectory. The value of the trajectory
@@ -349,7 +349,11 @@ class KinematicCentroidalSolver {
    */
   void UpdateCosts();
 
-  void SetComplexitySchedule(const std::vector<Complexity>& complexity_schedule){complexity_schedule_ = complexity_schedule;};
+  void SetComplexitySchedule(
+      const std::vector<Complexity>& complexity_schedule) {
+    complexity_schedule_ = complexity_schedule;
+  };
+
  protected:
   /*!
    * @brief Adds dynamics for centroidal state
@@ -389,17 +393,27 @@ class KinematicCentroidalSolver {
    */
   double GetKnotpointGain(int knot_point) const;
 
-  virtual void AddPlanarSlipConstraints(int knot_point){ DRAKE_DEMAND(false);};
+  virtual void AddPlanarSlipConstraints(int knot_point) {
+    DRAKE_DEMAND(false);
+  };
 
-  virtual void AddPlanarSlipCost(int knot_point, double terminal_gain){ DRAKE_DEMAND(false);};
+  virtual void AddPlanarSlipCost(int knot_point, double terminal_gain) {
+    DRAKE_DEMAND(false);
+  };
 
-  virtual void AddSlipReductionConstraint(int knot_point){ DRAKE_DEMAND(false);};
+  virtual void AddSlipReductionConstraint(int knot_point) {
+    DRAKE_DEMAND(false);
+  };
 
-  virtual void AddSlipLiftingConstraint(int knot_point){ DRAKE_DEMAND(false);};
+  virtual void AddSlipLiftingConstraint(int knot_point) {
+    DRAKE_DEMAND(false);
+  };
 
-  virtual void AddSlipDynamics(int knot_point){ DRAKE_DEMAND(false);};
+  virtual void AddSlipDynamics(int knot_point) { DRAKE_DEMAND(false); };
 
-  virtual drake::VectorX<double> LiftSlipSolution(int knot_point){ DRAKE_DEMAND(false);};
+  virtual drake::VectorX<double> LiftSlipSolution(int knot_point) {
+    DRAKE_DEMAND(false);
+  };
 
   /*!
    * @brief Add costs from internally stored variables
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
index 5189a9262c..0de6b90368 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
@@ -1,12 +1,11 @@
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h"
 
-#include "multibody/multibody_utils.h"
 #include "examples/Cassie/cassie_utils.h"
+#include "multibody/multibody_utils.h"
 
-
-std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<
-    double> &plant,
-                                                                                                                  double mu) {
+std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant,
+                    double mu) {
   auto left_toe_pair = dairlib::LeftToeFront(plant);
   auto left_heel_pair = dairlib::LeftToeRear(plant);
   auto right_toe_pair = dairlib::RightToeFront(plant);
@@ -58,9 +57,9 @@ drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(
       time_points, samples);
 }
 
-drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(const std::vector<Eigen::Vector3d> &vel_ewrt_w,
-                                                                            const std::vector<double> &time_points,
-                                                                            double m) {
+drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(
+    const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+    const std::vector<double>& time_points, double m) {
   DRAKE_DEMAND(vel_ewrt_w.size() == (time_points.size() - 1));
   auto n_points = time_points.size();
   std::vector<drake::MatrixX<double>> samples(n_points);
@@ -72,7 +71,6 @@ drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(cons
       time_points, samples);
 }
 
-
 drake::trajectories::PiecewisePolynomial<double>
 GenerateGeneralizedPosTrajectory(
     const Eigen::VectorXd& nominal_stand, const Eigen::Vector3d& p_ScmBase_W,
@@ -107,7 +105,7 @@ GenerateGeneralizedVelTrajectory(
 drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(
     const std::vector<Gait>& gait_sequence,
     const std::vector<double>& time_points) {
-  DRAKE_DEMAND(gait_sequence.size() == time_points.size()-1);
+  DRAKE_DEMAND(gait_sequence.size() == time_points.size() - 1);
 
   auto traj = gait_sequence[0].ToTrajectory(time_points[0], time_points[1]);
   for (int i = 1; i < gait_sequence.size(); i++) {
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
index ae139e44c4..251d815fb1 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
@@ -1,7 +1,8 @@
 #pragma once
 
-#include <drake/multibody/plant/multibody_plant.h>
 #include <drake/common/trajectories/piecewise_polynomial.h>
+#include <drake/multibody/plant/multibody_plant.h>
+
 #include "multibody/kinematic/world_point_evaluator.h"
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h"
 
@@ -11,83 +12,101 @@
  * @param mu coefficient of friction
  * @return
  */
-std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant, double mu);
-
+std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant,
+                    double mu);
 
 /*!
- * @brief given an initial com and velocity of com emrt w, calculate a com trajectory
+ * @brief given an initial com and velocity of com emrt w, calculate a com
+ * trajectory
  * @param current_com
- * @param vel_ewrt_w velocity of the com ewrt. Given a zoh between time points, must be the same length as time_points
- * @param time_points time values correspeding to velocities, trajectory start at the first, and end at the last, must be the same length as vel_ewrt_w
+ * @param vel_ewrt_w velocity of the com ewrt. Given a zoh between time points,
+ * must be the same length as time_points
+ * @param time_points time values correspeding to velocities, trajectory start
+ * at the first, and end at the last, must be the same length as vel_ewrt_w
  * @return trajectory of com velocity
  */
-drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(const Eigen::Vector3d& current_com,
-                                                                       const std::vector<Eigen::Vector3d>& vel_ewrt_w,
-                                                                       const std::vector<double>& time_points);
+drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(
+    const Eigen::Vector3d& current_com,
+    const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+    const std::vector<double>& time_points);
 
-drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(const std::vector<Eigen::Vector3d> &vel_ewrt_w,
-                                                                            const std::vector<double> &time_points,
-                                                                            double m);
+drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(
+    const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+    const std::vector<double>& time_points, double m);
 
 /*!
- * @brief Constructs a trajectory for the generalized positions of a constant joint state and floating base position from com trajectory
+ * @brief Constructs a trajectory for the generalized positions of a constant
+ * joint state and floating base position from com trajectory
  * @param nominal_stand [nq] generalized state
  * @param p_ScmBase_W vector from com to floating base in world frame
  * @param com_traj trajectory for the center of mass
- * @param base_pos_start index where the floating base position starts in generalized state
+ * @param base_pos_start index where the floating base position starts in
+ * generalized state
  * @return trajectory of generalized position
  */
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand,
-                                                                                  const Eigen::Vector3d& p_ScmBase_W,
-                                                                                  const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                  int base_pos_start);
+drake::trajectories::PiecewisePolynomial<double>
+GenerateGeneralizedPosTrajectory(
+    const Eigen::VectorXd& nominal_stand, const Eigen::Vector3d& p_ScmBase_W,
+    const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+    int base_pos_start);
 
 /*!
- * @brief constructs a trajectory for the generalized velocity where the joint velocity is 0 and floating base val from com trajectory
+ * @brief constructs a trajectory for the generalized velocity where the joint
+ * velocity is 0 and floating base val from com trajectory
  * @param com_traj
  * @param n_v number of velocity states
  * @param base_vel_start index where base vel starts
  * @return trajectory of generalized velocity
  */
-drake::trajectories::PiecewisePolynomial<double> GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial<double>& com_traj,
-                                                                                  int n_v,
-                                                                                  int base_vel_start);
+drake::trajectories::PiecewisePolynomial<double>
+GenerateGeneralizedVelTrajectory(
+    const drake::trajectories::PiecewisePolynomial<double>& com_traj, int n_v,
+    int base_vel_start);
 
 /*!
- * @brief given a vector of gaits and time points corresponding to when the gaits are active generate a mode sequence.
- *      The transition between gaits can be awkward depending on where the gaits are in phase space during the transition
- *      {TODO SRL} make transition clean by attempting to shift phase so mode sequence lines up
+ * @brief given a vector of gaits and time points corresponding to when the
+ * gaits are active generate a mode sequence. The transition between gaits can
+ * be awkward depending on where the gaits are in phase space during the
+ * transition {TODO SRL} make transition clean by attempting to shift phase so
+ * mode sequence lines up
  * @param gait_sequence vector of gaits
  * @param time_points vector of time points when each gait is active
- * @return trajectory of mode status start at time_point(0) and ending at time_point.end()-1
+ * @return trajectory of mode status start at time_point(0) and ending at
+ * time_point.end()-1
  */
-drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(const std::vector<Gait>& gait_sequence,
-                                                                      const std::vector<double>& time_points);
+drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(
+    const std::vector<Gait>& gait_sequence,
+    const std::vector<double>& time_points);
 
 /*!
- * @brief given a trajectory which describes the mode, and the mass of the robot calculate a nominal grf trajectory where the weight of the robot
- *          is distributed over the active contact points
+ * @brief given a trajectory which describes the mode, and the mass of the robot
+ * calculate a nominal grf trajectory where the weight of the robot is
+ * distributed over the active contact points
  * @param mode_trajectory
  * @param m
  * @return trajectory of grf for reference
  */
-drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
-                                                                      double m);
+drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(
+    const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
+    double m);
 
 /*!
- * @brief Calculate trajectory of world point evaluators from generalized state trajectory. This assumes first order hold
- * between knot points in state trajectories.
- * TODO If we start using more complicated state references with this function sample time more coarsely
+ * @brief Calculate trajectory of world point evaluators from generalized state
+ * trajectory. This assumes first order hold between knot points in state
+ * trajectories.
+ * TODO If we start using more complicated state references with this function
+ * sample time more coarsely
  * @param plant
  * @param contacts vector of world point evaluators
  * @param q_traj generalized position trajectory
  * @param v_traj generalized velocity trajectory
- * @return trajectory of contact points stacked [contact1_pos, contact1_vel, ... contact_n_pos, contact_n_vel]
+ * @return trajectory of contact points stacked [contact1_pos, contact1_vel, ...
+ * contact_n_pos, contact_n_vel]
  */
-drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(const drake::multibody::MultibodyPlant<double> &plant,
-                                                                               const std::vector<dairlib::multibody::WorldPointEvaluator<
-                                                                                   double>> &contacts,
-                                                                               const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &q_traj,
-                                                                               const drake::trajectories::PiecewisePolynomial<
-                                                                                   double> &v_traj);
+drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+        contacts,
+    const drake::trajectories::PiecewisePolynomial<double>& q_traj,
+    const drake::trajectories::PiecewisePolynomial<double>& v_traj);

From 73012838535f497cfd46a9bad36cebb99606c284 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 14 Dec 2022 18:19:19 -0500
Subject: [PATCH 68/76] Cleaning up code for pr

---
 .../kinematic_centroidal_planner/BUILD.bazel  |  2 +-
 .../cassie_kcmpc_trajopt.cc                   |  2 +-
 .../cassie_kinematic_centroidal_solver.cc     | 21 +++++----
 .../cassie_kinematic_centroidal_solver.h      | 12 ++---
 .../simple_models/BUILD.bazel                 | 22 ++++-----
 ...lip_constraints.cc => slip_constraints.cc} | 41 ++++++++---------
 ..._slip_constraints.h => slip_constraints.h} | 45 +++++++++----------
 .../{planar_slip_lifter.cc => slip_lifter.cc} | 21 ++++-----
 .../{planar_slip_lifter.h => slip_lifter.h}   | 25 +++++------
 ...p_lifting_test.cc => slip_lifting_test.cc} | 16 +++----
 ...planar_slip_reducer.cc => slip_reducer.cc} | 13 +++---
 .../{planar_slip_reducer.h => slip_reducer.h} |  4 +-
 .../simple_models/slip_utils.cc               | 21 +++++++++
 .../simple_models/slip_utils.h                |  4 ++
 .../kinematic_centroidal_solver.cc            | 14 +++---
 .../kinematic_centroidal_solver.h             |  8 ++--
 16 files changed, 147 insertions(+), 124 deletions(-)
 rename examples/Cassie/kinematic_centroidal_planner/simple_models/{planar_slip_constraints.cc => slip_constraints.cc} (89%)
 rename examples/Cassie/kinematic_centroidal_planner/simple_models/{planar_slip_constraints.h => slip_constraints.h} (72%)
 rename examples/Cassie/kinematic_centroidal_planner/simple_models/{planar_slip_lifter.cc => slip_lifter.cc} (95%)
 rename examples/Cassie/kinematic_centroidal_planner/simple_models/{planar_slip_lifter.h => slip_lifter.h} (82%)
 rename examples/Cassie/kinematic_centroidal_planner/simple_models/{planar_slip_lifting_test.cc => slip_lifting_test.cc} (95%)
 rename examples/Cassie/kinematic_centroidal_planner/simple_models/{planar_slip_reducer.cc => slip_reducer.cc} (93%)
 rename examples/Cassie/kinematic_centroidal_planner/simple_models/{planar_slip_reducer.h => slip_reducer.h} (97%)
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
 create mode 100644 examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h

diff --git a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
index 4c15a2b400..c0d87c6b48 100644
--- a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
@@ -97,7 +97,7 @@ cc_library(
         "//common",
         "//systems/trajectory_optimization/kinematic_centroidal_planner",
         "//multibody:visualization_utils",
-        "//examples/Cassie/kinematic_centroidal_planner/simple_models:planar_slip",
+        "//examples/Cassie/kinematic_centroidal_planner/simple_models:slip",
         "@drake//:drake_shared_library",
     ],
 )
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index c057348639..6064342706 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -118,7 +118,7 @@ int DoMain(int argc, char* argv[]) {
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),
             Complexity::KINEMATIC_CENTROIDAL);
   for (int i = 1; i < traj_params.n_knot_points; i++) {
-    complexity_schedule[i] = Complexity::PLANAR_SLIP;
+    complexity_schedule[i] = Complexity::SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
 
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index 432d325271..41589afbe2 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -2,7 +2,7 @@
 
 #include "common/find_resource.h"
 #include "examples/Cassie/cassie_utils.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h"
 
 #include "drake/multibody/parsing/parser.h"
 
@@ -41,7 +41,7 @@ void CassieKinematicCentroidalSolver::AddLoopClosure() {
   }
 }
 
-void CassieKinematicCentroidalSolver::AddPlanarSlipConstraints(int knot_point) {
+void CassieKinematicCentroidalSolver::AddSlipConstraints(int knot_point) {
   for (int contact_index = 0; contact_index < slip_contact_points_.size();
        contact_index++) {
     prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) -
@@ -77,8 +77,8 @@ void CassieKinematicCentroidalSolver::AddPlanarSlipConstraints(int knot_point) {
   }
 }
 
-void CassieKinematicCentroidalSolver::AddPlanarSlipCost(int knot_point,
-                                                        double terminal_gain) {
+void CassieKinematicCentroidalSolver::AddSlipCost(int knot_point,
+                                                  double terminal_gain) {
   const double t = dt_ * knot_point;
   if (com_ref_traj_) {
     prog_->AddQuadraticErrorCost(terminal_gain * Q_com_,
@@ -173,7 +173,7 @@ void CassieKinematicCentroidalSolver::AddSlipReductionConstraint(
 }
 
 void CassieKinematicCentroidalSolver::AddSlipLiftingConstraint(int knot_point) {
-  auto lifting_constraint = std::make_shared<PlanarSlipLiftingConstraint>(
+  auto lifting_constraint = std::make_shared<SlipLiftingConstraint>(
       plant_, lifters_[knot_point], 2, n_contact_points_, knot_point);
   prog_->AddConstraint(
       lifting_constraint,
@@ -187,10 +187,9 @@ void CassieKinematicCentroidalSolver::AddSlipLiftingConstraint(int knot_point) {
 
 void CassieKinematicCentroidalSolver::AddSlipDynamics(int knot_point) {
   if (!is_last_knot(knot_point)) {
-    auto slip_com_dynamics =
-        std::make_shared<PlanarSlipDynamicsConstraint<double>>(
-            r0_, k_, b_, m_, 2, slip_contact_sequence_[knot_point],
-            slip_contact_sequence_[knot_point + 1], dt_, knot_point);
+    auto slip_com_dynamics = std::make_shared<SlipDynamicsConstraint<double>>(
+        r0_, k_, b_, m_, 2, slip_contact_sequence_[knot_point],
+        slip_contact_sequence_[knot_point + 1], dt_, knot_point);
 
     slip_dynamics_binding_.push_back(prog_->AddConstraint(
         slip_com_dynamics,
@@ -301,11 +300,11 @@ void CassieKinematicCentroidalSolver::SetForceGuess(
 void CassieKinematicCentroidalSolver::Build(
     const drake::solvers::SolverOptions& solver_options) {
   for (int knot = 0; knot < n_knot_points_; knot++) {
-    lifters_.emplace_back(new PlanarSlipLifter(
+    lifters_.emplace_back(new SlipLifter(
         plant_, contexts_[knot].get(), slip_contact_points_,
         CreateContactPoints(plant_, 0), {{0, {0, 1}}, {1, {2, 3}}},
         nominal_stand_, k_, b_, r0_, slip_contact_sequence_[knot]));
-    reducers.emplace_back(new PlanarSlipReducer(
+    reducers.emplace_back(new SlipReducer(
         plant_, contexts_[knot].get(), slip_contact_points_,
         CreateContactPoints(plant_, 0), {{0, {0, 1}}, {1, {2, 3}}}, k_, b_, r0_,
         slip_contact_sequence_[knot]));
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index 503a42249b..ae8ab5f44f 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -2,8 +2,8 @@
 #include <iostream>
 
 #include "examples/Cassie/cassie_utils.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h"
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h"
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h"
 
@@ -100,9 +100,9 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
  private:
   void MapModeSequence();
 
-  void AddPlanarSlipConstraints(int knot_point) override;
+  void AddSlipConstraints(int knot_point) override;
 
-  void AddPlanarSlipCost(int knot_point, double terminal_gain) override;
+  void AddSlipCost(int knot_point, double terminal_gain) override;
 
   void AddSlipReductionConstraint(int knot_point) override;
 
@@ -135,8 +135,8 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
   std::vector<drake::solvers::VectorXDecisionVariable> slip_contact_vel_vars_;
   std::vector<drake::solvers::VectorXDecisionVariable> slip_force_vars_;
 
-  std::vector<std::shared_ptr<PlanarSlipLifter>> lifters_;
-  std::vector<std::shared_ptr<PlanarSlipReducer>> reducers;
+  std::vector<std::shared_ptr<SlipLifter>> lifters_;
+  std::vector<std::shared_ptr<SlipReducer>> reducers;
 
   std::vector<dairlib::multibody::WorldPointEvaluator<double>>
       slip_contact_points_;
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
index ef4f486ddf..b18e0ac35a 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel
@@ -2,13 +2,15 @@ package(default_visibility = ["//visibility:public"])
 
 
 cc_library(
-    name = "planar_slip",
-    srcs = ["planar_slip_constraints.cc",
-            "planar_slip_lifter.cc",
-            "planar_slip_reducer.cc"],
-    hdrs = ["planar_slip_constraints.h",
-            "planar_slip_lifter.h",
-            "planar_slip_reducer.h"],
+    name = "slip",
+    srcs = ["slip_constraints.cc",
+            "slip_lifter.cc",
+            "slip_reducer.cc",
+            "slip_utils.cc"],
+    hdrs = ["slip_constraints.h",
+            "slip_lifter.h",
+            "slip_reducer.h",
+            "slip_utils.h"],
     deps = [
         "//solvers:constraints",
         "//multibody/kinematic:kinematic",
@@ -19,11 +21,11 @@ cc_library(
 )
 
 cc_binary(
-    name = "cassie_planar_slip_lifting_test",
-    srcs = ["planar_slip_lifting_test.cc"],
+    name = "cassie_slip_lifting_test",
+    srcs = ["slip_lifting_test.cc"],
     deps = [
         "//examples/Cassie:cassie_utils",
-        "planar_slip",
+        "slip",
         "//common",
         "//multibody:visualization_utils",
         "//multibody/kinematic",
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
similarity index 89%
rename from examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
index 48ccef1e9d..5f873c5fdb 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
@@ -1,13 +1,17 @@
-#include "planar_slip_constraints.h"
+#include "slip_constraints.h"
 
 #include <iostream>
 #include <utility>
 
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h"
 #include "multibody/multibody_utils.h"
 
-PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(
+double SlipGrf(double k, double r0, double b, double r, double dr,
+               double force) {}
+
+SlipReductionConstraint::SlipReductionConstraint(
     const drake::multibody::MultibodyPlant<double>& plant,
-    std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
+    std::shared_ptr<SlipReducer> reducing_function, int n_slip_feet,
     int n_complex_feet, int knot_index)
     : dairlib::solvers::NonlinearConstraint<double>(
           3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet,
@@ -16,7 +20,7 @@ PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(
               plant.num_velocities(),
           Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
           Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet),
-          "PlanarSlipReductionConstraint[" + std::to_string(knot_index) + "]"),
+          "SlipReductionConstraint[" + std::to_string(knot_index) + "]"),
       reducing_function_(reducing_function),
       slip_dim_(3 + 3 + 2 * 3 * n_slip_feet + n_slip_feet),
       complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
@@ -36,7 +40,7 @@ PlanarSlipReductionConstraint::PlanarSlipReductionConstraint(
 ///     complex_contact_force
 ///     complex_gen_pos
 ///     complex_gen_vel
-void PlanarSlipReductionConstraint::EvaluateConstraint(
+void SlipReductionConstraint::EvaluateConstraint(
     const Eigen::Ref<const drake::VectorX<double>>& x,
     drake::VectorX<double>* y) const {
   const auto& slip_state = x.head(slip_dim_);
@@ -46,7 +50,7 @@ void PlanarSlipReductionConstraint::EvaluateConstraint(
 
 SlipGrfReductionConstrain::SlipGrfReductionConstrain(
     const drake::multibody::MultibodyPlant<double>& plant,
-    std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
+    std::shared_ptr<SlipReducer> reducing_function, int n_slip_feet,
     int n_complex_feet, int knot_index)
     : dairlib::solvers::NonlinearConstraint<double>(
           n_slip_feet,
@@ -78,9 +82,9 @@ void SlipGrfReductionConstrain::EvaluateConstraint(
        slip_contact_force;
 }
 
-PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(
+SlipLiftingConstraint::SlipLiftingConstraint(
     const drake::multibody::MultibodyPlant<double>& plant,
-    std::shared_ptr<PlanarSlipLifter> lifting_function, int n_slip_feet,
+    std::shared_ptr<SlipLifter> lifting_function, int n_slip_feet,
     int n_complex_feet, int knot_index)
     : dairlib::solvers::NonlinearConstraint<double>(
           6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
@@ -91,7 +95,7 @@ PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(
                                 plant.num_positions() + plant.num_velocities()),
           Eigen::VectorXd::Zero(6 + 3 + 3 * 3 * n_complex_feet +
                                 plant.num_positions() + plant.num_velocities()),
-          "PlanarSlipLiftingConstraint[" + std::to_string(knot_index) + "]"),
+          "SlipLiftingConstraint[" + std::to_string(knot_index) + "]"),
       lifting_function_(std::move(lifting_function)),
       slip_dim_(2 + 2 + 2 * 2 * n_slip_feet + n_slip_feet),
       complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
@@ -111,7 +115,7 @@ PlanarSlipLiftingConstraint::PlanarSlipLiftingConstraint(
 ///     complex_contact_force
 ///     complex_gen_pos
 ///     complex_gen_vel
-void PlanarSlipLiftingConstraint::EvaluateConstraint(
+void SlipLiftingConstraint::EvaluateConstraint(
     const Eigen::Ref<const drake::VectorX<double>>& x,
     drake::VectorX<double>* y) const {
   const auto& slip_state = x.head(slip_dim_);
@@ -120,14 +124,14 @@ void PlanarSlipLiftingConstraint::EvaluateConstraint(
 }
 
 template <typename T>
-PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(
+SlipDynamicsConstraint<T>::SlipDynamicsConstraint(
     double r0, double k, double b, T m, int n_feet,
     std::vector<bool> contact_mask0, std::vector<bool> contact_mask1, double dt,
     int knot_index)
     : dairlib::solvers::NonlinearConstraint<T>(
           3 + 3, 2 * (3 + 3 + 3 * n_feet + n_feet), Eigen::VectorXd::Zero(6),
           Eigen::VectorXd::Zero(6),
-          "PlanarSlipDynamicsConstraint[" + std::to_string(knot_index) + "]"),
+          "SlipDynamicsConstraint[" + std::to_string(knot_index) + "]"),
       r0_(r0),
       k_(k),
       b_(b),
@@ -147,7 +151,7 @@ PlanarSlipDynamicsConstraint<T>::PlanarSlipDynamicsConstraint(
 ///   - contact_pos1, active contact positions at time k+1
 ///   - force1, slip force in parallel with spring at time k+1
 template <typename T>
-void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(
+void SlipDynamicsConstraint<T>::EvaluateConstraint(
     const Eigen::Ref<const drake::VectorX<T>>& x, drake::VectorX<T>* y) const {
   const auto& com0 = x.head(3);
   const auto& vel0 = x.segment(3, 3);
@@ -174,7 +178,7 @@ void PlanarSlipDynamicsConstraint<T>::EvaluateConstraint(
 }
 
 template <typename T>
-drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
+drake::VectorX<T> SlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
     const drake::VectorX<T>& com_position, const drake::VectorX<T>& com_vel,
     const drake::VectorX<T>& contact_loc, const drake::VectorX<T>& slip_force,
     const std::vector<bool>& contact_mask) const {
@@ -186,10 +190,7 @@ drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
       const auto r = com_rt_foot.norm();
       const auto unit_vec = com_rt_foot / r;
       const auto dr = com_vel.dot(unit_vec);
-      auto F = k_ * (r0_ - r) + slip_force[foot] - b_ * dr;
-      if (F < 0) {
-        F = 0;
-      }
+      auto F = SlipGrf<T>(k_, r0_, b_, r, dr, slip_force[foot]);
       ddcom = ddcom + F * unit_vec / m_;
     }
   }
@@ -199,7 +200,7 @@ drake::VectorX<T> PlanarSlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
 }
 
 QuadraticLiftedCost::QuadraticLiftedCost(
-    std::shared_ptr<PlanarSlipLifter> lifting_function,
+    std::shared_ptr<SlipLifter> lifting_function,
     QuadraticLiftedCost::cost_element com_cost,
     QuadraticLiftedCost::cost_element momentum_cost,
     QuadraticLiftedCost::cost_element contact_cost,
@@ -251,4 +252,4 @@ Eigen::Matrix<double, -1, 1, 0> QuadraticLiftedCost::CalcCost(
 }
 
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
-    class PlanarSlipDynamicsConstraint);
+    class SlipDynamicsConstraint);
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
similarity index 72%
rename from examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
index d6a20b29fe..68fb18219d 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
@@ -2,8 +2,8 @@
 
 #include <drake/multibody/plant/multibody_plant.h>
 
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h"
 #include "multibody/kinematic/kinematic_evaluator_set.h"
 #include "multibody/kinematic/world_point_evaluator.h"
 #include "solvers/nonlinear_constraint.h"
@@ -13,19 +13,18 @@
 #include "drake/common/symbolic.h"
 #include "drake/solvers/constraint.h"
 
-class PlanarSlipReductionConstraint
+class SlipReductionConstraint
     : public dairlib::solvers::NonlinearConstraint<double> {
  public:
-  PlanarSlipReductionConstraint(
-      const drake::multibody::MultibodyPlant<double>& plant,
-      std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
-      int n_complex_feet, int knot_index);
+  SlipReductionConstraint(const drake::multibody::MultibodyPlant<double>& plant,
+                          std::shared_ptr<SlipReducer> reducing_function,
+                          int n_slip_feet, int n_complex_feet, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
                           drake::VectorX<double>* y) const override;
 
-  std::shared_ptr<PlanarSlipReducer> reducing_function_;
+  std::shared_ptr<SlipReducer> reducing_function_;
   const int slip_dim_;
   const int complex_dim_;
 };
@@ -35,43 +34,41 @@ class SlipGrfReductionConstrain
  public:
   SlipGrfReductionConstrain(
       const drake::multibody::MultibodyPlant<double>& plant,
-      std::shared_ptr<PlanarSlipReducer> reducing_function, int n_slip_feet,
+      std::shared_ptr<SlipReducer> reducing_function, int n_slip_feet,
       int n_complex_feet, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
                           drake::VectorX<double>* y) const override;
 
-  std::shared_ptr<PlanarSlipReducer> reducing_function_;
+  std::shared_ptr<SlipReducer> reducing_function_;
   const int n_slip_feet_;
   const int n_complex_feet_;
 };
 
-class PlanarSlipLiftingConstraint
+class SlipLiftingConstraint
     : public dairlib::solvers::NonlinearConstraint<double> {
  public:
-  PlanarSlipLiftingConstraint(
-      const drake::multibody::MultibodyPlant<double>& plant,
-      std::shared_ptr<PlanarSlipLifter> lifting_function, int n_slip_feet,
-      int n_complex_feet, int knot_index);
+  SlipLiftingConstraint(const drake::multibody::MultibodyPlant<double>& plant,
+                        std::shared_ptr<SlipLifter> lifting_function,
+                        int n_slip_feet, int n_complex_feet, int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
                           drake::VectorX<double>* y) const override;
 
-  std::shared_ptr<PlanarSlipLifter> lifting_function_;
+  std::shared_ptr<SlipLifter> lifting_function_;
   const int slip_dim_;
   const int complex_dim_;
 };
 
 template <typename T>
-class PlanarSlipDynamicsConstraint
-    : public dairlib::solvers::NonlinearConstraint<T> {
+class SlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
  public:
-  PlanarSlipDynamicsConstraint(double r0, double k, double b, T m, int n_feet,
-                               std::vector<bool> contact_mask0,
-                               std::vector<bool> contact_mask1, double dt,
-                               int knot_index);
+  SlipDynamicsConstraint(double r0, double k, double b, T m, int n_feet,
+                         std::vector<bool> contact_mask0,
+                         std::vector<bool> contact_mask1, double dt,
+                         int knot_index);
 
  private:
   void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<T>>& x,
@@ -107,7 +104,7 @@ class QuadraticLiftedCost : public dairlib::solvers::NonlinearCost<double> {
   };
 
  public:
-  QuadraticLiftedCost(std::shared_ptr<PlanarSlipLifter> lifting_function,
+  QuadraticLiftedCost(std::shared_ptr<SlipLifter> lifting_function,
                       cost_element com_cost, cost_element momentum_cost,
                       cost_element contact_cost, cost_element grf_cost,
                       cost_element q_cost, cost_element v_cost,
@@ -121,7 +118,7 @@ class QuadraticLiftedCost : public dairlib::solvers::NonlinearCost<double> {
       const cost_element& cost,
       const Eigen::Ref<const drake::VectorX<double>>& x) const;
 
-  std::shared_ptr<PlanarSlipLifter> lifting_function_;
+  std::shared_ptr<SlipLifter> lifting_function_;
   const cost_element com_cost_;
   const cost_element momentum_cost_;
   const cost_element contact_cost_;
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
similarity index 95%
rename from examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
index 61888fb7fc..aa240d6c89 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
@@ -1,12 +1,13 @@
-#include "planar_slip_lifter.h"
+#include "slip_lifter.h"
 
 #include <iostream>
 
 #include <drake/solvers/solve.h>
 
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h"
 #include "multibody/multibody_utils.h"
 
-PlanarSlipLifter::PlanarSlipLifter(
+SlipLifter::SlipLifter(
     const drake::multibody::MultibodyPlant<double>& plant,
     drake::systems::Context<double>* context,
     const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
@@ -62,7 +63,7 @@ PlanarSlipLifter::PlanarSlipLifter(
       M_PI * 13 / 180.0);
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(
+drake::VectorX<double> SlipLifter::LiftGeneralizedPosition(
     const drake::Vector3<double>& com_position,
     const drake::VectorX<double>& slip_feet_positions) const {
   DRAKE_DEMAND(slip_feet_positions.size() == 3 * slip_contact_points_.size());
@@ -109,7 +110,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedPosition(
   }
   return q_sol_normd;
 }
-drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(
+drake::VectorX<double> SlipLifter::LiftGeneralizedVelocity(
     const drake::VectorX<double>& generalized_pos,
     const drake::Vector3<double>& linear_momentum,
     const drake::Vector3<double>& com_pos,
@@ -174,7 +175,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGeneralizedVelocity(
   return rv;
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftContactPos(
+drake::VectorX<double> SlipLifter::LiftContactPos(
     const drake::VectorX<double>& generalized_position) const {
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_position,
                                                 context_);
@@ -185,7 +186,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftContactPos(
   return rv;
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftContactVel(
+drake::VectorX<double> SlipLifter::LiftContactVel(
     const drake::VectorX<double>& generalized_pos,
     const drake::VectorX<double>& generalized_vel) const {
   dairlib::multibody::SetPositionsIfNew<double>(plant_, generalized_pos,
@@ -200,7 +201,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftContactVel(
   return rv;
 }
 
-drake::VectorX<double> PlanarSlipLifter::LiftGrf(
+drake::VectorX<double> SlipLifter::LiftGrf(
     const drake::VectorX<double>& com_pos,
     const drake::VectorX<double>& com_vel,
     const drake::VectorX<double>& slip_feet_pos,
@@ -217,7 +218,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(
                     .dot(com_vel);
     double slip_grf_mag =
         slip_contact_mask_[simple_index]
-            ? slip_force[simple_index] + k_ * (r0_ - r) - b_ * dr
+            ? SlipGrf<double>(k_, r0_, b_, r, dr, slip_force[simple_index])
             : 0;
     if (slip_grf_mag < 0) {
       slip_grf_mag = 0;
@@ -262,7 +263,7 @@ drake::VectorX<double> PlanarSlipLifter::LiftGrf(
 ///     complex_contact_force
 ///     complex_gen_pos
 ///     complex_gen_vel
-void PlanarSlipLifter::Lift(
+void SlipLifter::Lift(
     const Eigen::Ref<const drake::VectorX<double>>& slip_state,
     drake::VectorX<double>* complex_state) const {
   const auto& slip_com = slip_state.head(kSLIP_DIM);
@@ -297,7 +298,7 @@ void PlanarSlipLifter::Lift(
               complex_contact_pos),
       generalized_pos, generalized_vel;
 }
-drake::VectorX<double> PlanarSlipLifter::Lift(
+drake::VectorX<double> SlipLifter::Lift(
     const Eigen::Ref<const drake::VectorX<double>>& slip_state) const {
   drake::VectorX<double> complex_state(
       6 + 3 + 3 * 3 * complex_contact_points_.size() + n_q_ + n_v_);
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h
similarity index 82%
rename from examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h
index e5a47de131..4fbcb8cc56 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h
@@ -12,19 +12,18 @@
 #include "drake/solvers/constraint.h"
 #include "drake/systems/trajectory_optimization/multiple_shooting.h"
 
-class PlanarSlipLifter {
+class SlipLifter {
  public:
-  PlanarSlipLifter(
-      const drake::multibody::MultibodyPlant<double>& plant,
-      drake::systems::Context<double>* context,
-      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
-          slip_contact_points,
-      const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
-          complex_contact_points,
-      const std::map<int, std::vector<int>>&
-          simple_foot_index_to_complex_foot_index,
-      const drake::VectorX<double>& nominal_stand, double k, double b,
-      double r0, const std::vector<bool>& contact_mask);
+  SlipLifter(const drake::multibody::MultibodyPlant<double>& plant,
+             drake::systems::Context<double>* context,
+             const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+                 slip_contact_points,
+             const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+                 complex_contact_points,
+             const std::map<int, std::vector<int>>&
+                 simple_foot_index_to_complex_foot_index,
+             const drake::VectorX<double>& nominal_stand, double k, double b,
+             double r0, const std::vector<bool>& contact_mask);
 
   void Lift(const Eigen::Ref<const drake::VectorX<double>>& slip_state,
             drake::VectorX<double>* complex_state) const;
@@ -39,7 +38,7 @@ class PlanarSlipLifter {
    * @note assumes identity orientation
    * @note implements numerical ik (can be slow)
    * @param com_position center of mass position in the world
-   * @param slip_feet_positions [2*n_slip_feet] locations of the planar slip
+   * @param slip_feet_positions [3*n_slip_feet] locations of the slip
    * feet
    * @return the generalized positions
    */
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
similarity index 95%
rename from examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
index daf328dd9a..03fdb4b627 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
@@ -14,9 +14,9 @@
 #include "common/find_resource.h"
 #include "examples/Cassie/cassie_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_constraints.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_lifter.h"
-#include "examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h"
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h"
 #include "multibody/visualization_utils.h"
 #include "systems/primitives/subvector_pass_through.h"
 
@@ -84,12 +84,12 @@ int main(int argc, char* argv[]) {
   //  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
 
   std::vector<bool> contact_mask = {true, true};
-  PlanarSlipLifter lifter(
+  SlipLifter lifter(
       plant, context.get(), {left_toe_eval, right_toe_eval},
       {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
       {{0, {0, 1}}, {1, {2, 3}}}, reference_state.head(plant.num_positions()),
       2000, 0, 0.5, contact_mask);
-  PlanarSlipReducer reducer(
+  SlipReducer reducer(
       plant, context.get(), {left_toe_eval, right_toe_eval},
       {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
       {{0, {0, 1}}, {1, {2, 3}}}, 2000, 0, 0.5, contact_mask);
@@ -118,8 +118,8 @@ int main(int argc, char* argv[]) {
   finish = std::chrono::high_resolution_clock::now();
   elapsed = finish - start;
   std::cout << "Solve time 2:" << elapsed.count() << std::endl;
-  auto constraint = PlanarSlipReductionConstraint(
-      plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
+  auto constraint = SlipReductionConstraint(
+      plant, std::make_shared<SlipReducer>(reducer), 2, 4, 0);
 
   drake::VectorX<double> error(slip_state.size());
   drake::VectorX<double> input(slip_state.size() + complex_state.size());
@@ -130,7 +130,7 @@ int main(int argc, char* argv[]) {
   //  std::cout<<error<<std::endl;
 
   auto grf_constraint = SlipGrfReductionConstrain(
-      plant, std::make_shared<PlanarSlipReducer>(reducer), 2, 4, 0);
+      plant, std::make_shared<SlipReducer>(reducer), 2, 4, 0);
   drake::VectorX<double> grf_error(2);
   drake::VectorX<double> grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3);
   //  grf_input << slip_com, slip_vel, slip_feet, complex_state.segment(3 + 6 +
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc
similarity index 93%
rename from examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc
index 4ad55af827..325a106db4 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc
@@ -1,10 +1,11 @@
-#include "planar_slip_reducer.h"
+#include "slip_reducer.h"
 
 #include <iostream>
 
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h"
 #include "multibody/multibody_utils.h"
 
-PlanarSlipReducer::PlanarSlipReducer(
+SlipReducer::SlipReducer(
     const drake::multibody::MultibodyPlant<double>& plant,
     drake::systems::Context<double>* context,
     const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
@@ -42,7 +43,7 @@ PlanarSlipReducer::PlanarSlipReducer(
 ///     slip_contact_pos
 ///     slip_contact_vel
 ///     slip_force
-void PlanarSlipReducer::Reduce(
+void SlipReducer::Reduce(
     const Eigen::Ref<const drake::VectorX<double>>& complex_state,
     drake::VectorX<double>* slip_state) const {
   const auto& complex_com = complex_state.segment(0, 3);
@@ -72,7 +73,7 @@ void PlanarSlipReducer::Reduce(
                 complex_grf);
 }
 
-drake::VectorX<double> PlanarSlipReducer::Reduce(
+drake::VectorX<double> SlipReducer::Reduce(
     const Eigen::Ref<const drake::VectorX<double>>& complex_state) const {
   drake::VectorX<double> slip_state(
       kSLIP_DIM + kSLIP_DIM + 2 * kSLIP_DIM * slip_contact_points_.size() +
@@ -81,7 +82,7 @@ drake::VectorX<double> PlanarSlipReducer::Reduce(
   return slip_state;
 }
 
-drake::VectorX<double> PlanarSlipReducer::ReduceGrf(
+drake::VectorX<double> SlipReducer::ReduceGrf(
     const Eigen::Ref<const drake::VectorX<double>>& complex_com,
     const Eigen::Ref<const drake::VectorX<double>>& com_vel,
     const Eigen::Ref<const drake::VectorX<double>>& slip_contact_pos,
@@ -94,7 +95,7 @@ drake::VectorX<double> PlanarSlipReducer::ReduceGrf(
     const Eigen::VectorXd unit_vec =
         (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized();
     const double dr = unit_vec.dot(com_vel);
-    const double spring_force = k_ * (r0_ - r) - b_ * dr;
+    const auto spring_force = SlipGrf<double>(k_, r0_, b_, r, dr, 0);
     auto complex_feet_it = simple_foot_index_to_complex_foot_index_.find(i);
     Eigen::Vector3d complex_force = Eigen::Vector3d::Zero(3);
     for (const auto complex_index : complex_feet_it->second) {
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h
similarity index 97%
rename from examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h
rename to examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h
index a9dd01655d..7bcec007ac 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/planar_slip_reducer.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h
@@ -12,9 +12,9 @@
 #include "drake/solvers/constraint.h"
 #include "drake/systems/trajectory_optimization/multiple_shooting.h"
 
-class PlanarSlipReducer {
+class SlipReducer {
  public:
-  PlanarSlipReducer(
+  SlipReducer(
       const drake::multibody::MultibodyPlant<double>& plant,
       drake::systems::Context<double>* context,
       const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
new file mode 100644
index 0000000000..7052e5de6b
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
@@ -0,0 +1,21 @@
+#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h"
+
+#include "multibody/multibody_utils.h"
+
+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;
+  if (F < 0) {
+    F = 0;
+  }
+  return F;
+}
+
+template
+double SlipGrf<double>(double k, double r0, double b, double r, double dr,
+                       double force);
+template
+drake::AutoDiffXd SlipGrf<drake::AutoDiffXd>(double k, double r0, double b,
+                                             drake::AutoDiffXd r,
+                                             drake::AutoDiffXd dr,
+                                             drake::AutoDiffXd force);
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h
new file mode 100644
index 0000000000..a297a92de2
--- /dev/null
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h
@@ -0,0 +1,4 @@
+#pragma once
+
+template <typename T>
+T SlipGrf(double k, double r0, double b, T r, T dr, T force);
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
index 4ed7270e1d..5dc048dbd6 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -246,13 +246,13 @@ void KinematicCentroidalSolver::Build(
         AddCentroidalDynamics(knot_point);
         AddKinematicsIntegrator(knot_point);
         if (!is_last_knot(knot_point) and
-            complexity_schedule_[knot_point + 1] == PLANAR_SLIP) {
+            complexity_schedule_[knot_point + 1] == SLIP) {
           AddSlipReductionConstraint(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
         }
         break;
-      case PLANAR_SLIP:
-        AddPlanarSlipConstraints(knot_point);
+      case SLIP:
+        AddSlipConstraints(knot_point);
         if (!is_last_knot(knot_point)) {
           switch (complexity_schedule_[knot_point + 1]) {
             case KINEMATIC_CENTROIDAL:
@@ -260,7 +260,7 @@ void KinematicCentroidalSolver::Build(
               AddKinematicsIntegrator(knot_point);
               AddSlipLiftingConstraint(knot_point);
               break;
-            case PLANAR_SLIP:
+            case SLIP:
               AddSlipDynamics(knot_point);
               break;
           }
@@ -324,8 +324,8 @@ void KinematicCentroidalSolver::AddCosts() {
                                        contact_force_[knot_point]);
         }
         break;
-      case PLANAR_SLIP:
-        AddPlanarSlipCost(knot_point, knot_point_gain);
+      case SLIP:
+        AddSlipCost(knot_point, knot_point_gain);
         break;
     }
   }
@@ -370,7 +370,7 @@ KinematicCentroidalSolver::Solve() {
       case KINEMATIC_CENTROIDAL:
         states.emplace_back(result_->GetSolution(state_vars(knot_point)));
         break;
-      case PLANAR_SLIP:
+      case SLIP:
         states.emplace_back(LiftSlipSolution(knot_point));
         break;
     }
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
index bc296ca003..5746cc4dd7 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
@@ -15,7 +15,7 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/systems/framework/context.h"
 
-enum Complexity { KINEMATIC_CENTROIDAL, PLANAR_SLIP };
+enum Complexity { KINEMATIC_CENTROIDAL, SLIP };
 
 /*!
  * @brief Class for solving nonlinear kinematic centroidal mpc. Implementation
@@ -393,11 +393,9 @@ class KinematicCentroidalSolver {
    */
   double GetKnotpointGain(int knot_point) const;
 
-  virtual void AddPlanarSlipConstraints(int knot_point) {
-    DRAKE_DEMAND(false);
-  };
+  virtual void AddSlipConstraints(int knot_point) { DRAKE_DEMAND(false); };
 
-  virtual void AddPlanarSlipCost(int knot_point, double terminal_gain) {
+  virtual void AddSlipCost(int knot_point, double terminal_gain) {
     DRAKE_DEMAND(false);
   };
 

From e19ce86a8eaf9cef29ad885757f57eabf18b4cd1 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Wed, 14 Dec 2022 23:43:38 -0500
Subject: [PATCH 69/76] Something seems broken

---
 .../cassie_kcmpc_trajopt.cc                   |  2 +-
 .../cassie_kinematic_centroidal_solver.cc     | 69 +++++++++++++++++++
 .../cassie_kinematic_centroidal_solver.h      |  9 ++-
 .../kinematic_centroidal_solver.cc            |  1 -
 4 files changed, 78 insertions(+), 3 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index 6064342706..97faeb0fa2 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -117,7 +117,7 @@ int DoMain(int argc, char* argv[]) {
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),
             Complexity::KINEMATIC_CENTROIDAL);
-  for (int i = 1; i < traj_params.n_knot_points; i++) {
+  for (int i = 15; i < traj_params.n_knot_points; i++) {
     complexity_schedule[i] = Complexity::SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index 41589afbe2..0d3dd24e24 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -170,6 +170,8 @@ void CassieKinematicCentroidalSolver::AddSlipReductionConstraint(
       {com_pos_vars(knot_point), slip_vel_vars_[knot_point],
        slip_contact_pos_vars_[knot_point], contact_force_[knot_point],
        slip_force_vars_[knot_point]});
+
+  AddSlipPosturePrincipleConstraint(knot_point);
 }
 
 void CassieKinematicCentroidalSolver::AddSlipLiftingConstraint(int knot_point) {
@@ -311,3 +313,70 @@ void CassieKinematicCentroidalSolver::Build(
   }
   KinematicCentroidalSolver::Build(solver_options);
 }
+void CassieKinematicCentroidalSolver::AddSlipPosturePrincipleConstraint(
+    int knot_point) {
+  const double eps = 1e-2;
+  // Zero hip yaw
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, x_vars_[knot_point][positions_map_.at("hip_yaw_left")]);
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, x_vars_[knot_point][positions_map_.at("hip_yaw_right")]);
+  //  // Zero hip yaw dot
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps,
+      state_vars(knot_point)[n_q_ + velocity_map_.at("hip_yaw_leftdot")]);
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps,
+      state_vars(knot_point)[n_q_ + velocity_map_.at("hip_yaw_rightdot")]);
+
+  //  // Identity orientation
+  prog_->AddBoundingBoxConstraint(
+      1 - eps, 1 + eps, state_vars(knot_point)[positions_map_.at("base_qw")]);
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qx")]);
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qy")]);
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qz")]);
+  //
+  // Zero angular velocity
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, state_vars(knot_point)[n_q_ + velocity_map_.at("base_wx")]);
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, state_vars(knot_point)[n_q_ + velocity_map_.at("base_wy")]);
+  prog_->AddBoundingBoxConstraint(
+      -eps, eps, state_vars(knot_point)[n_q_ + velocity_map_.at("base_wz")]);
+
+  // Toe and heel same velocity
+  prog_->AddConstraint(contact_vel_vars(knot_point, 0) ==
+                       contact_vel_vars(knot_point, 1));
+  prog_->AddConstraint(contact_vel_vars(knot_point, 2) ==
+                       contact_vel_vars(knot_point, 3));
+
+  // Toe and heel same height
+  prog_->AddConstraint(contact_pos_vars(knot_point, 0)[2] ==
+                       contact_pos_vars(knot_point, 1)[2]);
+  prog_->AddConstraint(contact_pos_vars(knot_point, 2)[2] ==
+                       contact_pos_vars(knot_point, 3)[2]);
+
+  //  // GRF per foot equal
+  prog_->AddConstraint(contact_force_vars(knot_point, 0) ==
+                       contact_force_vars(knot_point, 1));
+  prog_->AddConstraint(contact_force_vars(knot_point, 2) ==
+                       contact_force_vars(knot_point, 3));
+
+  //  //  // GRF per foot parallel to vector from foot center to com
+  //  prog_->AddConstraint((contact_force_vars(knot_point, 0)
+  //  / 1.0).normalized() ==
+  //                       (com_pos_vars(knot_point) -
+  //                        contact_pos_vars(knot_point, 0) / 2.0 -
+  //                        contact_pos_vars(knot_point, 1) / 2.0)
+  //                           .normalized());
+  //
+  //  prog_->AddConstraint((contact_force_vars(knot_point, 2)
+  //  / 1.0).normalized() ==
+  //                       (com_pos_vars(knot_point) -
+  //                        contact_pos_vars(knot_point, 2) / 2.0 -
+  //                        contact_pos_vars(knot_point, 3) / 2.0)
+  //                           .normalized());
+}
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index ae8ab5f44f..c4fd9e8d6b 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -29,7 +29,9 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
         k_(k),
         r0_(r0),
         b_(b),
-        nominal_stand_(nominal_stand) {
+        nominal_stand_(nominal_stand),
+        positions_map_(dairlib::multibody::MakeNameToPositionsMap(plant_)),
+        velocity_map_(dairlib::multibody::MakeNameToVelocitiesMap(plant_)) {
     AddPlantJointLimits(dairlib::JointNames());
     AddLoopClosure();
 
@@ -98,6 +100,8 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
   void Build(const drake::solvers::SolverOptions& solver_options) override;
 
  private:
+  void AddSlipPosturePrincipleConstraint(int knot_point);
+
   void MapModeSequence();
 
   void AddSlipConstraints(int knot_point) override;
@@ -148,4 +152,7 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
       {{false, false, false, false}, {false, false}}};
   std::vector<drake::solvers::Binding<drake::solvers::Constraint>>
       slip_dynamics_binding_;
+
+  std::map<std::string, int> positions_map_;
+  std::map<std::string, int> velocity_map_;
 };
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
index 5dc048dbd6..bb473abedf 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -519,7 +519,6 @@ void KinematicCentroidalSolver::AddPlantJointLimits(
     }
   }
 }
-
 drake::solvers::VectorXDecisionVariable KinematicCentroidalSolver::com_pos_vars(
     int knotpoint_index) const {
   return com_vars_[knotpoint_index];

From 0491e008c643a3b73e32fd6dc9697d1f0bd10b56 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 15 Dec 2022 12:50:58 -0500
Subject: [PATCH 70/76] Couldn't even tell when the swap happened

---
 .../kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc  | 2 +-
 .../cassie_kinematic_centroidal_solver.cc                 | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index 97faeb0fa2..f9727b924d 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -223,7 +223,7 @@ int DoMain(int argc, char* argv[]) {
 
   while (true) {
     drake::systems::Simulator<double> simulator(*diagram);
-    simulator.set_target_realtime_rate(1.0);
+    simulator.set_target_realtime_rate(0.25);
     simulator.Initialize();
     simulator.AdvanceTo(pp_xtraj.end_time());
     sleep(2);
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index 0d3dd24e24..4b6f527748 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -332,10 +332,10 @@ void CassieKinematicCentroidalSolver::AddSlipPosturePrincipleConstraint(
   //  // Identity orientation
   prog_->AddBoundingBoxConstraint(
       1 - eps, 1 + eps, state_vars(knot_point)[positions_map_.at("base_qw")]);
-  prog_->AddBoundingBoxConstraint(
-      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qx")]);
-  prog_->AddBoundingBoxConstraint(
-      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qy")]);
+  //  prog_->AddBoundingBoxConstraint(
+  //      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qx")]);
+  //  prog_->AddBoundingBoxConstraint(
+  //      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qy")]);
   prog_->AddBoundingBoxConstraint(
       -eps, eps, state_vars(knot_point)[positions_map_.at("base_qz")]);
   //

From d71aef76ac5b3f0dacce084ed29734d3e8c0591e Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 15 Dec 2022 17:01:05 -0500
Subject: [PATCH 71/76] Getting some alright performance

---
 .../cassie_kcmpc_trajopt.cc                   |   4 +-
 .../cassie_kinematic_centroidal_solver.cc     |  43 ++------
 .../cassie_kinematic_centroidal_solver.h      |   4 +-
 .../gaits/stand.yaml                          |   2 +-
 .../gaits/walk.yaml                           |   2 +-
 .../kinematic_centroidal_mpc_gains.yaml       |   8 +-
 .../kinematic_centroidal_solver.cc            | 103 ++++++++++++------
 .../kinematic_centroidal_solver.h             |   9 +-
 8 files changed, 91 insertions(+), 84 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index f9727b924d..fe4fee2f1a 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -107,7 +107,7 @@ int DoMain(int argc, char* argv[]) {
   CassieKinematicCentroidalSolver mpc(
       plant, traj_params.n_knot_points,
       time_points.back() / (traj_params.n_knot_points - 1), 0.4,
-      reference_state.head(plant.num_positions()), 3000, 80,
+      reference_state.head(plant.num_positions()), 3000, 115,
       sqrt(pow(traj_params.target_com_height, 2) +
            pow(traj_params.stance_width, 2)),
       traj_params.stance_width);
@@ -117,7 +117,7 @@ int DoMain(int argc, char* argv[]) {
   std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
   std::fill(complexity_schedule.begin(), complexity_schedule.end(),
             Complexity::KINEMATIC_CENTROIDAL);
-  for (int i = 15; i < traj_params.n_knot_points; i++) {
+  for (int i = 10; i < 30; i++) {
     complexity_schedule[i] = Complexity::SLIP;
   }
   mpc.SetComplexitySchedule(complexity_schedule);
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index 4b6f527748..d82dc01125 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -46,7 +46,7 @@ void CassieKinematicCentroidalSolver::AddSlipConstraints(int knot_point) {
        contact_index++) {
     prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) -
                           slip_com_vars_[knot_point])
-                             .squaredNorm() <= 1.5);
+                             .squaredNorm() <= 1.0);
 
     if (slip_contact_sequence_[knot_point][contact_index]) {
       // Foot isn't moving
@@ -103,7 +103,7 @@ void CassieKinematicCentroidalSolver::AddSlipCost(int knot_point,
   }
 
   if (contact_ref_traj_) {
-    const Eigen::MatrixXd Q_contact_pos = gains_.contact_pos.asDiagonal();
+    const Eigen::MatrixXd Q_contact_pos = 2 * gains_.contact_pos.asDiagonal();
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_contact_pos,
         Eigen::VectorXd(contact_ref_traj_->value(t)).segment(0, 3),
@@ -113,7 +113,7 @@ void CassieKinematicCentroidalSolver::AddSlipCost(int knot_point,
         Eigen::VectorXd(contact_ref_traj_->value(t)).segment(6, 3),
         slip_contact_pos_vars(knot_point, 1));
 
-    const Eigen::MatrixXd Q_contact_vel = gains_.contact_vel.asDiagonal();
+    const Eigen::MatrixXd Q_contact_vel = 2 * gains_.contact_vel.asDiagonal();
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_contact_vel,
         Eigen::VectorXd(contact_ref_traj_->value(t)).segment(12, 3),
@@ -150,7 +150,7 @@ void CassieKinematicCentroidalSolver::MapModeSequence() {
         complex_mode_to_slip_mode_.at(contact_sequence_[knot_point]);
   }
 }
-void CassieKinematicCentroidalSolver::AddSlipReductionConstraint(
+void CassieKinematicCentroidalSolver::AddSlipEqualityConstraint(
     int knot_point) {
   prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point));
   prog_->AddConstraint(slip_vel_vars_[knot_point] * m_ ==
@@ -174,19 +174,6 @@ void CassieKinematicCentroidalSolver::AddSlipReductionConstraint(
   AddSlipPosturePrincipleConstraint(knot_point);
 }
 
-void CassieKinematicCentroidalSolver::AddSlipLiftingConstraint(int knot_point) {
-  auto lifting_constraint = std::make_shared<SlipLiftingConstraint>(
-      plant_, lifters_[knot_point], 2, n_contact_points_, knot_point);
-  prog_->AddConstraint(
-      lifting_constraint,
-      {slip_com_vars_[knot_point], slip_vel_vars_[knot_point],
-       slip_contact_pos_vars_[knot_point], slip_contact_vel_vars_[knot_point],
-       slip_force_vars_[knot_point], com_pos_vars(knot_point),
-       momentum_vars(knot_point), contact_pos_[knot_point],
-       contact_vel_[knot_point], contact_force_[knot_point],
-       state_vars(knot_point)});
-}
-
 void CassieKinematicCentroidalSolver::AddSlipDynamics(int knot_point) {
   if (!is_last_knot(knot_point)) {
     auto slip_com_dynamics = std::make_shared<SlipDynamicsConstraint<double>>(
@@ -273,20 +260,6 @@ drake::VectorX<double> CassieKinematicCentroidalSolver::LiftSlipSolution(
       result_->GetSolution(slip_contact_pos_vars_[knot_point]),
       result_->GetSolution(slip_contact_vel_vars_[knot_point]),
       result_->GetSolution(slip_force_vars_[knot_point]);
-  if (knot_point == 1) {
-    auto grf_constraint = std::make_shared<SlipGrfReductionConstrain>(
-        plant_, reducers[knot_point], 2, 4, knot_point);
-    drake::VectorX<double> grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3);
-    drake::VectorX<double> grf_error(2);
-    grf_input << result_->GetSolution(com_pos_vars(knot_point)),
-        result_->GetSolution(slip_vel_vars_[knot_point]),
-        result_->GetSolution(slip_contact_pos_vars_[knot_point]),
-        result_->GetSolution(contact_force_[knot_point]),
-        result_->GetSolution(slip_force_vars_[knot_point]);
-    grf_constraint->Eval(grf_input, &grf_error);
-    std::cout << "Grf error" << std::endl;
-    std::cout << grf_error << std::endl;
-  }
   return lifters_[knot_point]->Lift(slip_state).tail(n_q_ + n_v_);
 }
 
@@ -329,7 +302,8 @@ void CassieKinematicCentroidalSolver::AddSlipPosturePrincipleConstraint(
       -eps, eps,
       state_vars(knot_point)[n_q_ + velocity_map_.at("hip_yaw_rightdot")]);
 
-  //  // Identity orientation
+  // Identity orientation
+  // TODO figure out why this identity quaternion constraint is hard for the mpc
   prog_->AddBoundingBoxConstraint(
       1 - eps, 1 + eps, state_vars(knot_point)[positions_map_.at("base_qw")]);
   //  prog_->AddBoundingBoxConstraint(
@@ -338,7 +312,7 @@ void CassieKinematicCentroidalSolver::AddSlipPosturePrincipleConstraint(
   //      -eps, eps, state_vars(knot_point)[positions_map_.at("base_qy")]);
   prog_->AddBoundingBoxConstraint(
       -eps, eps, state_vars(knot_point)[positions_map_.at("base_qz")]);
-  //
+
   // Zero angular velocity
   prog_->AddBoundingBoxConstraint(
       -eps, eps, state_vars(knot_point)[n_q_ + velocity_map_.at("base_wx")]);
@@ -365,7 +339,8 @@ void CassieKinematicCentroidalSolver::AddSlipPosturePrincipleConstraint(
   prog_->AddConstraint(contact_force_vars(knot_point, 2) ==
                        contact_force_vars(knot_point, 3));
 
-  //  //  // GRF per foot parallel to vector from foot center to com
+  // TODO figure out this constraint (nonlinear constraint)
+  // GRF per foot parallel to vector from foot center to com
   //  prog_->AddConstraint((contact_force_vars(knot_point, 0)
   //  / 1.0).normalized() ==
   //                       (com_pos_vars(knot_point) -
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index c4fd9e8d6b..9efebb13be 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -108,9 +108,7 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
 
   void AddSlipCost(int knot_point, double terminal_gain) override;
 
-  void AddSlipReductionConstraint(int knot_point) override;
-
-  void AddSlipLiftingConstraint(int knot_point) override;
+  void AddSlipEqualityConstraint(int knot_point) override;
 
   void AddSlipDynamics(int knot_point) override;
 
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
index 18b357a213..bbe933b88f 100644
--- a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml
@@ -2,4 +2,4 @@ gait_pattern:
   - start_phase: 0.0
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 0.8
+period: 0.55
diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
index c0107c8515..10cc5868a2 100644
--- a/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml
@@ -11,4 +11,4 @@ gait_pattern:
   - start_phase: 0.9
     end_phase: 1.0
     contact_status: [true, true, true, true]
-period: 1.6
+period: 1.1
diff --git a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
index 4ab2fb335f..d6f8f2eddf 100644
--- a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml
@@ -1,4 +1,4 @@
-com_position: [1, 1, 10]
+com_position: [2, 2, 5]
 generalized_positions:
   base_qw: 1.0
   base_qx: 1.0
@@ -27,9 +27,9 @@ generalized_velocities:
   ankle_joint_: 0.00002
   toe_: 0.00004
 
-contact_pos: [0.3, 5, 0.001]
-contact_vel: [0.2, 0.2, 0.2]
-contact_force: [0.00001, 0.00001, 0.00001]
+contact_pos: [2, 5, 0.001]
+contact_vel: [0.4, 0.4, 0.1]
+contact_force: [0.00002, 0.00002, 0.0004]
 lin_momentum: [0.00001, 0.00001, 0.00001]
 ang_momentum: [0.0000000, 0.0000000, 0.0000000]
 
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
index bb473abedf..e4dfa7fac2 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -247,7 +247,7 @@ void KinematicCentroidalSolver::Build(
         AddKinematicsIntegrator(knot_point);
         if (!is_last_knot(knot_point) and
             complexity_schedule_[knot_point + 1] == SLIP) {
-          AddSlipReductionConstraint(knot_point + 1);
+          AddSlipEqualityConstraint(knot_point + 1);
           AddCentroidalKinematicConsistency(knot_point + 1);
         }
         break;
@@ -258,7 +258,8 @@ void KinematicCentroidalSolver::Build(
             case KINEMATIC_CENTROIDAL:
               AddCentroidalDynamics(knot_point);
               AddKinematicsIntegrator(knot_point);
-              AddSlipLiftingConstraint(knot_point);
+              AddSlipEqualityConstraint(knot_point);
+              AddCentroidalKinematicConsistency(knot_point);
               break;
             case SLIP:
               AddSlipDynamics(knot_point);
@@ -280,55 +281,89 @@ void KinematicCentroidalSolver::SetConstantMomentumReference(
 }
 
 double KinematicCentroidalSolver::GetKnotpointGain(int knot_point) const {
-  const double terminal_gain = is_last_knot(knot_point) ? 50 : 1;
+  const double terminal_gain = is_last_knot(knot_point) ? 1 : 1;
   const double collocation_gain =
       (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1;
   return terminal_gain * collocation_gain;
 }
 
 void KinematicCentroidalSolver::AddCosts() {
+  // Loop through knot points and add running cost
   for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) {
-    double knot_point_gain = GetKnotpointGain(knot_point);
     double t = dt_ * knot_point;
 
     switch (complexity_schedule_[knot_point]) {
       case KINEMATIC_CENTROIDAL:
-        if (q_ref_traj_) {
-          prog_->AddQuadraticErrorCost(knot_point_gain * Q_q_,
-                                       q_ref_traj_->value(t),
-                                       state_vars(knot_point).head(n_q_));
-        }
-        if (v_ref_traj_) {
-          prog_->AddQuadraticErrorCost(knot_point_gain * Q_v_,
-                                       v_ref_traj_->value(t),
-                                       state_vars(knot_point).tail(n_v_));
-        }
-        if (com_ref_traj_) {
-          prog_->AddQuadraticErrorCost(knot_point_gain * Q_com_,
-                                       com_ref_traj_->value(t),
-                                       com_pos_vars(knot_point));
-        }
-        if (mom_ref_traj_) {
-          prog_->AddQuadraticErrorCost(knot_point_gain * Q_mom_,
-                                       mom_ref_traj_->value(t),
-                                       momentum_vars(knot_point));
-        }
-        if (contact_ref_traj_) {
-          prog_->AddQuadraticErrorCost(
-              knot_point_gain * Q_contact_, contact_ref_traj_->value(t),
-              {contact_pos_[knot_point], contact_vel_[knot_point]});
-        }
-        if (force_ref_traj_) {
-          prog_->AddQuadraticErrorCost(knot_point_gain * Q_force_,
-                                       force_ref_traj_->value(t),
-                                       contact_force_[knot_point]);
+        // If kinematic centroidal always going to integrate to kinematic
+        // centroidal
+        AddKinematicCentroidalCosts(knot_point, t, 0.5);
+        if (!is_last_knot(knot_point)) {
+          AddKinematicCentroidalCosts(knot_point + 1, t + dt_, 0.5);
         }
         break;
       case SLIP:
-        AddSlipCost(knot_point, knot_point_gain);
+        // If slip, and integrating to kinematic centroidal use kinematic
+        // centroidal cost If slip and integrating to slip use slip costs
+        if (!is_last_knot(knot_point)) {
+          switch (complexity_schedule_[knot_point + 1]) {
+            case KINEMATIC_CENTROIDAL:
+              AddKinematicCentroidalCosts(knot_point, t, 0.5);
+              AddKinematicCentroidalCosts(knot_point + 1, t + dt_, 0.5);
+              break;
+            case SLIP:
+              AddSlipCost(knot_point, 0.5);
+              AddSlipCost(knot_point + 1, 0.5);
+              break;
+          }
+        } else {
+          AddSlipCost(knot_point, 0.5);
+        }
         break;
     }
   }
+  int knot_point = n_knot_points_ - 1;
+  double knot_point_gain = GetKnotpointGain(knot_point);
+  double t = dt_ * knot_point;
+  switch (complexity_schedule_[knot_point]) {
+    case KINEMATIC_CENTROIDAL:
+      AddKinematicCentroidalCosts(knot_point, t, knot_point_gain);
+      break;
+    case SLIP:
+      AddSlipCost(knot_point, knot_point_gain);
+      break;
+  }
+}
+
+void KinematicCentroidalSolver::AddKinematicCentroidalCosts(
+    int knot_point, double t, double knot_point_gain) {
+  if (q_ref_traj_) {
+    prog_->AddQuadraticErrorCost(knot_point_gain * Q_q_, q_ref_traj_->value(t),
+                                 state_vars(knot_point).head(n_q_));
+  }
+  if (v_ref_traj_) {
+    prog_->AddQuadraticErrorCost(knot_point_gain * Q_v_, v_ref_traj_->value(t),
+                                 state_vars(knot_point).tail(n_v_));
+  }
+  if (com_ref_traj_) {
+    prog_->AddQuadraticErrorCost(knot_point_gain * Q_com_,
+                                 com_ref_traj_->value(t),
+                                 com_pos_vars(knot_point));
+  }
+  if (mom_ref_traj_) {
+    prog_->AddQuadraticErrorCost(knot_point_gain * Q_mom_,
+                                 mom_ref_traj_->value(t),
+                                 momentum_vars(knot_point));
+  }
+  if (contact_ref_traj_) {
+    prog_->AddQuadraticErrorCost(
+        knot_point_gain * Q_contact_, contact_ref_traj_->value(t),
+        {contact_pos_[knot_point], contact_vel_[knot_point]});
+  }
+  if (force_ref_traj_) {
+    prog_->AddQuadraticErrorCost(knot_point_gain * Q_force_,
+                                 force_ref_traj_->value(t),
+                                 contact_force_[knot_point]);
+  }
 }
 
 void KinematicCentroidalSolver::UpdateCosts() { DRAKE_DEMAND(false); }
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
index 5746cc4dd7..2e81c4f879 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
@@ -399,11 +399,7 @@ class KinematicCentroidalSolver {
     DRAKE_DEMAND(false);
   };
 
-  virtual void AddSlipReductionConstraint(int knot_point) {
-    DRAKE_DEMAND(false);
-  };
-
-  virtual void AddSlipLiftingConstraint(int knot_point) {
+  virtual void AddSlipEqualityConstraint(int knot_point) {
     DRAKE_DEMAND(false);
   };
 
@@ -418,6 +414,9 @@ class KinematicCentroidalSolver {
    */
   void AddCosts();
 
+  void AddKinematicCentroidalCosts(int knot_point, double t,
+                                   double knot_point_gain);
+
   bool is_first_knot(int knot_point_index) const {
     return knot_point_index == 0;
   };

From f08e1b01911f74c46fd95a5afddd1ed0b4ec7f2b Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 15 Dec 2022 17:51:30 -0500
Subject: [PATCH 72/76] Added gains to yaml

---
 .../kinematic_centroidal_planner/BUILD.bazel  |   5 +-
 ..._reference_utils.cc => cassie_kc_utils.cc} | 102 ++++++++++++------
 ...ie_reference_utils.h => cassie_kc_utils.h} |  14 ++-
 .../cassie_kcmpc_trajopt.cc                   |  14 +--
 .../motions/motion_jump.yaml                  |   3 +
 .../motions/motion_left_step.yaml             |   3 +
 .../motions/motion_right_step.yaml            |   3 +
 .../motions/motion_test.yaml                  |   4 +
 .../motions/motion_walk.yaml                  |   3 +
 .../simple_models/slip_lifting_test.cc        |   2 +-
 .../trajectory_parameters.h                   |   7 ++
 .../kinematic_centroidal_mpc.cc               |   6 +-
 12 files changed, 111 insertions(+), 55 deletions(-)
 rename examples/Cassie/kinematic_centroidal_planner/{cassie_reference_utils.cc => cassie_kc_utils.cc} (65%)
 rename examples/Cassie/kinematic_centroidal_planner/{cassie_reference_utils.h => cassie_kc_utils.h} (52%)

diff --git a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
index c0d87c6b48..b0d3dc2a39 100644
--- a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
@@ -77,13 +77,14 @@ cc_library(
 
 cc_library(
     name = "cassie_reference_utils",
-    srcs = ["cassie_reference_utils.cc"],
-    hdrs = ["cassie_reference_utils.h"],
+    srcs = ["cassie_kc_utils.cc"],
+    hdrs = ["cassie_kc_utils.h"],
     deps = [
         "//examples/Cassie:cassie_utils",
         "//common",
         "//systems/trajectory_optimization/kinematic_centroidal_planner:kinematic_centroidal_planner",
         "//multibody:visualization_utils",
+        ":cassie_kinematic_centroidal_mpc",
         "@drake//:drake_shared_library",
     ],
 )
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc
similarity index 65%
rename from examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc
rename to examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc
index 2b5215dbee..9d1f22f9df 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc
@@ -1,30 +1,34 @@
-#include "cassie_reference_utils.h"
-#include <drake/systems/framework/diagram_builder.h>
+#include "cassie_kc_utils.h"
+
+#include <iostream>
+#include <regex>
+
 #include <drake/geometry/scene_graph.h>
+#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
+#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 #include <drake/multibody/parsing/parser.h>
 #include <drake/multibody/plant/multibody_plant.h>
-#include <iostream>
-#include <drake/systems/analysis/simulator.h>
-#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>
 #include <drake/solvers/solve.h>
+#include <drake/systems/analysis/simulator.h>
+#include <drake/systems/framework/diagram_builder.h>
+
 #include "examples/Cassie/cassie_utils.h"
 #include "multibody/visualization_utils.h"
-#include <drake/multibody/inverse_kinematics/com_position_constraint.h>
 
 using drake::geometry::SceneGraph;
 using drake::multibody::MultibodyPlant;
 using drake::multibody::Parser;
 
-Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
-                                     double com_height,
-                                     double stance_width,
-                                     bool visualize) {
-  using Eigen::VectorXd;
+Eigen::VectorXd GenerateNominalStand(
+    const drake::multibody::MultibodyPlant<double>& plant, double com_height,
+    double stance_width, bool visualize) {
   using Eigen::Vector3d;
+  using Eigen::VectorXd;
   int n_q = plant.num_positions();
   int n_v = plant.num_velocities();
   int n_x = n_q + n_v;
-  std::map<std::string, int> positions_map = dairlib::multibody::MakeNameToPositionsMap(plant);
+  std::map<std::string, int> positions_map =
+      dairlib::multibody::MakeNameToPositionsMap(plant);
 
   Eigen::VectorXd q_ik_guess = Eigen::VectorXd::Zero(n_q);
 
@@ -58,12 +62,11 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
   Eigen::Vector3d heel_rt_toe = {.122, 0, 0};
 
   Eigen::Vector3d pelvis_pos = {0, 0, com_height};
-  Eigen::Vector3d l_toe_pos = {0.06, stance_width/2, 0};
-  Eigen::Vector3d r_toe_pos = {0.06, -stance_width/2, 0};
+  Eigen::Vector3d l_toe_pos = {0.06, stance_width / 2, 0};
+  Eigen::Vector3d r_toe_pos = {0.06, -stance_width / 2, 0};
 
   Eigen::Vector3d l_heel_pos = l_toe_pos - heel_rt_toe;
-  Eigen::Vector3d r_heel_pos = r_toe_pos-heel_rt_toe;
-
+  Eigen::Vector3d r_heel_pos = r_toe_pos - heel_rt_toe;
 
   const auto& world_frame = plant.world_frame();
   const auto& pelvis_frame = plant.GetFrameByName("pelvis");
@@ -74,19 +77,22 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
   drake::multibody::InverseKinematics ik(plant, context.get());
   double eps = 1e-3;
   Vector3d eps_vec = eps * VectorXd::Ones(3);
-  ik.AddOrientationConstraint(pelvis_frame, drake::math::RotationMatrix<double>(),
-                              world_frame, drake::math::RotationMatrix<double>(), eps);
-  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, world_frame,
-                           l_toe_pos - eps_vec,
+  ik.AddOrientationConstraint(
+      pelvis_frame, drake::math::RotationMatrix<double>(), world_frame,
+      drake::math::RotationMatrix<double>(), eps);
+  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first,
+                           world_frame, l_toe_pos - eps_vec,
                            l_toe_pos + eps_vec);
-  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first, world_frame,
-                           l_heel_pos - eps_vec,
+  ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first,
+                           world_frame, l_heel_pos - eps_vec,
                            l_heel_pos + eps_vec);
 
-  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first, world_frame,
-                           r_toe_pos - eps_vec, r_toe_pos + eps_vec);
-  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first, world_frame,
-                           r_heel_pos - eps_vec, r_heel_pos + eps_vec);
+  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first,
+                           world_frame, r_toe_pos - eps_vec,
+                           r_toe_pos + eps_vec);
+  ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first,
+                           world_frame, r_heel_pos - eps_vec,
+                           r_heel_pos + eps_vec);
 
   ik.get_mutable_prog()->AddLinearConstraint(
       (ik.q())(positions_map.at("hip_yaw_left")) == 0);
@@ -96,15 +102,16 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
   ik.get_mutable_prog()->AddLinearConstraint(
       (ik.q())(positions_map.at("knee_left")) +
           (ik.q())(positions_map.at("ankle_joint_left")) ==
-          M_PI * 13 / 180.0);
+      M_PI * 13 / 180.0);
   ik.get_mutable_prog()->AddLinearConstraint(
       (ik.q())(positions_map.at("knee_right")) +
           (ik.q())(positions_map.at("ankle_joint_right")) ==
-          M_PI * 13 / 180.0);
+      M_PI * 13 / 180.0);
 
-  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(&plant, std::nullopt, plant.world_frame(), context.get());
+  auto constraint = std::make_shared<drake::multibody::ComPositionConstraint>(
+      &plant, std::nullopt, plant.world_frame(), context.get());
   auto r = ik.get_mutable_prog()->NewContinuousVariables(3);
-  ik.get_mutable_prog()->AddConstraint(constraint, {ik.q(),r});
+  ik.get_mutable_prog()->AddConstraint(constraint, {ik.q(), r});
   Eigen::Vector3d rdes = {0, 0, com_height};
   ik.get_mutable_prog()->AddBoundingBoxConstraint(rdes, rdes, r);
 
@@ -117,15 +124,16 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
   q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q - 4);
   q_ik_guess = q_sol_normd;
 
-  if(visualize){
+  if (visualize) {
     // Build temporary diagram for visualization
     drake::systems::DiagramBuilder<double> builder_ik;
-    drake::geometry::SceneGraph<double>& scene_graph_ik = *builder_ik.AddSystem<drake::geometry::SceneGraph>();
+    drake::geometry::SceneGraph<double>& scene_graph_ik =
+        *builder_ik.AddSystem<drake::geometry::SceneGraph>();
     scene_graph_ik.set_name("scene_graph_ik");
     MultibodyPlant<double> plant_ik(0.0);
     Parser parser(&plant_ik, &scene_graph_ik);
-    std::string full_name =
-        dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+    std::string full_name = dairlib::FindResourceOrThrow(
+        "examples/Cassie/urdf/cassie_fixed_springs.urdf");
     parser.AddModelFromFile(full_name);
     plant_ik.Finalize();
 
@@ -148,4 +156,28 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<doub
   return rv;
 }
 
-
+std::vector<Complexity> GenerateComplexitySchedule(
+    int n_knot_points, const std::vector<std::string>& complexity_string_list) {
+  std::vector<Complexity> complexity_schedule;
+  std::regex number_regex("(^|\\s)([0-9]+)($|\\s)");
+  std::smatch match;
+  for (const auto& complexity_string : complexity_string_list) {
+    if (complexity_string.at(0) == 'c') {
+      std::regex_search(complexity_string, match, number_regex);
+      std::cout << std::stoi(match[0]) << std::endl;
+      for (int i = 0; i < std::stoi(match[0]); i++) {
+        complexity_schedule.push_back(Complexity::KINEMATIC_CENTROIDAL);
+      }
+    } else if (complexity_string.at(0) == 's') {
+      std::regex_search(complexity_string, match, number_regex);
+      std::cout << std::stoi(match[0]) << std::endl;
+      for (int i = 0; i < std::stoi(match[0]); i++) {
+        complexity_schedule.push_back(Complexity::SLIP);
+      }
+    } else {
+      DRAKE_DEMAND(false);
+    }
+  }
+  DRAKE_DEMAND(n_knot_points == complexity_schedule.size());
+  return complexity_schedule;
+}
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h
similarity index 52%
rename from examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h
rename to examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h
index d5fe476d4b..5dedaa2d73 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h
@@ -1,9 +1,11 @@
 
 #pragma once
 
-#include <drake/multibody/plant/multibody_plant.h>
 #include <Eigen/Core>
 #include <drake/common/trajectories/piecewise_polynomial.h>
+#include <drake/multibody/plant/multibody_plant.h>
+
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h"
 #include "multibody/kinematic/world_point_evaluator.h"
 
 /*!
@@ -14,7 +16,9 @@
  * @param visualize true if the stand should be visualized
  * @return vector of state [nq + nv]
  */
-Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant<double> &plant,
-                                     double com_height,
-                                     double stance_width,
-                                     bool visualize = false);
\ No newline at end of file
+Eigen::VectorXd GenerateNominalStand(
+    const drake::multibody::MultibodyPlant<double>& plant, double com_height,
+    double stance_width, bool visualize = false);
+
+std::vector<Complexity> GenerateComplexitySchedule(
+    int n_knot_points, const std::vector<std::string>& complexity_string_list);
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index fe4fee2f1a..b40a746ec1 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -18,8 +18,8 @@
 
 #include "common/find_resource.h"
 #include "dairlib/lcmt_timestamped_saved_traj.hpp"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h"
-#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h"
 #include "systems/primitives/subvector_pass_through.h"
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h"
@@ -107,20 +107,16 @@ int DoMain(int argc, char* argv[]) {
   CassieKinematicCentroidalSolver mpc(
       plant, traj_params.n_knot_points,
       time_points.back() / (traj_params.n_knot_points - 1), 0.4,
-      reference_state.head(plant.num_positions()), 3000, 115,
+      reference_state.head(plant.num_positions()), traj_params.spring_constant,
+      traj_params.damping_coefficient,
       sqrt(pow(traj_params.target_com_height, 2) +
            pow(traj_params.stance_width, 2)),
       traj_params.stance_width);
   mpc.SetGains(gains);
   mpc.SetMinimumFootClearance(traj_params.swing_foot_minimum_height);
 
-  std::vector<Complexity> complexity_schedule(traj_params.n_knot_points);
-  std::fill(complexity_schedule.begin(), complexity_schedule.end(),
-            Complexity::KINEMATIC_CENTROIDAL);
-  for (int i = 10; i < 30; i++) {
-    complexity_schedule[i] = Complexity::SLIP;
-  }
-  mpc.SetComplexitySchedule(complexity_schedule);
+  mpc.SetComplexitySchedule(GenerateComplexitySchedule(
+      traj_params.n_knot_points, traj_params.complexity_schedule));
 
   KcmpcReferenceGenerator generator(plant, plant_context.get(),
                                     CreateContactPoints(plant, 0));
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
index ffc7579313..46cd1daf6b 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
@@ -2,6 +2,8 @@ n_knot_points: 25
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.015
+spring_constant: 3000
+damping_coefficient: 115
 duration_scaling:
   [ 1.0, 1.0, 1.0 ]
 gait_sequence: [ 'stand', 'jump', 'stand' ]
@@ -9,3 +11,4 @@ com_vel_values:
   [ 0, 0, 0,
     0.5, 0, 0,
     0, 0, 0 ]
+complexity_schedule: ['c 25']
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
index aee4daa77a..8902209af0 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
@@ -2,6 +2,8 @@ n_knot_points: 20
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.1
+spring_constant: 3000
+damping_coefficient: 115
 duration_scaling:
   [ 1.0, 1.0, 1.0 ]
 gait_sequence: [ 'stand', 'left_step', 'stand' ]
@@ -9,3 +11,4 @@ com_vel_values:
   [ 0, 0, 0,
     1.0, 0, 0,
     0, 0, 0 ]
+complexity_schedule: ['c 20']
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
index feb7ef4c5b..c1dd0f8588 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
@@ -2,6 +2,8 @@ n_knot_points: 20
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.1
+spring_constant: 3000
+damping_coefficient: 115
 duration_scaling:
   [ 1.0, 1.0, 1.0 ]
 gait_sequence: [ 'stand', 'right_step', 'stand' ]
@@ -9,3 +11,4 @@ com_vel_values:
   [ 0, 0, 0,
     1.0, 0, 0,
     0, 0, 0 ]
+complexity_schedule: ['c 20']
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
index ee2d58ea91..5a7ed6f90e 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
@@ -2,6 +2,8 @@ n_knot_points: 25
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.03
+spring_constant: 3000
+damping_coefficient: 115
 duration_scaling:
   [ 1.0, 1.0, 1.0 ]
 gait_sequence: [ 'stand', 'left_step', 'stand' ]
@@ -9,3 +11,5 @@ com_vel_values:
   [ 0, 0, 0,
     0, 0, 0,
     0, 0, 0 ]
+complexity_schedule: ['c 25']
+
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
index aae8a88b1e..a7dba99ea0 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
@@ -2,6 +2,8 @@ n_knot_points: 45
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.02
+spring_constant: 3000
+damping_coefficient: 115
 duration_scaling:
   [ 1.0, 3.0, 1.0 ]
 gait_sequence: [ 'stand', 'walk', 'walk' ]
@@ -9,3 +11,4 @@ com_vel_values:
   [ 0, 0, 0,
     0.5, 0.0, 0,
     0, 0, 0 ]
+complexity_schedule: ['c 10', 's 20', 'c 15']
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
index 03fdb4b627..a94e6f951e 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
@@ -13,7 +13,7 @@
 
 #include "common/find_resource.h"
 #include "examples/Cassie/cassie_utils.h"
-#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h"
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h"
diff --git a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
index dfa027ca20..f66df8199e 100644
--- a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
+++ b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
@@ -9,6 +9,8 @@ struct TrajectoryParameters {
   double target_com_height;
   double stance_width;
   double swing_foot_minimum_height;
+  double spring_constant;
+  double damping_coefficient;
 
   std::vector<double> duration_scaling;
   std::vector<std::string> gait_sequence;
@@ -16,15 +18,20 @@ struct TrajectoryParameters {
 
   std::vector<Eigen::Vector3d> com_vel_vector;
 
+  std::vector<std::string> complexity_schedule;
+
   template <typename Archive>
   void Serialize(Archive* a) {
     a->Visit(DRAKE_NVP(n_knot_points));
     a->Visit(DRAKE_NVP(target_com_height));
     a->Visit(DRAKE_NVP(stance_width));
     a->Visit(DRAKE_NVP(swing_foot_minimum_height));
+    a->Visit(DRAKE_NVP(spring_constant));
+    a->Visit(DRAKE_NVP(damping_coefficient));
     a->Visit(DRAKE_NVP(duration_scaling));
     a->Visit(DRAKE_NVP(gait_sequence));
     a->Visit(DRAKE_NVP(com_vel_values));
+    a->Visit(DRAKE_NVP(complexity_schedule));
     Eigen::Map<Eigen::Matrix3Xd> input(com_vel_values.data(), 3,
                                        com_vel_values.size() / 3);
     for (int i = 0; i < input.cols(); ++i) {
diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
index 374ea4f982..f93122da5c 100644
--- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
+++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc
@@ -2,17 +2,17 @@
 
 #include <drake/common/yaml/yaml_io.h>
 
-//#include "multibody/multibody_utils.h"
+// #include "multibody/multibody_utils.h"
 #include "common/eigen_utils.h"
+#include "examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h"
 #include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h"
-#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h"
 #include "systems/framework/output_vector.h"
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h"
 #include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h"
 
 #include "drake/common/trajectories/piecewise_polynomial.h"
 #include "drake/systems/framework/leaf_system.h"
-//#include <iostream>
+// #include <iostream>
 
 using dairlib::systems::BasicVector;
 using dairlib::systems::OutputVector;

From 99e90da951ed481d6f276f2691a4736767be6534 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 15 Dec 2022 17:59:01 -0500
Subject: [PATCH 73/76] Moved magic number to traj params

---
 .../kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc    | 1 +
 .../cassie_kinematic_centroidal_solver.cc                   | 6 +++++-
 .../cassie_kinematic_centroidal_solver.h                    | 3 +++
 .../kinematic_centroidal_planner/motions/motion_jump.yaml   | 1 +
 .../motions/motion_left_step.yaml                           | 1 +
 .../motions/motion_right_step.yaml                          | 1 +
 .../kinematic_centroidal_planner/motions/motion_test.yaml   | 1 +
 .../kinematic_centroidal_planner/motions/motion_walk.yaml   | 1 +
 .../kinematic_centroidal_planner/trajectory_parameters.h    | 2 ++
 9 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
index b40a746ec1..35eefd29ca 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc
@@ -114,6 +114,7 @@ int DoMain(int argc, char* argv[]) {
       traj_params.stance_width);
   mpc.SetGains(gains);
   mpc.SetMinimumFootClearance(traj_params.swing_foot_minimum_height);
+  mpc.SetMaximumSlipLegLength(traj_params.max_slip_leg_length);
 
   mpc.SetComplexitySchedule(GenerateComplexitySchedule(
       traj_params.n_knot_points, traj_params.complexity_schedule));
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index d82dc01125..2745037e60 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -46,7 +46,7 @@ void CassieKinematicCentroidalSolver::AddSlipConstraints(int knot_point) {
        contact_index++) {
     prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) -
                           slip_com_vars_[knot_point])
-                             .squaredNorm() <= 1.0);
+                             .squaredNorm() <= pow(max_slip_leg_length_, 2));
 
     if (slip_contact_sequence_[knot_point][contact_index]) {
       // Foot isn't moving
@@ -355,3 +355,7 @@ void CassieKinematicCentroidalSolver::AddSlipPosturePrincipleConstraint(
   //                        contact_pos_vars(knot_point, 3) / 2.0)
   //                           .normalized());
 }
+void CassieKinematicCentroidalSolver::SetMaximumSlipLegLength(
+    double max_leg_length) {
+  max_slip_leg_length_ = max_leg_length;
+}
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index 9efebb13be..8ba0e79775 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -99,6 +99,8 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
 
   void Build(const drake::solvers::SolverOptions& solver_options) override;
 
+  void SetMaximumSlipLegLength(double max_leg_length);
+
  private:
   void AddSlipPosturePrincipleConstraint(int knot_point);
 
@@ -130,6 +132,7 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
   double m_;
   const drake::VectorX<double> nominal_stand_;
   const double slip_ground_offset_ = 0;
+  double max_slip_leg_length_ = 10;
 
   std::vector<drake::solvers::VectorXDecisionVariable> slip_com_vars_;
   std::vector<drake::solvers::VectorXDecisionVariable> slip_vel_vars_;
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
index 46cd1daf6b..3f8f8e2111 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml
@@ -2,6 +2,7 @@ n_knot_points: 25
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.015
+max_slip_leg_length: 1
 spring_constant: 3000
 damping_coefficient: 115
 duration_scaling:
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
index 8902209af0..3b1f73c7ce 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml
@@ -2,6 +2,7 @@ n_knot_points: 20
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.1
+max_slip_leg_length: 1
 spring_constant: 3000
 damping_coefficient: 115
 duration_scaling:
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
index c1dd0f8588..1ba16e6a06 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml
@@ -2,6 +2,7 @@ n_knot_points: 20
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.1
+max_slip_leg_length: 1
 spring_constant: 3000
 damping_coefficient: 115
 duration_scaling:
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
index 5a7ed6f90e..01b8b32e90 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml
@@ -2,6 +2,7 @@ n_knot_points: 25
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.03
+max_slip_leg_length: 1
 spring_constant: 3000
 damping_coefficient: 115
 duration_scaling:
diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
index a7dba99ea0..e55d663b9e 100644
--- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
+++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml
@@ -2,6 +2,7 @@ n_knot_points: 45
 target_com_height: 0.8
 stance_width: 0.25
 swing_foot_minimum_height: 0.02
+max_slip_leg_length: 1
 spring_constant: 3000
 damping_coefficient: 115
 duration_scaling:
diff --git a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
index f66df8199e..448b37bcdf 100644
--- a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
+++ b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h
@@ -9,6 +9,7 @@ struct TrajectoryParameters {
   double target_com_height;
   double stance_width;
   double swing_foot_minimum_height;
+  double max_slip_leg_length;
   double spring_constant;
   double damping_coefficient;
 
@@ -26,6 +27,7 @@ struct TrajectoryParameters {
     a->Visit(DRAKE_NVP(target_com_height));
     a->Visit(DRAKE_NVP(stance_width));
     a->Visit(DRAKE_NVP(swing_foot_minimum_height));
+    a->Visit(DRAKE_NVP(max_slip_leg_length));
     a->Visit(DRAKE_NVP(spring_constant));
     a->Visit(DRAKE_NVP(damping_coefficient));
     a->Visit(DRAKE_NVP(duration_scaling));

From 655ded65f4e5609e05d72404c7688c7be052b505 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 16 Dec 2022 12:39:26 -0500
Subject: [PATCH 74/76] Documented code and fixed unit test

---
 .../cassie_kinematic_centroidal_solver.h      | 24 +++++
 .../simple_models/slip_constraints.cc         | 93 -------------------
 .../simple_models/slip_constraints.h          | 57 +-----------
 .../simple_models/slip_lifter.cc              |  9 +-
 .../simple_models/slip_lifter.h               | 52 ++++++++++-
 .../simple_models/slip_lifting_test.cc        | 23 ++---
 .../simple_models/slip_utils.cc               | 17 ++--
 .../kinematic_centroidal_solver.h             | 30 +++++-
 8 files changed, 124 insertions(+), 181 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index 8ba0e79775..0eef0feccb 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -83,17 +83,34 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
   drake::solvers::VectorXDecisionVariable slip_contact_vel_vars(
       int knot_point_index, int slip_foot_index);
 
+  /*!
+   * @brief Overload of setting kc com position to also set slip com position
+   * @param com_trajectory
+   */
   void SetComPositionGuess(
       const drake::trajectories::PiecewisePolynomial<double>& com_trajectory)
       override;
 
+  /*!
+   * @brief overload of setting kc generalized state guess to also set slip contact postion and velocity
+   * @param q_traj
+   * @param v_traj
+   */
   void SetRobotStateGuess(
       const drake::trajectories::PiecewisePolynomial<double>& q_traj,
       const drake::trajectories::PiecewisePolynomial<double>& v_traj) override;
 
+  /*!
+   * @brief overload of setting kc momentum guess to set slip com velocity
+   * @param momentum_trajectory
+   */
   void SetMomentumGuess(const drake::trajectories::PiecewisePolynomial<double>&
                             momentum_trajectory) override;
 
+  /*!
+   * @brief overload of setting kc force to set initial guess for slip force
+   * @param force_trajectory
+   */
   void SetForceGuess(const drake::trajectories::PiecewisePolynomial<double>&
                          force_trajectory) override;
 
@@ -102,8 +119,15 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
   void SetMaximumSlipLegLength(double max_leg_length);
 
  private:
+  /*!
+   * @brief Add constraint at knot point for complex state to be on the slip submanifold
+   * @param knot_point
+   */
   void AddSlipPosturePrincipleConstraint(int knot_point);
 
+  /*!
+   * @brief map complex mode sequence to the simple mode sequence
+   */
   void MapModeSequence();
 
   void AddSlipConstraints(int knot_point) override;
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
index 5f873c5fdb..363908d0e3 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
@@ -82,47 +82,6 @@ void SlipGrfReductionConstrain::EvaluateConstraint(
        slip_contact_force;
 }
 
-SlipLiftingConstraint::SlipLiftingConstraint(
-    const drake::multibody::MultibodyPlant<double>& plant,
-    std::shared_ptr<SlipLifter> lifting_function, int n_slip_feet,
-    int n_complex_feet, int knot_index)
-    : dairlib::solvers::NonlinearConstraint<double>(
-          6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
-              plant.num_velocities(),
-          3 + 3 + 3 * 2 * n_slip_feet + 6 + 3 + 3 * 3 * n_complex_feet +
-              plant.num_positions() + n_slip_feet + plant.num_velocities(),
-          Eigen::VectorXd::Zero(6 + 3 + 3 * 3 * n_complex_feet +
-                                plant.num_positions() + plant.num_velocities()),
-          Eigen::VectorXd::Zero(6 + 3 + 3 * 3 * n_complex_feet +
-                                plant.num_positions() + plant.num_velocities()),
-          "SlipLiftingConstraint[" + std::to_string(knot_index) + "]"),
-      lifting_function_(std::move(lifting_function)),
-      slip_dim_(2 + 2 + 2 * 2 * n_slip_feet + n_slip_feet),
-      complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() +
-                   plant.num_velocities()) {}
-
-/// Input is of the form and should match the lifting function input and output:
-///     slip_com
-///     slip_velocity
-///     slip_contact_pos
-///     slip_contact_vel
-///     slip_force
-///     complex_com
-///     complex_ang_momentum
-///     complex_lin_momentum
-///     complex_contact_pos
-///     complex_contact_vel
-///     complex_contact_force
-///     complex_gen_pos
-///     complex_gen_vel
-void SlipLiftingConstraint::EvaluateConstraint(
-    const Eigen::Ref<const drake::VectorX<double>>& x,
-    drake::VectorX<double>* y) const {
-  const auto& slip_state = x.head(slip_dim_);
-  const auto& complex_state = x.tail(complex_dim_);
-  *y = lifting_function_->Lift(slip_state) - complex_state;
-}
-
 template <typename T>
 SlipDynamicsConstraint<T>::SlipDynamicsConstraint(
     double r0, double k, double b, T m, int n_feet,
@@ -199,57 +158,5 @@ drake::VectorX<T> SlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
   return derivative;
 }
 
-QuadraticLiftedCost::QuadraticLiftedCost(
-    std::shared_ptr<SlipLifter> lifting_function,
-    QuadraticLiftedCost::cost_element com_cost,
-    QuadraticLiftedCost::cost_element momentum_cost,
-    QuadraticLiftedCost::cost_element contact_cost,
-    QuadraticLiftedCost::cost_element grf_cost,
-    QuadraticLiftedCost::cost_element q_cost,
-    QuadraticLiftedCost::cost_element v_cost, double terminal_gain,
-    int n_slip_feet, int knot_point)
-    : dairlib::solvers::NonlinearCost<double>(
-          3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet,
-          "LiftedCost[" + std::to_string(knot_point) + "]"),
-      lifting_function_(std::move(lifting_function)),
-      com_cost_(std::move(com_cost)),
-      momentum_cost_(std::move(momentum_cost)),
-      contact_cost_(std::move(contact_cost)),
-      grf_cost_(std::move(grf_cost)),
-      q_cost_(std::move(q_cost)),
-      v_cost_(std::move(v_cost)),
-      terminal_gain_(terminal_gain) {}
-
-void QuadraticLiftedCost::EvaluateCost(
-    const Eigen::Ref<const drake::VectorX<double>>& x,
-    drake::VectorX<double>* y) const {
-  const auto lifted_state = lifting_function_->Lift(x);
-
-  const auto& com = lifted_state.head(3);
-  const auto& momentum = lifted_state.segment(3, 6);
-  const auto& contact_info =
-      lifted_state.segment(3 + 6, contact_cost_.ref.size());
-  const auto& grf = lifted_state.segment(3 + 6 + contact_cost_.ref.size(),
-                                         grf_cost_.ref.size());
-  const auto& q = lifted_state.segment(
-      3 + 6 + contact_cost_.ref.size() + grf_cost_.ref.size(),
-      q_cost_.ref.size());
-  const auto& v =
-      lifted_state.segment(3 + 6 + contact_cost_.ref.size() +
-                               grf_cost_.ref.size() + q_cost_.ref.size(),
-                           v_cost_.ref.size());
-
-  *y = CalcCost(com_cost_, com) + CalcCost(momentum_cost_, momentum) +
-       CalcCost(contact_cost_, contact_info) + CalcCost(grf_cost_, grf) +
-       CalcCost(q_cost_, q) + CalcCost(v_cost_, v);
-}
-
-Eigen::Matrix<double, -1, 1, 0> QuadraticLiftedCost::CalcCost(
-    const QuadraticLiftedCost::cost_element& cost,
-    const Eigen::Ref<const drake::VectorX<double>>& x) const {
-  auto error = x - cost.ref;
-  return terminal_gain_ * error.transpose() * cost.Q * error;
-}
-
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
     class SlipDynamicsConstraint);
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
index 68fb18219d..5a98a435b1 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
@@ -46,22 +46,6 @@ class SlipGrfReductionConstrain
   const int n_complex_feet_;
 };
 
-class SlipLiftingConstraint
-    : public dairlib::solvers::NonlinearConstraint<double> {
- public:
-  SlipLiftingConstraint(const drake::multibody::MultibodyPlant<double>& plant,
-                        std::shared_ptr<SlipLifter> lifting_function,
-                        int n_slip_feet, int n_complex_feet, int knot_index);
-
- private:
-  void EvaluateConstraint(const Eigen::Ref<const drake::VectorX<double>>& x,
-                          drake::VectorX<double>* y) const override;
-
-  std::shared_ptr<SlipLifter> lifting_function_;
-  const int slip_dim_;
-  const int complex_dim_;
-};
-
 template <typename T>
 class SlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
  public:
@@ -87,43 +71,4 @@ class SlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
   const std::vector<bool> contact_mask0_;
   const std::vector<bool> contact_mask1_;
   const double dt_;
-};
-
-///     complex_com
-///     complex_ang_momentum
-///     complex_lin_momentum
-///     complex_contact_pos
-///     complex_contact_vel
-///     complex_contact_force
-///     complex_gen_pos
-///     complex_gen_vel
-class QuadraticLiftedCost : public dairlib::solvers::NonlinearCost<double> {
-  struct cost_element {
-    const Eigen::MatrixXd& Q;
-    Eigen::MatrixXd ref;
-  };
-
- public:
-  QuadraticLiftedCost(std::shared_ptr<SlipLifter> lifting_function,
-                      cost_element com_cost, cost_element momentum_cost,
-                      cost_element contact_cost, cost_element grf_cost,
-                      cost_element q_cost, cost_element v_cost,
-                      double terminal_gain, int n_slip_feet, int knot_point);
-
- private:
-  void EvaluateCost(const Eigen::Ref<const drake::VectorX<double>>& x,
-                    drake::VectorX<double>* y) const override;
-
-  Eigen::Matrix<double, -1, 1, 0> CalcCost(
-      const cost_element& cost,
-      const Eigen::Ref<const drake::VectorX<double>>& x) const;
-
-  std::shared_ptr<SlipLifter> lifting_function_;
-  const cost_element com_cost_;
-  const cost_element momentum_cost_;
-  const cost_element contact_cost_;
-  const cost_element grf_cost_;
-  const cost_element q_cost_;
-  const cost_element v_cost_;
-  const double terminal_gain_;
-};
+};
\ No newline at end of file
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
index aa240d6c89..289f1b7372 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
@@ -102,7 +102,7 @@ drake::VectorX<double> SlipLifter::LiftGeneralizedPosition(
 
   q_sol_normd.segment(4, 3) = q_sol_normd.segment(4, 3) + com_position;
   // Set initial guess for next time
-  //  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
+  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd);
   // Remove added constraints
   ik_.get_mutable_prog()->RemoveConstraint(com_constraint);
   for (const auto& constraint : foot_constraints) {
@@ -212,17 +212,14 @@ drake::VectorX<double> SlipLifter::LiftGrf(
   for (int simple_index = 0; simple_index < slip_contact_points_.size();
        simple_index++) {
     // Calculate the slip grf
-    double r = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos).norm();
-    double dr = (slip_feet_pos.segment(simple_index * 3, 3) - com_pos)
+    double r = (com_pos - slip_feet_pos.segment(simple_index * 3, 3)).norm();
+    double dr = (com_pos - slip_feet_pos.segment(simple_index * 3, 3))
                     .normalized()
                     .dot(com_vel);
     double slip_grf_mag =
         slip_contact_mask_[simple_index]
             ? SlipGrf<double>(k_, r0_, b_, r, dr, slip_force[simple_index])
             : 0;
-    if (slip_grf_mag < 0) {
-      slip_grf_mag = 0;
-    }
     // Find the average location for all of the complex contact points that make
     // up the SLIP foot
     drake::Vector3<double> average_pos = drake::VectorX<double>::Zero(3);
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h
index 4fbcb8cc56..e5daaddbb8 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h
@@ -12,8 +12,33 @@
 #include "drake/solvers/constraint.h"
 #include "drake/systems/trajectory_optimization/multiple_shooting.h"
 
+/*!
+ * @brief This class lifts the slip state and control to an equivalent kinematic
+ * centroidal state and control. It assumes that there is zero orientation and
+ * angular momentum
+ *
+ * @note Currently this class assumes the slip contact points or [ltoe, rtoe]
+ * and the complex contact points are [ltoe, lheel, rtoe, rheel]
+ */
 class SlipLifter {
  public:
+  /*!
+   * @brief Constructor
+   * @param plant
+   * @param context
+   * @param slip_contact_points vector of world point evaluators for slip
+   * contact points
+   * @param complex_contact_points vector of world point evaluators for complex
+   * contact points
+   * @param simple_foot_index_to_complex_foot_index map from slip contact point
+   * index to vector of complex contact points on the same feet
+   * @param nominal_stand
+   * @param k
+   * @param b
+   * @param r0
+   * @param contact_mask vector of booleans describing which slip contact points
+   * are in stance
+   */
   SlipLifter(const drake::multibody::MultibodyPlant<double>& plant,
              drake::systems::Context<double>* context,
              const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
@@ -25,6 +50,21 @@ class SlipLifter {
              const drake::VectorX<double>& nominal_stand, double k, double b,
              double r0, const std::vector<bool>& contact_mask);
 
+  /// Input is of the form:
+  ///     slip_com
+  ///     slip_velocity
+  ///     slip_contact_pos
+  ///     slip_contact_vel
+  ///     slip_force
+  /// Output is of the form:
+  ///     complex_com
+  ///     complex_ang_momentum
+  ///     complex_lin_momentum
+  ///     complex_contact_pos
+  ///     complex_contact_vel
+  ///     complex_contact_force
+  ///     complex_gen_pos
+  ///     complex_gen_vel
   void Lift(const Eigen::Ref<const drake::VectorX<double>>& slip_state,
             drake::VectorX<double>* complex_state) const;
 
@@ -70,6 +110,17 @@ class SlipLifter {
       const drake::VectorX<double>& generalized_pos,
       const drake::VectorX<double>& generalized_vel) const;
 
+  /*!
+   * @brief Lifts the grf by evenly distributing the slip grf into the
+   * equivalent toe and heel such that the toe and heel grf are equal and
+   * parallel
+   * @param com_pos
+   * @param com_vel
+   * @param slip_feet_pos
+   * @param slip_force
+   * @param complex_contact_point_pos
+   * @return
+   */
   drake::VectorX<double> LiftGrf(
       const drake::VectorX<double>& com_pos,
       const drake::VectorX<double>& com_vel,
@@ -84,7 +135,6 @@ class SlipLifter {
   const double k_;
   const double b_;
   const double r0_;
-  const std::vector<double> stance_widths_;
   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>
       slip_contact_points_;
   const std::vector<dairlib::multibody::WorldPointEvaluator<double>>
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
index a94e6f951e..2331787c68 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc
@@ -71,28 +71,22 @@ int main(int argc, char* argv[]) {
       plant, right_heel_pair.first, right_heel_pair.second,
       Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
 
-  //  auto left_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-  //      plant, {0,0,0}, left_heel_pair.second,
-  //      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  //
-  //  auto right_slip_eval = dairlib::multibody::WorldPointEvaluator<double>(
-  //      plant, {0,0,0}, right_heel_pair.second,
-  //      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-
   dairlib::multibody::SetPositionsAndVelocitiesIfNew<double>(
       plant, reference_state, context.get());
-  //  std::cout<<left_slip_eval.EvalFull(*context)<<std::endl;
 
   std::vector<bool> contact_mask = {true, true};
+  double r0 = 0.8;
+  double k = 2000;
+  double b = 20;
   SlipLifter lifter(
       plant, context.get(), {left_toe_eval, right_toe_eval},
       {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
       {{0, {0, 1}}, {1, {2, 3}}}, reference_state.head(plant.num_positions()),
-      2000, 0, 0.5, contact_mask);
+      k, b, r0, contact_mask);
   SlipReducer reducer(
       plant, context.get(), {left_toe_eval, right_toe_eval},
       {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval},
-      {{0, {0, 1}}, {1, {2, 3}}}, 2000, 0, 0.5, contact_mask);
+      {{0, {0, 1}}, {1, {2, 3}}}, k, b, r0, contact_mask);
 
   Eigen::Vector3d slip_com = {0.2, 0, 0.7};
   Eigen::Vector3d slip_vel = {0.1, 0, 0.1};
@@ -127,19 +121,16 @@ int main(int argc, char* argv[]) {
   constraint.DoEval(input, &error);
   std::cout << "Max Error in inverse test: " << error.cwiseAbs().maxCoeff()
             << std::endl;
-  //  std::cout<<error<<std::endl;
 
   auto grf_constraint = SlipGrfReductionConstrain(
       plant, std::make_shared<SlipReducer>(reducer), 2, 4, 0);
   drake::VectorX<double> grf_error(2);
   drake::VectorX<double> grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3);
-  //  grf_input << slip_com, slip_vel, slip_feet, complex_state.segment(3 + 6 +
-  //  12 + 12,12),slip_force;
-
+  grf_input << slip_com, slip_vel, slip_feet,
+      complex_state.segment(3 + 6 + 12 + 12, 12), slip_force;
   grf_constraint.DoEval(grf_input, &grf_error);
   std::cout << "Max Error in grf inverse test: "
             << grf_error.cwiseAbs().maxCoeff() << std::endl;
-
   if (true) {
     // Build temporary diagram for visualization
     drake::systems::DiagramBuilder<double> builder_ik;
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
index 7052e5de6b..49593ee981 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
@@ -1,5 +1,7 @@
 #include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h"
 
+#include <iostream>
+
 #include "multibody/multibody_utils.h"
 
 template <typename T>
@@ -11,11 +13,10 @@ T SlipGrf(double k, double r0, double b, T r, T dr, T force) {
   return F;
 }
 
-template
-double SlipGrf<double>(double k, double r0, double b, double r, double dr,
-                       double force);
-template
-drake::AutoDiffXd SlipGrf<drake::AutoDiffXd>(double k, double r0, double b,
-                                             drake::AutoDiffXd r,
-                                             drake::AutoDiffXd dr,
-                                             drake::AutoDiffXd force);
+template double SlipGrf<double>(double k, double r0, double b, double r,
+                                double dr, double force);
+template drake::AutoDiffXd SlipGrf<drake::AutoDiffXd>(double k, double r0,
+                                                      double b,
+                                                      drake::AutoDiffXd r,
+                                                      drake::AutoDiffXd dr,
+                                                      drake::AutoDiffXd force);
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
index 2e81c4f879..daafb337fd 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h
@@ -393,18 +393,40 @@ class KinematicCentroidalSolver {
    */
   double GetKnotpointGain(int knot_point) const;
 
+  /*!
+   * @brief Add relevant slip constraints to knot point
+   * @param knot_point
+   */
   virtual void AddSlipConstraints(int knot_point) { DRAKE_DEMAND(false); };
 
-  virtual void AddSlipCost(int knot_point, double terminal_gain) {
+  /*!
+   * @brief add slip cost to knot point
+   * @param knot_point
+   * @param gain
+   */
+  virtual void AddSlipCost(int knot_point, double gain) {
     DRAKE_DEMAND(false);
   };
 
+  /*!
+   * @brief Add constraint to knot point that slip state is equivalent to complex state
+   * @param knot_point
+   */
   virtual void AddSlipEqualityConstraint(int knot_point) {
     DRAKE_DEMAND(false);
   };
 
+  /*!
+   * @brief Add constraint to knot point
+   * @param knot_point
+   */
   virtual void AddSlipDynamics(int knot_point) { DRAKE_DEMAND(false); };
 
+  /*!
+   * @brief Lift the slip solution to generalized state at knot point
+   * @param knot_point
+   * @return full robot generalized state
+   */
   virtual drake::VectorX<double> LiftSlipSolution(int knot_point) {
     DRAKE_DEMAND(false);
   };
@@ -414,6 +436,12 @@ class KinematicCentroidalSolver {
    */
   void AddCosts();
 
+  /*!
+   * @brief Add cost for kinematic centroidal at knot point
+   * @param knot_point
+   * @param t
+   * @param knot_point_gain
+   */
   void AddKinematicCentroidalCosts(int knot_point, double t,
                                    double knot_point_gain);
 

From 1c28a189bc3bab79218d6017e1c3648a24b0d277 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Fri, 16 Dec 2022 13:06:17 -0500
Subject: [PATCH 75/76] Fixing merge mistakes

---
 .../reference_generation_utils.cc             |  38 ------
 .../reference_generation_utils.h              | 112 ++++++++++++++++++
 2 files changed, 112 insertions(+), 38 deletions(-)
 create mode 100644 systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h

diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
index d064d460ec..83ec321e75 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc
@@ -4,44 +4,6 @@
 #include "multibody/multibody_utils.h"
 #include "examples/Cassie/cassie_utils.h"
 
-
-std::vector<dairlib::multibody::WorldPointEvaluator<double>> CreateContactPoints(const drake::multibody::MultibodyPlant<
-    double> &plant,
-                                                                                                                  double mu) {
-  auto left_toe_pair = dairlib::LeftToeFront(plant);
-  auto left_heel_pair = dairlib::LeftToeRear(plant);
-  auto right_toe_pair = dairlib::RightToeFront(plant);
-  auto right_heel_pair = dairlib::RightToeRear(plant);
-
-  std::vector<int> active_inds{0, 1, 2};
-
-  auto left_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, left_toe_pair.first, left_toe_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  left_toe_eval.set_frictional();
-  left_toe_eval.set_mu(mu);
-
-  auto left_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, left_heel_pair.first, left_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  left_heel_eval.set_frictional();
-  left_heel_eval.set_mu(mu);
-
-  auto right_toe_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, right_toe_pair.first, right_toe_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  right_toe_eval.set_frictional();
-  right_toe_eval.set_mu(mu);
-
-  auto right_heel_eval = dairlib::multibody::WorldPointEvaluator<double>(
-      plant, right_heel_pair.first, right_heel_pair.second,
-      Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds);
-  right_heel_eval.set_frictional();
-  right_heel_eval.set_mu(mu);
-
-  return {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval};
-}
-
 std::vector<dairlib::multibody::WorldPointEvaluator<double>>
 CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant,
                     double mu) {
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
new file mode 100644
index 0000000000..251d815fb1
--- /dev/null
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h
@@ -0,0 +1,112 @@
+#pragma once
+
+#include <drake/common/trajectories/piecewise_polynomial.h>
+#include <drake/multibody/plant/multibody_plant.h>
+
+#include "multibody/kinematic/world_point_evaluator.h"
+#include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h"
+
+/*!
+ * @brief creates vector of world point evaluators for cassie
+ * @param plant cassie plant
+ * @param mu coefficient of friction
+ * @return
+ */
+std::vector<dairlib::multibody::WorldPointEvaluator<double>>
+CreateContactPoints(const drake::multibody::MultibodyPlant<double>& plant,
+                    double mu);
+
+/*!
+ * @brief given an initial com and velocity of com emrt w, calculate a com
+ * trajectory
+ * @param current_com
+ * @param vel_ewrt_w velocity of the com ewrt. Given a zoh between time points,
+ * must be the same length as time_points
+ * @param time_points time values correspeding to velocities, trajectory start
+ * at the first, and end at the last, must be the same length as vel_ewrt_w
+ * @return trajectory of com velocity
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateComTrajectory(
+    const Eigen::Vector3d& current_com,
+    const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+    const std::vector<double>& time_points);
+
+drake::trajectories::PiecewisePolynomial<double> GenerateMomentumTrajectory(
+    const std::vector<Eigen::Vector3d>& vel_ewrt_w,
+    const std::vector<double>& time_points, double m);
+
+/*!
+ * @brief Constructs a trajectory for the generalized positions of a constant
+ * joint state and floating base position from com trajectory
+ * @param nominal_stand [nq] generalized state
+ * @param p_ScmBase_W vector from com to floating base in world frame
+ * @param com_traj trajectory for the center of mass
+ * @param base_pos_start index where the floating base position starts in
+ * generalized state
+ * @return trajectory of generalized position
+ */
+drake::trajectories::PiecewisePolynomial<double>
+GenerateGeneralizedPosTrajectory(
+    const Eigen::VectorXd& nominal_stand, const Eigen::Vector3d& p_ScmBase_W,
+    const drake::trajectories::PiecewisePolynomial<double>& com_traj,
+    int base_pos_start);
+
+/*!
+ * @brief constructs a trajectory for the generalized velocity where the joint
+ * velocity is 0 and floating base val from com trajectory
+ * @param com_traj
+ * @param n_v number of velocity states
+ * @param base_vel_start index where base vel starts
+ * @return trajectory of generalized velocity
+ */
+drake::trajectories::PiecewisePolynomial<double>
+GenerateGeneralizedVelTrajectory(
+    const drake::trajectories::PiecewisePolynomial<double>& com_traj, int n_v,
+    int base_vel_start);
+
+/*!
+ * @brief given a vector of gaits and time points corresponding to when the
+ * gaits are active generate a mode sequence. The transition between gaits can
+ * be awkward depending on where the gaits are in phase space during the
+ * transition {TODO SRL} make transition clean by attempting to shift phase so
+ * mode sequence lines up
+ * @param gait_sequence vector of gaits
+ * @param time_points vector of time points when each gait is active
+ * @return trajectory of mode status start at time_point(0) and ending at
+ * time_point.end()-1
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateModeSequence(
+    const std::vector<Gait>& gait_sequence,
+    const std::vector<double>& time_points);
+
+/*!
+ * @brief given a trajectory which describes the mode, and the mass of the robot
+ * calculate a nominal grf trajectory where the weight of the robot is
+ * distributed over the active contact points
+ * @param mode_trajectory
+ * @param m
+ * @return trajectory of grf for reference
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateGrfReference(
+    const drake::trajectories::PiecewisePolynomial<double>& mode_trajectory,
+    double m);
+
+/*!
+ * @brief Calculate trajectory of world point evaluators from generalized state
+ * trajectory. This assumes first order hold between knot points in state
+ * trajectories.
+ * TODO If we start using more complicated state references with this function
+ * sample time more coarsely
+ * @param plant
+ * @param contacts vector of world point evaluators
+ * @param q_traj generalized position trajectory
+ * @param v_traj generalized velocity trajectory
+ * @return trajectory of contact points stacked [contact1_pos, contact1_vel, ...
+ * contact_n_pos, contact_n_vel]
+ */
+drake::trajectories::PiecewisePolynomial<double> GenerateContactPointReference(
+    const drake::multibody::MultibodyPlant<double>& plant,
+    const std::vector<dairlib::multibody::WorldPointEvaluator<double>>&
+        contacts,
+    const drake::trajectories::PiecewisePolynomial<double>& q_traj,
+    const drake::trajectories::PiecewisePolynomial<double>& v_traj);

From f7b89c23dc5d502307a1423488315eb91c7d2235 Mon Sep 17 00:00:00 2001
From: Shane Rozen-Levy <srozen01@seas.upenn.edu>
Date: Thu, 12 Jan 2023 15:34:34 -0500
Subject: [PATCH 76/76] Responding to pr comments

---
 .../kinematic_centroidal_planner/BUILD.bazel  |  4 +-
 .../cassie_kc_utils.cc                        |  4 +-
 .../cassie_kinematic_centroidal_solver.cc     | 44 +++++++++++++--
 .../cassie_kinematic_centroidal_solver.h      | 54 ++++---------------
 .../simple_models/slip_constraints.cc         |  2 +-
 .../simple_models/slip_constraints.h          |  2 +
 .../simple_models/slip_lifter.cc              |  2 +-
 .../simple_models/slip_reducer.cc             |  2 +-
 .../simple_models/slip_utils.cc               | 16 +++---
 .../simple_models/slip_utils.h                |  2 +-
 .../kinematic_centroidal_solver.cc            |  2 -
 11 files changed, 69 insertions(+), 65 deletions(-)

diff --git a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
index b0d3dc2a39..c5aa1d74c0 100644
--- a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
+++ b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel
@@ -41,7 +41,7 @@ cc_binary(
         "//solvers:optimization_utils",
         "//systems/primitives",
         "//examples/Cassie/kinematic_centroidal_planner:cassie_kinematic_centroidal_mpc",
-        "//examples/Cassie/kinematic_centroidal_planner:cassie_reference_utils",
+        "//examples/Cassie/kinematic_centroidal_planner:cassie_kc_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -76,7 +76,7 @@ cc_library(
 )
 
 cc_library(
-    name = "cassie_reference_utils",
+    name = "cassie_kc_utils",
     srcs = ["cassie_kc_utils.cc"],
     hdrs = ["cassie_kc_utils.h"],
     deps = [
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc
index 9d1f22f9df..0860148d07 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc
@@ -162,6 +162,8 @@ std::vector<Complexity> GenerateComplexitySchedule(
   std::regex number_regex("(^|\\s)([0-9]+)($|\\s)");
   std::smatch match;
   for (const auto& complexity_string : complexity_string_list) {
+    DRAKE_DEMAND(complexity_string.at(0) == 'c' or
+                 complexity_string.at(0) == 's');
     if (complexity_string.at(0) == 'c') {
       std::regex_search(complexity_string, match, number_regex);
       std::cout << std::stoi(match[0]) << std::endl;
@@ -174,8 +176,6 @@ std::vector<Complexity> GenerateComplexitySchedule(
       for (int i = 0; i < std::stoi(match[0]); i++) {
         complexity_schedule.push_back(Complexity::SLIP);
       }
-    } else {
-      DRAKE_DEMAND(false);
     }
   }
   DRAKE_DEMAND(n_knot_points == complexity_schedule.size());
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
index 2745037e60..879832168a 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc
@@ -6,6 +6,41 @@
 
 #include "drake/multibody/parsing/parser.h"
 
+CassieKinematicCentroidalSolver::CassieKinematicCentroidalSolver(
+    const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points,
+    double dt, double mu, const drake::VectorX<double>& nominal_stand, double k,
+    double b, double r0, double stance_width)
+    : KinematicCentroidalSolver(plant, n_knot_points, dt,
+                                CreateContactPoints(plant, mu)),
+      l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
+      r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
+      loop_closure_evaluators(Plant()),
+      slip_contact_sequence_(n_knot_points),
+      k_(k),
+      r0_(r0),
+      b_(b),
+      nominal_stand_(nominal_stand),
+      positions_map_(dairlib::multibody::MakeNameToPositionsMap(plant_)),
+      velocity_map_(dairlib::multibody::MakeNameToVelocitiesMap(plant_)) {
+  AddPlantJointLimits(dairlib::JointNames());
+  AddLoopClosure();
+
+  slip_contact_points_ = CreateSlipContactPoints(plant, mu);
+  for (int knot = 0; knot < n_knot_points; knot++) {
+    slip_com_vars_.push_back(
+        prog_->NewContinuousVariables(3, "slip_com_" + std::to_string(knot)));
+    slip_vel_vars_.push_back(
+        prog_->NewContinuousVariables(3, "slip_vel_" + std::to_string(knot)));
+    slip_contact_pos_vars_.push_back(prog_->NewContinuousVariables(
+        2 * 3, "slip_contact_pos_" + std::to_string(knot)));
+    slip_contact_vel_vars_.push_back(prog_->NewContinuousVariables(
+        2 * 3, "slip_contact_vel_" + std::to_string(knot)));
+    slip_force_vars_.push_back(
+        prog_->NewContinuousVariables(2, "slip_force_" + std::to_string(knot)));
+  }
+  m_ = plant_.CalcTotalMass(*contexts_[0]);
+}
+
 std::vector<dairlib::multibody::WorldPointEvaluator<double>>
 CassieKinematicCentroidalSolver::CreateSlipContactPoints(
     const drake::multibody::MultibodyPlant<double>& plant, double mu) {
@@ -86,9 +121,10 @@ void CassieKinematicCentroidalSolver::AddSlipCost(int knot_point,
                                  slip_com_vars_[knot_point]);
   }
 
-  // Project linear momentum
+  // Project linear momentum cost into center of mass velocity by multiplying
+  // gain by mass
   if (mom_ref_traj_) {
-    const Eigen::MatrixXd Q_vel = gains_.lin_momentum.asDiagonal() * m_;
+    const Eigen::MatrixXd Q_vel = m_ * gains_.lin_momentum.asDiagonal();
     prog_->AddQuadraticErrorCost(
         terminal_gain * Q_vel, Eigen::VectorXd(mom_ref_traj_->value(t)).tail(3),
         slip_vel_vars_[knot_point]);
@@ -153,7 +189,7 @@ void CassieKinematicCentroidalSolver::MapModeSequence() {
 void CassieKinematicCentroidalSolver::AddSlipEqualityConstraint(
     int knot_point) {
   prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point));
-  prog_->AddConstraint(slip_vel_vars_[knot_point] * m_ ==
+  prog_->AddConstraint(m_ * slip_vel_vars_[knot_point] ==
                        momentum_vars(knot_point).tail(3));
   prog_->AddConstraint(slip_contact_pos_vars(knot_point, 0) ==
                        contact_pos_vars(knot_point, 0));
@@ -187,6 +223,8 @@ void CassieKinematicCentroidalSolver::AddSlipDynamics(int knot_point) {
          slip_com_vars_[knot_point + 1], slip_vel_vars_[knot_point + 1],
          slip_contact_pos_vars_[knot_point + 1],
          slip_force_vars_[knot_point + 1]}));
+
+    // Trapazoidal integration of the contact positions
     prog_->AddConstraint(slip_contact_pos_vars_[knot_point + 1] ==
                          slip_contact_pos_vars_[knot_point] +
                              0.5 * dt_ *
diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
index 0eef0feccb..071afe60e9 100644
--- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
+++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h
@@ -19,42 +19,7 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
       const drake::multibody::MultibodyPlant<double>& plant, int n_knot_points,
       double dt, double mu, const drake::VectorX<double>& nominal_stand,
       double k = 1000, double b = 20, double r0 = 0.5,
-      double stance_width = 0.2)
-      : KinematicCentroidalSolver(plant, n_knot_points, dt,
-                                  CreateContactPoints(plant, mu)),
-        l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())),
-        r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())),
-        loop_closure_evaluators(Plant()),
-        slip_contact_sequence_(n_knot_points),
-        k_(k),
-        r0_(r0),
-        b_(b),
-        nominal_stand_(nominal_stand),
-        positions_map_(dairlib::multibody::MakeNameToPositionsMap(plant_)),
-        velocity_map_(dairlib::multibody::MakeNameToVelocitiesMap(plant_)) {
-    AddPlantJointLimits(dairlib::JointNames());
-    AddLoopClosure();
-
-    slip_contact_points_ = CreateSlipContactPoints(plant, mu);
-    for (int knot = 0; knot < n_knot_points; knot++) {
-      slip_com_vars_.push_back(
-          prog_->NewContinuousVariables(3, "slip_com_" + std::to_string(knot)));
-      slip_vel_vars_.push_back(
-          prog_->NewContinuousVariables(3, "slip_vel_" + std::to_string(knot)));
-      slip_contact_pos_vars_.push_back(prog_->NewContinuousVariables(
-          2 * 3, "slip_contact_pos_" + std::to_string(knot)));
-      slip_contact_vel_vars_.push_back(prog_->NewContinuousVariables(
-          2 * 3, "slip_contact_vel_" + std::to_string(knot)));
-      slip_force_vars_.push_back(prog_->NewContinuousVariables(
-          2, "slip_force_" + std::to_string(knot)));
-    }
-    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(),
-              stance_mode);
-
-    m_ = plant_.CalcTotalMass(*contexts_[0]);
-  }
+      double stance_width = 0.2);
 
   std::vector<dairlib::multibody::WorldPointEvaluator<double>>
   CreateSlipContactPoints(const drake::multibody::MultibodyPlant<double>& plant,
@@ -92,7 +57,8 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
       override;
 
   /*!
-   * @brief overload of setting kc generalized state guess to also set slip contact postion and velocity
+   * @brief overload of setting kc generalized state guess to also set slip
+   * contact postion and velocity
    * @param q_traj
    * @param v_traj
    */
@@ -120,7 +86,8 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
 
  private:
   /*!
-   * @brief Add constraint at knot point for complex state to be on the slip submanifold
+   * @brief Add constraint at knot point for complex state to be on the slip
+   * submanifold
    * @param knot_point
    */
   void AddSlipPosturePrincipleConstraint(int knot_point);
@@ -170,11 +137,12 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver {
   std::vector<dairlib::multibody::WorldPointEvaluator<double>>
       slip_contact_points_;
 
-  std::map<std::vector<bool>, std::vector<bool>> complex_mode_to_slip_mode_{
-      {{true, true, true, true}, {true, true}},
-      {{true, true, false, false}, {true, false}},
-      {{false, false, true, true}, {false, true}},
-      {{false, false, false, false}, {false, false}}};
+  std::unordered_map<std::vector<bool>, std::vector<bool>>
+      complex_mode_to_slip_mode_{
+          {{true, true, true, true}, {true, true}},
+          {{true, true, false, false}, {true, false}},
+          {{false, false, true, true}, {false, true}},
+          {{false, false, false, false}, {false, false}}};
   std::vector<drake::solvers::Binding<drake::solvers::Constraint>>
       slip_dynamics_binding_;
 
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
index 363908d0e3..675d0e371c 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc
@@ -149,7 +149,7 @@ drake::VectorX<T> SlipDynamicsConstraint<T>::CalcTimeDerivativesWithForce(
       const auto r = com_rt_foot.norm();
       const auto unit_vec = com_rt_foot / r;
       const auto dr = com_vel.dot(unit_vec);
-      auto F = SlipGrf<T>(k_, r0_, b_, r, dr, slip_force[foot]);
+      auto F = CalcSlipGrf<T>(k_, r0_, b_, r, dr, slip_force[foot]);
       ddcom = ddcom + F * unit_vec / m_;
     }
   }
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
index 5a98a435b1..558a3ab1f7 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h
@@ -68,6 +68,8 @@ class SlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint<T> {
   const double b_;
   const T m_;
   const int n_feet_;
+
+  // Boolean vectors describing which feet are active (in stance).
   const std::vector<bool> contact_mask0_;
   const std::vector<bool> contact_mask1_;
   const double dt_;
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
index 289f1b7372..8b1ebe5b72 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc
@@ -218,7 +218,7 @@ drake::VectorX<double> SlipLifter::LiftGrf(
                     .dot(com_vel);
     double slip_grf_mag =
         slip_contact_mask_[simple_index]
-            ? SlipGrf<double>(k_, r0_, b_, r, dr, slip_force[simple_index])
+            ? CalcSlipGrf<double>(k_, r0_, b_, r, dr, slip_force[simple_index])
             : 0;
     // Find the average location for all of the complex contact points that make
     // up the SLIP foot
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc
index 325a106db4..284cbaf002 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc
@@ -95,7 +95,7 @@ drake::VectorX<double> SlipReducer::ReduceGrf(
     const Eigen::VectorXd unit_vec =
         (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized();
     const double dr = unit_vec.dot(com_vel);
-    const auto spring_force = SlipGrf<double>(k_, r0_, b_, r, dr, 0);
+    const auto spring_force = CalcSlipGrf<double>(k_, r0_, b_, r, dr, 0);
     auto complex_feet_it = simple_foot_index_to_complex_foot_index_.find(i);
     Eigen::Vector3d complex_force = Eigen::Vector3d::Zero(3);
     for (const auto complex_index : complex_feet_it->second) {
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
index 49593ee981..4334c61dcb 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc
@@ -5,18 +5,16 @@
 #include "multibody/multibody_utils.h"
 
 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;
+T CalcSlipGrf(double k, double r0, double b, T r, T dr, T force) {
+  auto F = force + k * (r0 - r) + b * (0 - dr);
   if (F < 0) {
     F = 0;
   }
   return F;
 }
 
-template double SlipGrf<double>(double k, double r0, double b, double r,
-                                double dr, double force);
-template drake::AutoDiffXd SlipGrf<drake::AutoDiffXd>(double k, double r0,
-                                                      double b,
-                                                      drake::AutoDiffXd r,
-                                                      drake::AutoDiffXd dr,
-                                                      drake::AutoDiffXd force);
+template double CalcSlipGrf<double>(double k, double r0, double b, double r,
+                                    double dr, double force);
+template drake::AutoDiffXd CalcSlipGrf<drake::AutoDiffXd>(
+    double k, double r0, double b, drake::AutoDiffXd r, drake::AutoDiffXd dr,
+    drake::AutoDiffXd force);
diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h
index a297a92de2..8cbdbe1183 100644
--- a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h
+++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h
@@ -1,4 +1,4 @@
 #pragma once
 
 template <typename T>
-T SlipGrf(double k, double r0, double b, T r, T dr, T force);
+T CalcSlipGrf(double k, double r0, double b, T r, T dr, T force);
diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
index e4dfa7fac2..08d6a403a3 100644
--- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
+++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc
@@ -66,8 +66,6 @@ KinematicCentroidalSolver::KinematicCentroidalSolver(
   std::vector<bool> stance_mode(n_contact_points_);
   std::fill(stance_mode.begin(), stance_mode.end(), true);
   std::fill(contact_sequence_.begin(), contact_sequence_.end(), stance_mode);
-  std::fill(complexity_schedule_.begin(), complexity_schedule_.end(),
-            Complexity::KINEMATIC_CENTROIDAL);
 }
 
 void KinematicCentroidalSolver::SetGenPosReference(