Add different high level command modes

Author: yminchen

bindings/pydairlib/analysis_scripts/log_plotter_cassie.py

@@ -170,7 +170,7 @@ def main():
 
   plot_state(x, t_x, u, t_u, x_datatypes, u_datatypes, t_osc_debug, fsm)
 
-  plot_osc_debug(t_osc_debug, fsm, osc_debug, osc_debug_reg_cost, t_cassie_out, estop_signal, osc_output)
+  # plot_osc_debug(t_osc_debug, fsm, osc_debug, osc_debug_reg_cost, t_cassie_out, estop_signal, osc_output)
 
   # plot_feet_positions(plant_w_spr, context, x, l_toe_frame, mid_contact_disp, world,
   #   t_x, t_slice, t_osc_debug, fsm, "left foot", True)
@@ -225,6 +225,15 @@ def main():
   # print("shell command = ", cmd)
   # RunCommand(cmd, True)
 
+  ### Compute torque square
+  # # u_cost = 0.0
+  # # for i in range(len(u)):
+  # #   u_cost += u[i].T @ u[i]
+  # u_cost = np.sum(u**2)
+  # u_cost /= len(u)
+  # print(u_cost)
+  # import pdb;pdb.set_trace()
+
   plt.show()
 
 

examples/Cassie/osc/high_level_command.cc

@@ -34,6 +34,8 @@ using drake::systems::LeafSystem;
 using drake::multibody::JacobianWrtVariable;
 using drake::trajectories::PiecewisePolynomial;
 
