Add lcmt_osc_output; log regularization cost; update log_cassie_plotter accordingly

Author: yminchen

bindings/pydairlib/analysis_scripts/log_plotter_cassie.py

@@ -100,7 +100,7 @@ def main():
 
   # Load the main log file
   x, u_meas, imu_aceel, t_x, u, t_u, contact_switch, t_contact_switch, contact_info, contact_info_locs, t_contact_info, \
-  osc_debug, t_osc_debug, fsm, estop_signal, switch_signal, t_controller_switch, t_pd, kp, kd, cassie_out, u_pd, t_u_pd, \
+  osc_debug, osc_debug_reg_cost, t_osc_debug, fsm, estop_signal, switch_signal, t_controller_switch, t_pd, kp, kd, cassie_out, u_pd, t_u_pd, \
   u_dispatcher, t_u_dispatcher, \
   osc_output, input_supervisor_status, t_input_supervisor, full_log = process_lcm_log.process_log(log, pos_map, vel_map, act_map, controller_channel)
 
@@ -198,7 +198,7 @@ def main():
   if filename2 != "":
     log2 = lcm.EventLog(filename2, "r")
     x2, u_meas, imu_aceel, t_x2, u, t_u, contact_switch, t_contact_switch, contact_info, contact_info_locs, t_contact_info, \
-    osc_debug, t_osc_debug, fsm, estop_signal, switch_signal, t_controller_switch, t_pd, kp, kd, cassie_out, u_pd, t_u_pd, \
+    osc_debug, osc_debug_reg_cost, t_osc_debug, fsm, estop_signal, switch_signal, t_controller_switch, t_pd, kp, kd, cassie_out, u_pd, t_u_pd, \
     u_dispatcher, t_u_dispatcher, \
     osc_output, input_supervisor_status, t_input_supervisor, full_log = process_lcm_log.process_log(log2, pos_map, vel_map, act_map, controller_channel)
 
@@ -665,36 +665,76 @@ def plot_contact_est(log, t_osc_debug, fsm, t_u, u, t_x, x, u_meas):
   # plt.legend(["l_contact", "r_contact", "fsm", "pelvis y (%dx)" % y_scale, "pelvis ydot (%dx)" % ydot_scale])
 
 
-def plot_osc_debug(t_osc_debug, fsm, osc_debug, t_cassie_out, estop_signal, osc_output):
-  input_cost = np.zeros(t_osc_debug.shape[0])
-  acceleration_cost = np.zeros(t_osc_debug.shape[0])
-  soft_constraint_cost = np.zeros(t_osc_debug.shape[0])
-  tracking_cost = np.zeros((t_osc_debug.shape[0], len(osc_debug)))
+def plot_osc_debug(t_osc_debug, fsm, osc_debug, osc_debug_reg_cost, t_cassie_out, estop_signal, osc_output):
+  """
+  1. Plot trajectories
+  """
+  # input_cost = np.zeros(t_osc_debug.shape[0])
+  # acceleration_cost = np.zeros(t_osc_debug.shape[0])
+  # soft_constraint_cost = np.zeros(t_osc_debug.shape[0])
+  tracking_costs = np.zeros((t_osc_debug.shape[0], len(osc_debug)))
   tracking_cost_map = dict()
   num_tracking_cost = 0
+  reg_costs = np.zeros((t_osc_debug.shape[0], len(osc_debug_reg_cost)))
+  reg_cost_map = dict()
+  num_reg_cost = 0
 
+  reg_cost_names = list(osc_debug_reg_cost.keys())
+
+  # i is the time index
   for i in range(t_osc_debug.shape[0]):
-    input_cost[i] = osc_output[i].input_cost
-    acceleration_cost[i] = osc_output[i].acceleration_cost
-    soft_constraint_cost[i] = osc_output[i].soft_constraint_cost
+    # input_cost[i] = osc_output[i].input_cost
+    # acceleration_cost[i] = osc_output[i].acceleration_cost
+    # soft_constraint_cost[i] = osc_output[i].soft_constraint_cost
     for j in range(len(osc_output[i].tracking_data_names)):
       name = osc_output[i].tracking_data_names[j]
-      if osc_output[i].tracking_data_names[j] not in tracking_cost_map:
+      if name not in tracking_cost_map:
         tracking_cost_map[name] = num_tracking_cost
         num_tracking_cost += 1
