Enable Fixed Complexity Slip MPC

bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml

@@ -5,4 +5,4 @@ visualize_mode: 2
 realtime_rate: 1.0
 num_poses: 10
 use_transparency: 1
-use_springs: 1
+use_springs: 1

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,14 +76,15 @@ cc_library(
 )
 
 cc_library(
-    name = "cassie_reference_utils",
-    srcs = ["cassie_reference_utils.cc"],
-    hdrs = ["cassie_reference_utils.h"],
+    name = "cassie_kc_utils",
+    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",
     ],
 )
@@ -97,6 +98,7 @@ cc_library(
         "//common",
         "//systems/trajectory_optimization/kinematic_centroidal_planner",
         "//multibody:visualization_utils",
+        "//examples/Cassie/kinematic_centroidal_planner/simple_models:slip",
         "@drake//:drake_shared_library",
     ],
 )

examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc → 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) {
+    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;
+      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);
+      }
+    }
+  }
+  DRAKE_DEMAND(n_knot_points == complexity_schedule.size());
+  return complexity_schedule;
+}

examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h → 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);
+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);

examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc

@@ -1,3 +1,5 @@
+#include <unistd.h>
+
 #include <iostream>
 
 #include <drake/common/yaml/yaml_io.h>
@@ -16,12 +18,13 @@
 
 #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"
 #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;
@@ -33,7 +36,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,
@@ -90,25 +93,35 @@ 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());
   std::vector<double> time_points = KcmpcReferenceGenerator::GenerateTimePoints(
       traj_params.duration_scaling, gait_samples);
 
+  Eigen::VectorXd reference_state = GenerateNominalStand(
+      plant, traj_params.target_com_height, traj_params.stance_width, false);
+
   // Create MPC and set gains
   CassieKinematicCentroidalSolver 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()), 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);
+  mpc.SetMaximumSlipLegLength(traj_params.max_slip_leg_length);
+
+  mpc.SetComplexitySchedule(GenerateComplexitySchedule(
+      traj_params.n_knot_points, traj_params.complexity_schedule));
 
   KcmpcReferenceGenerator generator(plant, plant_context.get(),
                                     CreateContactPoints(plant, 0));
 
-  Eigen::VectorXd reference_state = GenerateNominalStand(
-      plant, traj_params.target_com_height, traj_params.stance_width, false);
   generator.SetNominalReferenceConfiguration(
       reference_state.head(plant.num_positions()));
   generator.SetComKnotPoints({time_points, traj_params.com_vel_vector});
@@ -131,7 +144,9 @@ int DoMain(int argc, char* argv[]) {
   mpc.SetContactTrackingReference(
       std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
           generator.contact_traj_));
-  mpc.SetConstantMomentumReference(Eigen::VectorXd::Zero(6));
+  mpc.SetMomentumReference(
+      std::make_unique<drake::trajectories::PiecewisePolynomial<double>>(
+          generator.momentum_trajectory_));
   mpc.SetModeSequence(generator.contact_sequence_);
   //
   //  // Set initial guess
@@ -140,6 +155,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;
@@ -148,7 +164,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");
@@ -204,12 +220,11 @@ 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);
   }
 }
 
-int main(int argc, char* argv[]) {
-  DoMain(argc, argv);
-}
+int main(int argc, char* argv[]) { DoMain(argc, argv); }