+using drake::MatrixX;
+
 namespace dairlib {
 namespace cassie {
 namespace osc {
@@ -47,7 +49,8 @@ HighLevelCommand::HighLevelCommand(
       this->DeclareAbstractInputPort("lcmt_cassie_output",
                                      drake::Value<dairlib::lcmt_cassie_out>{})
           .get_index();
-  use_radio_command_ = true;
+  //  use_radio_command_ = true;
+  high_level_mode_ = radio;
 
   vel_scale_rot_ = vel_scale_rot;
   vel_scale_trans_sagital_ = vel_scale_trans_sagital;
@@ -63,7 +66,9 @@ HighLevelCommand::HighLevelCommand(
     const Vector2d& global_target_position,
     const Vector2d& params_of_no_turning)
     : HighLevelCommand(plant, context) {
-  use_radio_command_ = false;
+  //  use_radio_command_ = false;
+  high_level_mode_ = desired_xy_position;
+
   kp_yaw_ = kp_yaw;
   kd_yaw_ = kd_yaw;
   vel_max_yaw_ = vel_max_yaw;
@@ -107,10 +112,51 @@ HighLevelCommand::HighLevelCommand(
   des_vel_idx_ = DeclareDiscreteState(VectorXd::Zero(3));
 }
 
+void HighLevelCommand::SetOpenLoopVelCommandTraj() {
+  high_level_mode_ = open_loop_vel_command_traj;
+
+  std::vector<double> breaks = {0};
+  std::vector<MatrixXd> knots = {(MatrixX<double>(3, 1) << 0, 0, 0).finished()};
+  breaks.push_back(breaks.back() + 2);
+  knots.push_back((MatrixX<double>(3, 1) << 0, 0, 0).finished());
+  breaks.push_back(breaks.back() + 5);
+  knots.push_back((MatrixX<double>(3, 1) << 0, 2, 0).finished());
+  breaks.push_back(breaks.back() + 2);
+  knots.push_back((MatrixX<double>(3, 1) << 0.7, 2, 0).finished());
+  breaks.push_back(breaks.back() + 1.5);
+  knots.push_back((MatrixX<double>(3, 1) << 0.7, 2, 0).finished());
+  breaks.push_back(breaks.back() + 2);
+  knots.push_back((MatrixX<double>(3, 1) << 0, 2, 0).finished());
+  breaks.push_back(breaks.back() + 2);
+  knots.push_back((MatrixX<double>(3, 1) << 0, 2, 0).finished());
+  breaks.push_back(breaks.back() + 1);
+  knots.push_back((MatrixX<double>(3, 1) << 0, 0, 0).finished());
+  breaks.push_back(breaks.back() + 10000000);
+  knots.push_back((MatrixX<double>(3, 1) << 0, 0, 0).finished());
+
+  // Construct the PiecewisePolynomial.
+  desired_vel_command_traj_ =
+      PiecewisePolynomial<double>::FirstOrderHold(breaks, knots);
+};
+
+void HighLevelCommand::SetDesiredXYTraj() {
+  high_level_mode_ = desired_xy_traj;
+
+  std::vector<double> breaks = {0};
+  std::vector<MatrixXd> knots = {(MatrixX<double>(3, 1) << 0, 0, 0).finished()};
+
+  // TODO: finish this
+  DRAKE_UNREACHABLE();
+
+  // Construct the PiecewisePolynomial.
+  desired_xy_traj_ =
+      PiecewisePolynomial<double>::FirstOrderHold(breaks, knots);
+};
+
 EventStatus HighLevelCommand::DiscreteVariableUpdate(
     const Context<double>& context,
     DiscreteValues<double>* discrete_state) const {
-  if (use_radio_command_) {
+  if (high_level_mode_ == radio) {
     const auto& cassie_out = this->EvalInputValue<dairlib::lcmt_cassie_out>(
         context, cassie_out_port_);
     // TODO(yangwill) make sure there is a message available
@@ -123,9 +169,15 @@ EventStatus HighLevelCommand::DiscreteVariableUpdate(
         vel_scale_trans_lateral_ * cassie_out->pelvis.radio.channel[1];
     des_vel(1) += vel_command_offset_x_;  //hack: help rom_iter=300 walk in place at start
     discrete_state->get_mutable_vector(des_vel_idx_).set_value(des_vel);
-  } else {
+  } else if (high_level_mode_ == desired_xy_position) {
     discrete_state->get_mutable_vector(des_vel_idx_)
         .set_value(CalcCommandFromTargetPosition(context));
+  } else if (high_level_mode_ == open_loop_vel_command_traj) {
+    discrete_state->get_mutable_vector(des_vel_idx_)
+        .set_value(desired_vel_command_traj_.value(context.get_time()));
+  } else if (high_level_mode_ == desired_xy_traj) {
+    discrete_state->get_mutable_vector(des_vel_idx_)
+        .set_value(CalcCommandFromDesiredXYTraj(context));
   }
 
   return EventStatus::Succeeded();
@@ -228,6 +280,14 @@ VectorXd HighLevelCommand::CalcCommandFromTargetPosition(
   return des_vel;
 }
 
+VectorXd HighLevelCommand::CalcCommandFromDesiredXYTraj(
+    const Context<double>& context) const {
+  Vector3d des_vel;
+
+  DRAKE_UNREACHABLE();
+  return des_vel;
+}
+
 void HighLevelCommand::CopyHeadingAngle(const Context<double>& context,
                                         BasicVector<double>* output) const {
   double desired_heading_pos =

examples/Cassie/osc/high_level_command.h

@@ -68,6 +68,9 @@ class HighLevelCommand : public drake::systems::LeafSystem<double> {
                    const Eigen::Vector2d& global_target_position,
                    const Eigen::Vector2d& params_of_no_turning);
 
+  HighLevelCommand(const drake::multibody::MultibodyPlant<double>& plant,
+                   drake::systems::Context<double>* context);
+
   // Input/output ports
   const drake::systems::InputPort<double>& get_state_input_port() const {
     return this->get_input_port(state_port_);
@@ -86,10 +89,11 @@ class HighLevelCommand : public drake::systems::LeafSystem<double> {
   double vel_command_offset_x_ = 0;
   /// ///
 
- private:
-  HighLevelCommand(const drake::multibody::MultibodyPlant<double>& plant,
-                   drake::systems::Context<double>* context);
+  // Hacks -- setting desired command trajectory
+  void SetOpenLoopVelCommandTraj();
+  void SetDesiredXYTraj();
 
+ private:
   drake::systems::EventStatus DiscreteVariableUpdate(
       const drake::systems::Context<double>& context,
       drake::systems::DiscreteValues<double>* discrete_state) const;
@@ -108,7 +112,7 @@ class HighLevelCommand : public drake::systems::LeafSystem<double> {
   drake::systems::Context<double>* context_;
   const drake::multibody::BodyFrame<double>& world_;
   const drake::multibody::Body<double>& pelvis_;
-  bool use_radio_command_;
+  // bool use_radio_command_;
 
   // Port index
   int state_port_;
@@ -141,6 +145,20 @@ class HighLevelCommand : public drake::systems::LeafSystem<double> {
   double vel_scale_rot_;
   double vel_scale_trans_sagital_;
   double vel_scale_trans_lateral_;
+
+  // Testing
+  enum high_level_modes {
+    radio,
+    desired_xy_position,
+    open_loop_vel_command_traj,
+    desired_xy_traj
+  };
+
+  int high_level_mode_;
+  drake::trajectories::PiecewisePolynomial<double> desired_vel_command_traj_;
+  drake::trajectories::PiecewisePolynomial<double> desired_xy_traj_;
+  Eigen::VectorXd CalcCommandFromDesiredXYTraj(
+      const drake::systems::Context<double>& context) const;
 };
 
 }  // namespace osc

examples/goldilocks_models/procman_scripts/cassie_controller_sim_fast_turning.pmd

@@ -31,7 +31,7 @@ group "1.rom-space-controller" {
         host = "localhost";
     }
     cmd "3.1.rom-planner (short time limit)" {
-        exec = "./bazel-bin/examples/goldilocks_models/run_cassie_rom_planner_process --zero_touchdown_impact=true --fix_duration=true --use_ipopt=true --n_step=2 --feas_tol=1e-2 --log_solver_info=false  --knots_per_mode=5 --spring_model=false --min_mpc_thread_loop_duration=0.04";
+        exec = "./bazel-bin/examples/goldilocks_models/run_cassie_rom_planner_process --zero_touchdown_impact=true --fix_duration=true --use_ipopt=true --n_step=2 --feas_tol=1e-2 --log_solver_info=false  --knots_per_mode=5 --spring_model=false --min_mpc_thread_loop_duration=0.04 --pelvis_height=0.9";
         host = "localhost";
     }
     cmd "3.3.rom-planner (debug a given solve_idx)" {

examples/goldilocks_models/procman_scripts/cassie_controller_sim_uneven_terrain.pmd

@@ -177,7 +177,7 @@ group "5.lcm-tools" {
 
 group "1.rom-space-controller-with-init-traj" {
     cmd "2.1.rom-walking-controller (w init)" {
-        exec = "bazel-bin/examples/goldilocks_models/run_cassie_rom_controller --channel_u=ROM_WALKING --use_IK=false --get_swing_foot_from_planner=true --close_sim_gap=false --spring_model=false --init_traj_file_name=0_rom_trajectory --offset_swing_hip_yaw_for_heading=true ";
+        exec = "bazel-bin/examples/goldilocks_models/run_cassie_rom_controller --channel_u=ROM_WALKING --use_IK=false --get_swing_foot_from_planner=true --close_sim_gap=false --spring_model=false --init_traj_file_name=0_rom_trajectory --offset_swing_hip_yaw_for_heading=true  --high_level_traj_mode=open_loop_desired_vel_command";
         host = "localhost";
     }
     cmd "2.2.rom-planner (w init)" {

examples/goldilocks_models/run_cassie_rom_controller.cc

@@ -182,6 +182,13 @@ DEFINE_bool(offset_swing_hip_yaw_for_heading, false,
 
 DEFINE_double(ground_incline, 0.0, "ground incline");
 
+// Testing -- to create different behavior for Cassie
+DEFINE_string(high_level_traj_mode, "default",
+              "Options:"
+              "default"
+              "open_loop_desired_vel_command"
+              "desired_x_y_yaw_path");
+
 int DoMain(int argc, char* argv[]) {
   gflags::ParseCommandLineFlags(&argc, &argv, true);
 
@@ -513,30 +520,43 @@ int DoMain(int argc, char* argv[]) {
   Eigen::Vector2d params_of_no_turning(gains.yaw_deadband_blur,
                                        gains.yaw_deadband_radius);
   cassie::osc::HighLevelCommand* high_level_command;
-  if (gains.use_radio) {
-    high_level_command = builder.AddSystem<cassie::osc::HighLevelCommand>(
-        plant_w_spr, context_w_spr.get(), gains.vel_scale_rot,
-        gains.vel_scale_trans_sagital, gains.vel_scale_trans_lateral);
-    auto cassie_out_receiver =
-        builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_cassie_out>(
-            gains.use_virtual_radio ? "CASSIE_OUTPUT_ONLY_RADIO"
-                                    : FLAGS_cassie_out_channel,
-            &lcm_local));
-    builder.Connect(cassie_out_receiver->get_output_port(),
-                    high_level_command->get_cassie_output_port());
-
-    // hack: help rom_iter=300 walk in place at start
-    if (!FLAGS_close_sim_gap) {
-      high_level_command->vel_command_offset_x_ =
-          gains.vel_command_offset_x * ((model_iter - 1) / 300);
+  if (FLAGS_high_level_traj_mode == "default") {
+    if (gains.use_radio) {
+      high_level_command = builder.AddSystem<cassie::osc::HighLevelCommand>(
+          plant_w_spr, context_w_spr.get(), gains.vel_scale_rot,
+          gains.vel_scale_trans_sagital, gains.vel_scale_trans_lateral);
+      auto cassie_out_receiver =
+          builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_cassie_out>(
+              gains.use_virtual_radio ? "CASSIE_OUTPUT_ONLY_RADIO"
+                                      : FLAGS_cassie_out_channel,
+              &lcm_local));
+      builder.Connect(cassie_out_receiver->get_output_port(),
+                      high_level_command->get_cassie_output_port());
+
+      // hack: help rom_iter=300 walk in place at start
+      if (!FLAGS_close_sim_gap) {
+        high_level_command->vel_command_offset_x_ =
+            gains.vel_command_offset_x * ((model_iter - 1) / 300);
+      }
+    } else {
+      high_level_command = builder.AddSystem<cassie::osc::HighLevelCommand>(
+          plant_w_spr, context_w_spr.get(), gains.kp_yaw, gains.kd_yaw,
+          gains.vel_max_yaw, gains.kp_pos_sagital, gains.kd_pos_sagital,
+          gains.vel_max_sagital, gains.kp_pos_lateral, gains.kd_pos_lateral,
+          gains.vel_max_lateral, gains.target_pos_offset,
+          global_target_position, params_of_no_turning);
     }
-  } else {
+  } else if (FLAGS_high_level_traj_mode == "open_loop_desired_vel_command") {
+    // hack -- testing desired command velocity traj
     high_level_command = builder.AddSystem<cassie::osc::HighLevelCommand>(
-        plant_w_spr, context_w_spr.get(), gains.kp_yaw, gains.kd_yaw,
-        gains.vel_max_yaw, gains.kp_pos_sagital, gains.kd_pos_sagital,
-        gains.vel_max_sagital, gains.kp_pos_lateral, gains.kd_pos_lateral,
-        gains.vel_max_lateral, gains.target_pos_offset, global_target_position,
-        params_of_no_turning);
+        plant_w_spr, context_w_spr.get());
+    high_level_command->SetOpenLoopVelCommandTraj();
+  } else if (FLAGS_high_level_traj_mode == "desired_x_y_yaw_path") {
+    high_level_command = builder.AddSystem<cassie::osc::HighLevelCommand>(
+        plant_w_spr, context_w_spr.get());
+    high_level_command->SetDesiredXYTraj();
+  } else {
+    DRAKE_UNREACHABLE();
   }
   builder.Connect(state_receiver->get_output_port(0),
                   high_level_command->get_state_input_port());

examples/goldilocks_models/terrains/stones.yaml

@@ -16,7 +16,7 @@ stones:
 #  - [[1, 0.0, 0.5], [0.0, 0.0, 1.0], [0.25, 0.25, 0.5], [0.0]]
 
 #  - [[0.0, 0, 0.0], [0.0, 0.0, 1.0], [5.0, 5.0, 0.1], [0.0]]   # small middle terrain
-#  - [[0.0, 0, 0.0], [0.0, 0.0, 1.0], [25.0, 25.0, 0.1], [0.0]]   # big middle terrain
+  - [[0.0, 0, 0.0], [0.0, 0.0, 1.0], [25.0, 25.0, 0.1], [0.0]]   # big middle terrain
 
 ################### Ramps #####################
 
@@ -31,9 +31,9 @@ stones:
 #  - [[5.5, 0, 0.2], [0.0, 0.0, 1.0], [5.0, 5.0, 0.1], [0.0]]
 
 # Ramp with 20% incline
-  - [[0.0, 0, 0.0], [0.0, 0.0, 1.0], [5.0, 5.0, 0.1], [0.0]]
-  - [[1, 0, 0.0], [-0.2, 0.0, 1.0], [4.08, 4.0, 0.1], [0.0]]   # 4 / cos(arctan(0.2))
-  - [[5.5, 0, 0.4], [0.0, 0.0, 1.0], [5.0, 5.0, 0.1], [0.0]]
+#  - [[0.0, 0, 0.0], [0.0, 0.0, 1.0], [5.0, 5.0, 0.1], [0.0]]
+#  - [[1, 0, 0.0], [-0.2, 0.0, 1.0], [4.08, 4.0, 0.1], [0.0]]   # 4 / cos(arctan(0.2))
+#  - [[5.5, 0, 0.4], [0.0, 0.0, 1.0], [5.0, 5.0, 0.1], [0.0]]
 
 ################### Roads #####################