-      tracking_cost[i, tracking_cost_map[name]] = osc_output[i].tracking_cost[j]
+      tracking_costs[i, tracking_cost_map[name]] = osc_output[i].tracking_costs[j]
+
+    for j in range(len(reg_cost_names)):
+      name = reg_cost_names[j]
+      if name not in reg_cost_map:
+        reg_cost_map[name] = num_reg_cost
+        num_reg_cost += 1
+      reg_costs[i, reg_cost_map[name]] = osc_debug_reg_cost[name][i]
 
   for name in tracking_cost_map.keys():
     print(name)
     print(tracking_cost_map[name])
+  print("")
+  for name in reg_cost_map.keys():
+    print(name)
+    print(reg_cost_map[name])
+
+  # sanity check cost >= 0
+  tracking_cost_nonnegative = np.all(tracking_costs >= 0, axis=0)
+  for i in range(len(tracking_cost_nonnegative)):
+    if not tracking_cost_nonnegative[i]:
+      print(list(tracking_cost_map.keys())[i], " has negative cost!!!")
+  reg_cost_nonnegative = np.all(reg_costs >= 0, axis=0)
+  for i in range(len(reg_cost_nonnegative)):
+    if not reg_cost_nonnegative[i]:
+      print(reg_cost_names[i], " has negative cost!!!")
+  if not np.all(tracking_cost_nonnegative) or not np.all(reg_cost_nonnegative):
+    raise ValueError("There is a negative cost")
+
+  import pdb;pdb.set_trace()
 
   plt.figure("costs")
-  plt.plot(t_osc_debug[t_osc_debug_slice], input_cost[t_osc_debug_slice])
-  plt.plot(t_osc_debug[t_osc_debug_slice], acceleration_cost[t_osc_debug_slice])
-  plt.plot(t_osc_debug[t_osc_debug_slice], soft_constraint_cost[t_osc_debug_slice])
-  plt.plot(t_osc_debug[t_osc_debug_slice], tracking_cost[t_osc_debug_slice])
-  plt.legend(['input_cost', 'acceleration_cost', 'soft_constraint_cost'] +
-             list(tracking_cost_map))
+  # plt.plot(t_osc_debug[t_osc_debug_slice], input_cost[t_osc_debug_slice])
+  # plt.plot(t_osc_debug[t_osc_debug_slice], acceleration_cost[t_osc_debug_slice])
+  # plt.plot(t_osc_debug[t_osc_debug_slice], soft_constraint_cost[t_osc_debug_slice])
+  plt.plot(t_osc_debug[t_osc_debug_slice], tracking_costs[t_osc_debug_slice])
+  plt.plot(t_osc_debug[t_osc_debug_slice], reg_costs[t_osc_debug_slice], "--")
+  # plt.legend(['input_cost', 'acceleration_cost', 'soft_constraint_cost'] +
+  #            list(tracking_cost_map))
+  plt.legend(list(tracking_cost_map) + list(reg_cost_map))
+
+  """
+  2. Plot trajectories
+  """
   osc_traj00 = "swing_ft_traj"
   # osc_traj00 = "stance_hip_rpy_traj"
   # osc_traj0 = "swing_ft_traj"

bindings/pydairlib/analysis_scripts/process_lcm_log.py

@@ -119,6 +119,7 @@ def process_log(log, pos_map, vel_map, act_map, controller_channel = ""):
   estop_signal = []
   switch_signal = []
   osc_debug = dict()
+  osc_debug_reg_cost = dict()
   contact_forces = [[], [], [], []]  # Allocate space for all 4 point contacts
   contact_info_locs = [[], [], [], []]
   cassie_out = []  # Cassie out types
@@ -147,7 +148,7 @@ def process_log(log, pos_map, vel_map, act_map, controller_channel = ""):
   print("osc_debug_channel_name = " + osc_debug_channel_name)
 
   t_dispatcher_end = -1   # stop reading lcm messages after this time
-  # t_dispatcher_end = 256.5
+  # t_dispatcher_end = 74.6
 
   for event in log:
     if event.channel not in full_log and event.channel not in unknown_types:
@@ -220,13 +221,19 @@ def process_log(log, pos_map, vel_map, act_map, controller_channel = ""):
       # and then rebuild the plotting script.
       msg = dairlib.lcmt_osc_output.decode(event.data)
       osc_output.append(msg)
+      t_osc_debug.append(msg.utime / 1e6)
+      fsm.append(msg.fsm_state)
       num_osc_tracking_data = len(msg.tracking_data)
       for i in range(num_osc_tracking_data):
         if msg.tracking_data[i].name not in osc_debug:
           osc_debug[msg.tracking_data[i].name] = lcmt_osc_tracking_data_t()
         osc_debug[msg.tracking_data[i].name].append(msg.tracking_data[i], msg.utime / 1e6)
-      t_osc_debug.append(msg.utime / 1e6)
-      fsm.append(msg.fsm_state)
+      num_osc_reg_cost = len(msg.regularization_costs)
+      for i in range(num_osc_reg_cost):
+        if msg.regularization_cost_names[i] not in osc_debug_reg_cost:
+          osc_debug_reg_cost[msg.regularization_cost_names[i]] = [msg.regularization_costs[i]]
+        osc_debug_reg_cost[msg.regularization_cost_names[i]].append(msg.regularization_costs[i])
+      # import pdb;pdb.set_trace()
     if event.channel == "CASSIE_ACCELERATION":
       msg = dairlib.lcmt_timestamped_vector.decode(event.data)
       vdot.append(msg.data)
@@ -315,10 +322,12 @@ def process_log(log, pos_map, vel_map, act_map, controller_channel = ""):
 
   for key in osc_debug:
     osc_debug[key].convertToNP()
+  for key in osc_debug_reg_cost:
+    osc_debug_reg_cost[key] = np.array(osc_debug_reg_cost[key])
 
   x = np.hstack((q, v))  # combine into state vector
 
   return x, u_meas, imu_aceel, t_x, u, t_u, contact_switch, t_contact_switch, contact_forces, contact_info_locs, \
-         t_contact_info, osc_debug, t_osc_debug, fsm, estop_signal, \
+         t_contact_info, osc_debug, osc_debug_reg_cost, t_osc_debug, fsm, estop_signal, \
          switch_signal, t_controller_switch, t_pd, kp, kd, cassie_out, u_pd, \
          t_u_pd, u_dispatcher, t_u_dispatcher, osc_output, input_supervisor_status, t_input_supervisor, full_log

lcmtypes/lcmt_osc_output.lcm

@@ -1,3 +1,6 @@
+/*
+Before 20221122:
+
 package dairlib;
 
 struct lcmt_osc_output
@@ -15,3 +18,22 @@ struct lcmt_osc_output
   double soft_constraint_cost;
   double tracking_cost [num_tracking_data];
 }
+*/
+
+package dairlib;
+
+struct lcmt_osc_output
+{
+  int64_t utime;
+  int32_t fsm_state;
+  int32_t num_tracking_data;
+  int32_t num_regularization_costs;
+
+  lcmt_osc_qp_output qp_output;
+  string tracking_data_names[num_tracking_data];
+  double tracking_costs [num_tracking_data];
+  double regularization_costs [num_regularization_costs];
+  string regularization_cost_names [num_regularization_costs];
+
+  lcmt_osc_tracking_data tracking_data[num_tracking_data];
+}

systems/controllers/osc/operational_space_control.cc

@@ -492,10 +492,11 @@ void OperationalSpaceControl::Build() {
     /// hard constraint version
     prog_->AddBoundingBoxConstraint(0, 0, epsilon_blend_);
   }
+
   // Testing -- penalize the front contact for optimal model as a regularization
   // term, in order to help the solver to find good solution.
   if (w_rom_force_reg_ > 0) {
-    contact_force_reg_cost_ =
+    front_contact_force_reg_cost_ =
         prog_
             ->AddQuadraticCost(
                 w_rom_force_reg_ * MatrixXd::Identity(2, 2), VectorXd::Zero(2),
@@ -928,25 +929,36 @@ VectorXd OperationalSpaceControl::SolveQp(
   }
 
   // (Testing) 7. Cost for staying close to the previous input
-  if (w_input_reg_ > 0) {
-    input_reg_cost_->UpdateCoefficients(W_input_reg_,
-                                        -W_input_reg_ * (*u_sol_));
+  if (W_input_reg_.size() > 0) {
+    //    input_reg_cost_->UpdateCoefficients(W_input_reg_,
+    //                                        -W_input_reg_ * (*u_sol_));
+    input_reg_cost_->UpdateCoefficients(
+        W_input_reg_, -W_input_reg_ * (*u_sol_),
+        0.5 * u_sol_->transpose() * W_input_reg_ * (*u_sol_));
   }
   // (Testing) Cost for staying close to the previous solution
   if (W_lambda_c_reg_.size() > 0) {
     //    lambda_c_cost_->UpdateCoefficients(W_lambda_c_reg_,
-    //    VectorXd::Zero(n_c_));
-    lambda_c_cost_->UpdateCoefficients(W_lambda_c_reg_,
-                                       -W_lambda_c_reg_ * (*lambda_c_sol_));
+    //                                       -W_lambda_c_reg_ *
+    //                                       (*lambda_c_sol_));
+    lambda_c_cost_->UpdateCoefficients(
+        W_lambda_c_reg_, -W_lambda_c_reg_ * (*lambda_c_sol_),
+        0.5 * lambda_c_sol_->transpose() * W_lambda_c_reg_ * (*lambda_c_sol_));
   }
   if (W_lambda_h_reg_.size() > 0) {
     //    lambda_h_cost_->UpdateCoefficients(W_lambda_h_reg_,
-    //    VectorXd::Zero(n_h_));
-    lambda_h_cost_->UpdateCoefficients(W_lambda_h_reg_,
-                                       -W_lambda_h_reg_ * (*lambda_h_sol_));
+    //                                       -W_lambda_h_reg_ *
+    //                                       (*lambda_h_sol_));
+    lambda_h_cost_->UpdateCoefficients(
+        W_lambda_h_reg_, -W_lambda_h_reg_ * (*lambda_h_sol_),
+        0.5 * lambda_h_sol_->transpose() * W_lambda_h_reg_ * (*lambda_h_sol_));
   }
   if (W_vdot_reg_.size() > 0) {
-    vdot_reg_cost_->UpdateCoefficients(W_vdot_reg_, -W_vdot_reg_ * (*dv_sol_));
+    //    vdot_reg_cost_->UpdateCoefficients(W_vdot_reg_, -W_vdot_reg_ *
+    //    (*dv_sol_));
+    vdot_reg_cost_->UpdateCoefficients(
+        W_vdot_reg_, -W_vdot_reg_ * (*dv_sol_),
+        0.5 * dv_sol_->transpose() * W_vdot_reg_ * (*dv_sol_));
   }
 
   if (is_rom_modification_) {
@@ -1120,23 +1132,65 @@ void OperationalSpaceControl::AssignOscLcmOutput(
 
   output->utime = state->get_timestamp() * 1e6;
   output->fsm_state = fsm_state;
-  output->input_cost =
+  double input_cost =
       (W_input_.size() > 0)
           ? (0.5 * (*u_sol_).transpose() * W_input_ * (*u_sol_))(0)
           : 0;
-  output->acceleration_cost =
+  double acceleration_cost =
       (W_joint_accel_.size() > 0)
           ? (0.5 * (*dv_sol_).transpose() * W_joint_accel_ * (*dv_sol_))(0)
           : 0;
-  output->soft_constraint_cost =
+  double soft_constraint_cost =
       (w_soft_constraint_ > 0)
           ? (0.5 * w_soft_constraint_ * (*epsilon_sol_).transpose() *
              (*epsilon_sol_))(0)
           : 0;
+  VectorXd y_input_reg_cost = VectorXd::Zero(1);
+  VectorXd y_lambda_c_cost = VectorXd::Zero(1);
+  VectorXd y_lambda_h_cost = VectorXd::Zero(1);
+  VectorXd y_vdot_reg_cost = VectorXd::Zero(1);
+  VectorXd y_front_contact_force_reg_cost = VectorXd::Zero(1);
+  if (input_reg_cost_) {
+    input_reg_cost_->Eval(*u_sol_, &y_input_reg_cost);
+  }
+  if (lambda_c_cost_) {
+    lambda_c_cost_->Eval(*lambda_c_sol_, &y_lambda_c_cost);
+  }
+  if (lambda_h_cost_) {
+    lambda_h_cost_->Eval(*lambda_h_sol_, &y_lambda_h_cost);
+  }
+  if (vdot_reg_cost_) {
+    vdot_reg_cost_->Eval(*dv_sol_, &y_vdot_reg_cost);
+  }
+  if (front_contact_force_reg_cost_) {
+    VectorXd input(2);
+    input << lambda_c_sol_->segment<1>(2), lambda_c_sol_->segment<1>(8);
+    front_contact_force_reg_cost_->Eval(input, &y_front_contact_force_reg_cost);
+  }
+
+  output->regularization_costs.clear();
+  output->regularization_cost_names.clear();
+
+  output->regularization_costs.push_back(input_cost);
+  output->regularization_cost_names.push_back("input_cost");
+  output->regularization_costs.push_back(acceleration_cost);
+  output->regularization_cost_names.push_back("acceleration_cost");
+  output->regularization_costs.push_back(soft_constraint_cost);
+  output->regularization_cost_names.push_back("soft_constraint_cost");
+  output->regularization_costs.push_back(y_input_reg_cost[0]);
+  output->regularization_cost_names.push_back("input_reg_cost");
+  output->regularization_costs.push_back(y_lambda_c_cost[0]);
+  output->regularization_cost_names.push_back("lambda_c_cost");
+  output->regularization_costs.push_back(y_lambda_h_cost[0]);
+  output->regularization_cost_names.push_back("lambda_h_cost");
+  output->regularization_costs.push_back(y_vdot_reg_cost[0]);
+  output->regularization_cost_names.push_back("vdot_reg_cost");
+  output->regularization_costs.push_back(y_front_contact_force_reg_cost[0]);
+  output->regularization_cost_names.push_back("front_contact_force_reg_cost");
 
   output->tracking_data_names.clear();
   output->tracking_data.clear();
-  output->tracking_cost.clear();
+  output->tracking_costs.clear();
 
   lcmt_osc_qp_output qp_output;
   qp_output.solve_time = solve_time_;
@@ -1153,7 +1207,7 @@ void OperationalSpaceControl::AssignOscLcmOutput(
   output->qp_output = qp_output;
 
   output->tracking_data.reserve(tracking_data_vec_->size());
-  output->tracking_cost.reserve(tracking_data_vec_->size());
+  output->tracking_costs.reserve(tracking_data_vec_->size());
 
   for (unsigned int i = 0; i < tracking_data_vec_->size(); i++) {
     auto tracking_data = tracking_data_vec_->at(i);
@@ -1188,13 +1242,14 @@ void OperationalSpaceControl::AssignOscLcmOutput(
       const MatrixXd& W = tracking_data->GetWeight();
       const MatrixXd& J_t = tracking_data->GetJ();
       const VectorXd& JdotV_t = tracking_data->GetJdotTimesV();
-      output->tracking_cost.push_back(
+      output->tracking_costs.push_back(
           (0.5 * (J_t * (*dv_sol_) + JdotV_t - ddy_t).transpose() * W *
            (J_t * (*dv_sol_) + JdotV_t - ddy_t))(0));
     }
   }
 
   output->num_tracking_data = output->tracking_data_names.size();
+  output->num_regularization_costs = output->regularization_cost_names.size();
 }
 
 void OperationalSpaceControl::CalcOptimalInput(

systems/controllers/osc/operational_space_control.h

@@ -399,7 +399,7 @@ class OperationalSpaceControl : public drake::systems::LeafSystem<double> {
   drake::solvers::VectorXDecisionVariable epsilon_blend_;
 
   // Optional feature -- regularizing input
-  drake::solvers::QuadraticCost* input_reg_cost_;
+  drake::solvers::QuadraticCost* input_reg_cost_ = nullptr;
   double w_input_reg_ = -1;
   Eigen::MatrixXd W_input_reg_;
 
@@ -415,7 +415,7 @@ class OperationalSpaceControl : public drake::systems::LeafSystem<double> {
   // (penalize the front contact for optimal model as a regularization term, in
   //  order to help the solver to find good solution.)
   double w_rom_force_reg_ = -1;
-  drake::solvers::QuadraticCost* contact_force_reg_cost_;
+  drake::solvers::QuadraticCost* front_contact_force_reg_cost_ = nullptr;
 };
 
 }  // namespace dairlib::systems::controllers