From 825d7c6b59ab039ac8dcc2d540c7012f87d1692c Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Wed, 27 Oct 2021 18:33:47 -0400
Subject: [PATCH 01/10] WIP: chicken head plan

---
 src/plans/CMakeLists.txt               |  3 ++-
 src/plans/chicken_head_plan.cc         | 13 +++++++++++++
 src/plans/chicken_head_plan.h          | 19 +++++++++++++++++++
 src/plans/iiwa_plan_factory.cc         |  3 +++
 src/plans/plan_base.h                  | 11 ++---------
 src/plans/task_space_trajectory_plan.h |  6 +++---
 6 files changed, 42 insertions(+), 13 deletions(-)
 create mode 100644 src/plans/chicken_head_plan.cc
 create mode 100644 src/plans/chicken_head_plan.h

diff --git a/src/plans/CMakeLists.txt b/src/plans/CMakeLists.txt
index a5a711c..e770f01 100644
--- a/src/plans/CMakeLists.txt
+++ b/src/plans/CMakeLists.txt
@@ -1,7 +1,8 @@
 add_library(robot_plans
         plan_base.h plan_base.cc
         joint_space_trajectory_plan.h joint_space_trajectory_plan.cc
-        task_space_trajectory_plan.h task_space_trajectory_plan.cc)
+        task_space_trajectory_plan.h task_space_trajectory_plan.cc
+        chicken_head_plan.h chicken_head_plan.cc)
 target_link_libraries(robot_plans drake::drake)
 target_include_directories(robot_plans PUBLIC ${PROJECT_SOURCE_DIR}/src)
 
diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
new file mode 100644
index 0000000..7a83e6a
--- /dev/null
+++ b/src/plans/chicken_head_plan.cc
@@ -0,0 +1,13 @@
+#include "plans/chicken_head_plan.h"
+
+#include <utility>
+
+ChickenHeadPlan::ChickenHeadPlan(
+    drake::math::RigidTransformd X_WT0,
+    const drake::multibody::MultibodyPlant<double> *plant)
+    : PlanBase(plant), X_WT0_(std::move(X_WT0)),
+      frame_L7_(plant->GetFrameByName("iiwa_link_7")) {
+
+
+
+}
diff --git a/src/plans/chicken_head_plan.h b/src/plans/chicken_head_plan.h
new file mode 100644
index 0000000..a6afb62
--- /dev/null
+++ b/src/plans/chicken_head_plan.h
@@ -0,0 +1,19 @@
+#include "plans/plan_base.h"
+#include "drake/lcm/drake_lcm.h"
+
+#include <Eigen/Dense>
+
+class ChickenHeadPlan : public PlanBase {
+ public:
+  ChickenHeadPlan(drake::math::RigidTransformd  X_WT0,
+                  const drake::multibody::MultibodyPlant<double> *plant);
+
+ private:
+  const drake::math::RigidTransformd X_WT0_;
+  const drake::multibody::Frame<double>& frame_L7_;
+  // Relative transform between the frame L7 and the frame tool frame T.
+  const drake::math::RigidTransformd X_L7T_ = drake::math::RigidTransformd(
+      drake::math::RollPitchYawd(Eigen::Vector3d(0, -M_PI / 2, 0)),
+      Eigen::Vector3d(0, 0, 0.255));
+  std::unique_ptr<drake::lcm::DrakeLcm> lcm_;
+};
\ No newline at end of file
diff --git a/src/plans/iiwa_plan_factory.cc b/src/plans/iiwa_plan_factory.cc
index b7059d0..4b46864 100644
--- a/src/plans/iiwa_plan_factory.cc
+++ b/src/plans/iiwa_plan_factory.cc
@@ -17,6 +17,9 @@ using std::endl;
 using std::vector;
 
 IiwaPlanFactory::IiwaPlanFactory(const YAML::Node &config) : config_(config) {
+  // Constructs a MultibodyPlant contianing on the IIWA robot with its link 0
+  // welded to the world frame. This is the plant to be used by all Plans
+  // constructed by this factory, if the plan needs a plant.
   plant_ = std::make_unique<drake::multibody::MultibodyPlant<double>>(1e-3);
   auto parser = drake::multibody::Parser(plant_.get());
 
diff --git a/src/plans/plan_base.h b/src/plans/plan_base.h
index 15c072b..59cc37d 100644
--- a/src/plans/plan_base.h
+++ b/src/plans/plan_base.h
@@ -6,14 +6,6 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include <Eigen/Dense>
 
-// Description of a single contact.
-struct ContactInfo {
-  Eigen::Vector3d contact_force;                 // in world frame.
-  Eigen::Vector3d contact_point;                 // in world frame.
-  drake::multibody::BodyIndex contact_link_idx;  // tied to a specific MBP.
-  std::optional<Eigen::Vector3d> contact_normal; // in world frame?
-};
-
 struct State {
   State() = default;
   State(const Eigen::Ref<const Eigen::VectorXd> &q,
@@ -23,7 +15,6 @@ struct State {
   Eigen::VectorXd q;
   Eigen::VectorXd v;
   Eigen::VectorXd tau_ext;
-  std::optional<std::vector<ContactInfo>> contact_results;
 };
 
 struct Command {
@@ -48,5 +39,7 @@ class PlanBase {
   [[nodiscard]] virtual double duration() const = 0;
 
 protected:
+  // If a child Plan needs an MBP of the robot, it needs to get it from the
+  // Factory which constructs the Plan.
   drake::multibody::MultibodyPlant<double> const *const plant_{nullptr};
 };
diff --git a/src/plans/task_space_trajectory_plan.h b/src/plans/task_space_trajectory_plan.h
index b7e2520..f915cfa 100644
--- a/src/plans/task_space_trajectory_plan.h
+++ b/src/plans/task_space_trajectory_plan.h
@@ -20,10 +20,11 @@ class TaskSpaceTrajectoryPlan : public PlanBase {
       drake::math::RigidTransformd X_ET,
       const drake::multibody::MultibodyPlant<double> *plant,
       const drake::multibody::Frame<double> &frame_E,
-      double control_time_step, Eigen::VectorXd nominal_joint_position)
+      double control_time_step,
+      Eigen::VectorXd nominal_joint_position)
       : PlanBase(plant), quat_traj_(std::move(quat_traj)),
         X_ET_(std::move(X_ET)), xyz_traj_(std::move(xyz_traj)),
-        frame_E_(frame_E), nominal_joint_position_(nominal_joint_position) {
+        frame_E_(frame_E) {
 
     params_ = std::make_unique<
         drake::manipulation::planner::DifferentialInverseKinematicsParameters>(
@@ -46,7 +47,6 @@ class TaskSpaceTrajectoryPlan : public PlanBase {
   const drake::trajectories::PiecewiseQuaternionSlerp<double> quat_traj_;
   const drake::trajectories::PiecewisePolynomial<double> xyz_traj_;
   const drake::math::RigidTransformd X_ET_;
-  const Eigen::VectorXd nominal_joint_position_;
 
   // frame of end-effector body + offset.
   const drake::multibody::Frame<double> &frame_E_;

From 051a23d45a507a61e6724966aebc16e8b399d2f3 Mon Sep 17 00:00:00 2001
From: Unknown <pangtao@mit.edu>
Date: Thu, 28 Oct 2021 00:07:13 -0400
Subject: [PATCH 02/10] WIP.

---
 src/plans/chicken_head_plan.cc          | 39 +++++++++++++++++---
 src/plans/chicken_head_plan.h           | 37 ++++++++++++++++---
 src/plans/task_space_trajectory_plan.cc | 48 ++++++++++++++++---------
 src/plans/task_space_trajectory_plan.h  | 16 ++-------
 4 files changed, 101 insertions(+), 39 deletions(-)

diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
index 7a83e6a..3e5a89c 100644
--- a/src/plans/chicken_head_plan.cc
+++ b/src/plans/chicken_head_plan.cc
@@ -1,13 +1,44 @@
 #include "plans/chicken_head_plan.h"
 
-#include <utility>
+
 
 ChickenHeadPlan::ChickenHeadPlan(
-    drake::math::RigidTransformd X_WT0,
+    const drake::math::RigidTransformd& X_WL7_0,
     const drake::multibody::MultibodyPlant<double> *plant)
-    : PlanBase(plant), X_WT0_(std::move(X_WT0)),
+    : PlanBase(plant), X_WCd_(X_WL7_0 * X_L7T_),
       frame_L7_(plant->GetFrameByName("iiwa_link_7")) {
+  // X_TC subscription.
+  owned_lcm_ = std::make_unique<drake::lcm::DrakeLcm>();
+  rpe_sub_ = std::make_unique<drake::lcm::Subscriber<drake::lcmt_robot_state>>(
+      owned_lcm_.get(), "X_TC");
+  sub_thread_ = std::thread(&ChickenHeadPlan::ReceiveRelativePose, this);
+}
 
+ChickenHeadPlan::~ChickenHeadPlan() {
+  if (sub_thread_.joinable()) {
+    sub_thread_.join();
+  }
+}
 
-
+[[noreturn]] void ChickenHeadPlan::ReceiveRelativePose() const {
+  while (true) {
+    rpe_sub_->clear();
+    drake::lcm::LcmHandleSubscriptionsUntil(
+        owned_lcm_.get(), [&]() { return rpe_sub_->count() > 0; });
+    const auto& X_TC_msg = rpe_sub_->message();
+    const auto& q_xyz = X_TC_msg.joint_position;
+    const auto Q_TC = Eigen::Quaterniond(q_xyz[0], q_xyz[1], q_xyz[2],
+                                         q_xyz[3]);
+    const auto p_TC = Eigen::Vector3d(q_xyz[4], q_xyz[5], q_xyz[6]);
+    {
+      std::lock_guard<std::mutex> lock(mutex_rpe_);
+      X_TC_.set_rotation(Q_TC);
+      X_TC_.set_translation(p_TC);
+    }
+  }
 }
+
+void ChickenHeadPlan::Step(const State &state, double control_period, double t,
+          Command *cmd) const {
+
+}
\ No newline at end of file
diff --git a/src/plans/chicken_head_plan.h b/src/plans/chicken_head_plan.h
index a6afb62..c43865d 100644
--- a/src/plans/chicken_head_plan.h
+++ b/src/plans/chicken_head_plan.h
@@ -1,19 +1,46 @@
+#include <thread>
+
 #include "plans/plan_base.h"
 #include "drake/lcm/drake_lcm.h"
-
+#include "drake/lcmt_robot_state.hpp"
 #include <Eigen/Dense>
 
+/*
+ * Frame hierarchy:
+ * L7 --> T --> C
+ * L7 is the frame of link 7 of the IIWA.
+ * T is fixed relative to L7, called the tool frame.
+ * C is fixed to the tool handle, called the compliant frame.
+ * X_L7T is fixed and hard coded.
+ * X_WL7_0 is the pose of frame L7 at the beginning of the plan.
+ * Frame Cd (Compliant-desired) is initialized to be identical to T when the
+ *  plan is constructed: X_WT_0 == X_WCd
+ * The plan subscribes to Relative Pose Estimator and receives X_TC.
+ * The goal is to have T track Td (Tool_desired), so that
+ *  X_WTd * X_TC = X_WCd.
+ */
 class ChickenHeadPlan : public PlanBase {
  public:
-  ChickenHeadPlan(drake::math::RigidTransformd  X_WT0,
+  ChickenHeadPlan(const drake::math::RigidTransformd& X_WL7_0,
                   const drake::multibody::MultibodyPlant<double> *plant);
+  ~ChickenHeadPlan() override;
 
+  void Step(const State &state, double control_period, double t,
+            Command *cmd) const override;
  private:
-  const drake::math::RigidTransformd X_WT0_;
-  const drake::multibody::Frame<double>& frame_L7_;
+  [[noreturn]] void ReceiveRelativePose() const;
+
   // Relative transform between the frame L7 and the frame tool frame T.
   const drake::math::RigidTransformd X_L7T_ = drake::math::RigidTransformd(
       drake::math::RollPitchYawd(Eigen::Vector3d(0, -M_PI / 2, 0)),
       Eigen::Vector3d(0, 0, 0.255));
-  std::unique_ptr<drake::lcm::DrakeLcm> lcm_;
+  const drake::math::RigidTransformd X_WCd_;
+  const drake::multibody::Frame<double>& frame_L7_;
+
+  // Relative pose estimator subscription.
+  std::unique_ptr<drake::lcm::DrakeLcm> owned_lcm_;
+  std::unique_ptr<drake::lcm::Subscriber<drake::lcmt_robot_state>> rpe_sub_;
+  std::thread sub_thread_;
+  mutable std::mutex mutex_rpe_;
+  mutable drake::math::RigidTransformd X_TC_;
 };
\ No newline at end of file
diff --git a/src/plans/task_space_trajectory_plan.cc b/src/plans/task_space_trajectory_plan.cc
index 0251929..5db1e36 100644
--- a/src/plans/task_space_trajectory_plan.cc
+++ b/src/plans/task_space_trajectory_plan.cc
@@ -1,16 +1,34 @@
 #include "task_space_trajectory_plan.h"
 
+using drake::MatrixX;
+using drake::Vector3;
+using drake::Vector6;
+using drake::manipulation::planner::ComputePoseDiffInCommonFrame;
 using drake::manipulation::planner::DifferentialInverseKinematicsResult;
 using drake::manipulation::planner::DifferentialInverseKinematicsStatus;
 using drake::manipulation::planner::internal::DoDifferentialInverseKinematics;
-using drake::manipulation::planner::ComputePoseDiffInCommonFrame;
-using drake::Vector3;
-using drake::Vector6;
-using drake::MatrixX;
 
 using std::cout;
 using std::endl;
 
+TaskSpaceTrajectoryPlan::TaskSpaceTrajectoryPlan(
+    drake::trajectories::PiecewiseQuaternionSlerp<double> quat_traj,
+    drake::trajectories::PiecewisePolynomial<double> xyz_traj,
+    drake::math::RigidTransformd X_ET,
+    const drake::multibody::MultibodyPlant<double> *plant,
+    const drake::multibody::Frame<double> &frame_E, double control_time_step,
+    Eigen::VectorXd nominal_joint_position)
+    : PlanBase(plant), quat_traj_(std::move(quat_traj)), X_ET_(std::move(X_ET)),
+      xyz_traj_(std::move(xyz_traj)), frame_E_(frame_E) {
+
+  params_ = std::make_unique<
+      drake::manipulation::planner::DifferentialInverseKinematicsParameters>(
+      plant_->num_positions(), plant_->num_velocities());
+  params_->set_timestep(control_time_step);
+  params_->set_nominal_joint_position(nominal_joint_position);
+  plant_context_ = plant_->CreateDefaultContext();
+}
+
 void TaskSpaceTrajectoryPlan::Step(const State &state, double control_period,
                                    double t, Command *cmd) const {
 
@@ -19,23 +37,21 @@ void TaskSpaceTrajectoryPlan::Step(const State &state, double control_period,
 
   // 2. Ask diffik to solve for desired position.
   const drake::math::RigidTransformd X_WT_desired(quat_traj_.orientation(t),
-                                            xyz_traj_.value(t));
-  const auto& frame_W = plant_->world_frame();
-  const auto X_WE = plant_->CalcRelativeTransform(
-      *plant_context_, frame_W, frame_E_);
+                                                  xyz_traj_.value(t));
+  const auto &frame_W = plant_->world_frame();
+  const auto X_WE =
+      plant_->CalcRelativeTransform(*plant_context_, frame_W, frame_E_);
   const auto X_WT = X_WE * X_ET_;
 
   const Vector6<double> V_WT_desired =
-      ComputePoseDiffInCommonFrame(
-          X_WT.GetAsIsometry3(), X_WT_desired.GetAsIsometry3()) /
-          params_->get_timestep();
+      ComputePoseDiffInCommonFrame(X_WT.GetAsIsometry3(),
+                                   X_WT_desired.GetAsIsometry3()) /
+      params_->get_timestep();
 
   MatrixX<double> J_WT(6, plant_->num_velocities());
-  plant_->CalcJacobianSpatialVelocity(*plant_context_,
-                                    drake::multibody::JacobianWrtVariable::kV,
-                                    frame_E_, X_ET_.translation(),
-                                    frame_W, frame_W, &J_WT);
-
+  plant_->CalcJacobianSpatialVelocity(
+      *plant_context_, drake::multibody::JacobianWrtVariable::kV, frame_E_,
+      X_ET_.translation(), frame_W, frame_W, &J_WT);
 
   DifferentialInverseKinematicsResult result = DoDifferentialInverseKinematics(
       state.q, state.v, X_WT, J_WT,
diff --git a/src/plans/task_space_trajectory_plan.h b/src/plans/task_space_trajectory_plan.h
index f915cfa..eec2699 100644
--- a/src/plans/task_space_trajectory_plan.h
+++ b/src/plans/task_space_trajectory_plan.h
@@ -19,20 +19,8 @@ class TaskSpaceTrajectoryPlan : public PlanBase {
       drake::trajectories::PiecewisePolynomial<double> xyz_traj,
       drake::math::RigidTransformd X_ET,
       const drake::multibody::MultibodyPlant<double> *plant,
-      const drake::multibody::Frame<double> &frame_E,
-      double control_time_step,
-      Eigen::VectorXd nominal_joint_position)
-      : PlanBase(plant), quat_traj_(std::move(quat_traj)),
-        X_ET_(std::move(X_ET)), xyz_traj_(std::move(xyz_traj)),
-        frame_E_(frame_E) {
-
-    params_ = std::make_unique<
-        drake::manipulation::planner::DifferentialInverseKinematicsParameters>(
-        plant_->num_positions(), plant_->num_velocities());
-    params_->set_timestep(control_time_step);
-    params_->set_nominal_joint_position(nominal_joint_position);
-    plant_context_ = plant_->CreateDefaultContext();
-  }
+      const drake::multibody::Frame<double> &frame_E, double control_time_step,
+      Eigen::VectorXd nominal_joint_position);
 
   ~TaskSpaceTrajectoryPlan() override = default;
 

From d257e4dd9b244a8d5a58e67a1c039b3cb57db762 Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Thu, 28 Oct 2021 18:59:33 -0400
Subject: [PATCH 03/10] Chicken head plan version 1 (no low-pass filtering)
 works.

---
 src/examples/run_chicken_head_plan.py   | 33 +++++++++++
 src/examples/simple_test.py             | 13 ++---
 src/examples/zmq_client_example.py      |  4 +-
 src/plan_runner_client/zmq_client.py    | 52 ++++++++++++-----
 src/plans/chicken_head_plan.cc          | 78 ++++++++++++++++++++-----
 src/plans/chicken_head_plan.h           | 18 ++++--
 src/plans/iiwa_plan_factory.cc          | 48 ++++++++++-----
 src/plans/iiwa_plan_factory.h           | 16 +++++
 src/plans/task_space_trajectory_plan.cc | 16 +++--
 src/plans/task_space_trajectory_plan.h  |  1 -
 10 files changed, 211 insertions(+), 68 deletions(-)
 create mode 100644 src/examples/run_chicken_head_plan.py

diff --git a/src/examples/run_chicken_head_plan.py b/src/examples/run_chicken_head_plan.py
new file mode 100644
index 0000000..0ca8940
--- /dev/null
+++ b/src/examples/run_chicken_head_plan.py
@@ -0,0 +1,33 @@
+import time
+
+import numpy as np
+from plan_runner_client.zmq_client import PlanManagerZmqClient
+from drake import lcmt_robot_plan, lcmt_robot_state
+
+zmq_client = PlanManagerZmqClient()
+
+frame_L7 = zmq_client.plant.GetFrameByName('iiwa_link_7')
+n_q = 7
+X_WL7 = zmq_client.get_ee_pose_commanded(frame_L7)
+
+#%%
+joint_names = ["qw_chicken", "qx_chicken", "qy_chicken", "qz_chicken", "px_chicken",
+               "py_chicken", "pz_chicken"]
+t_knots = [0, 5.]
+
+msg_plan = lcmt_robot_plan()
+msg_plan.utime = round(time.time() * 1000)
+msg_plan.num_states = 2
+
+for i in range(len(t_knots)):
+    msg_state = lcmt_robot_state()
+    msg_state.utime = int(1e6 * t_knots[i])
+    msg_state.num_joints = n_q
+    msg_state.joint_name = joint_names
+    msg_state.joint_position = np.hstack(
+        [X_WL7.rotation().ToQuaternion().wxyz(), X_WL7.translation()])
+    msg_plan.plan.append(msg_state)
+
+#%%
+zmq_client.send_plan(msg_plan)
+zmq_client.wait_for_plan_to_finish()
diff --git a/src/examples/simple_test.py b/src/examples/simple_test.py
index dce2730..a3b11a5 100644
--- a/src/examples/simple_test.py
+++ b/src/examples/simple_test.py
@@ -28,8 +28,8 @@ def normalized(x):
 
 # Step 1. Check that the correct Cartesian position is valid and kinematics
 # are working correctly.
-q_now = zmq_client.get_current_joint_angles()
-X_WE = zmq_client.get_current_ee_pose(frame_E)
+q_now = zmq_client.get_joint_angles_commanded()
+X_WE = zmq_client.get_ee_pose_commanded(frame_E)
 print("Current joint angles:")
 print(q_now)
 print("Current end-effector position:")
@@ -52,7 +52,7 @@ def normalized(x):
 t_knots = np.linspace(0, duration, 22)
 q_knots = np.zeros((22, 7))
 
-q_knots[0, :] = zmq_client.get_current_joint_angles()
+q_knots[0, :] = zmq_client.get_joint_angles_commanded()
 for i in range(7):
     q_knots[3 * i + 1, :] = q_knots[3 * i, :]
     q_knots[3 * i + 1, i] += 0.2
@@ -63,7 +63,6 @@ def normalized(x):
 
 plan_msg1 = calc_joint_space_plan_msg(t_knots, q_knots)
 zmq_client.send_plan(plan_msg1)
-time.sleep(1.0)
 zmq_client.wait_for_plan_to_finish()
 time.sleep(2.0)
 
@@ -72,12 +71,11 @@ def normalized(x):
 t_knots = np.linspace(0, duration, 2)
 q_knots = np.zeros((2, 7))
 
-q_knots[0, :] = zmq_client.get_current_joint_angles()
+q_knots[0, :] = zmq_client.get_joint_angles_commanded()
 q_knots[1, :] = [0, 0.3, 0.0, -1.75, 0.0, 1.0, 0.0]
 
 plan_msg2 = calc_joint_space_plan_msg(t_knots, q_knots)
 zmq_client.send_plan(plan_msg2)
-time.sleep(1.0)
 zmq_client.wait_for_plan_to_finish()
 
 time.sleep(2.0)
@@ -87,7 +85,7 @@ def normalized(x):
 t_knots = np.linspace(0, duration, 2)
 
 X_WT_lst = []
-X_WT_lst.append(zmq_client.get_current_ee_pose(frame_E))
+X_WT_lst.append(zmq_client.get_ee_pose_commanded(frame_E))
 X_WT_lst.append(RigidTransform(RollPitchYaw([0, -np.pi, 0]),
                                np.array([0.5, 0.0, 0.5])))
 
@@ -130,5 +128,4 @@ def normalized(x):
 
 plan_msg3 = calc_task_space_plan_msg(RigidTransform(), X_WT_lst, t_knots)
 zmq_client.send_plan(plan_msg3)
-time.sleep(1.0)
 zmq_client.wait_for_plan_to_finish()
diff --git a/src/examples/zmq_client_example.py b/src/examples/zmq_client_example.py
index c6a037f..2cd9f6d 100644
--- a/src/examples/zmq_client_example.py
+++ b/src/examples/zmq_client_example.py
@@ -13,7 +13,7 @@
 zmq_client = PlanManagerZmqClient()
 
 t_knots = np.array([0, 10])
-q0 = zmq_client.get_current_joint_angles()
+q0 = zmq_client.get_joint_angles_measured()
 
 if len(q0) == 0:
     raise RuntimeError("No messages were detected in IIWA_STATUS. " +
@@ -53,7 +53,7 @@ def run_task_space_plan():
     frame_E = zmq_client.plant.GetFrameByName('iiwa_link_7')
     X_ET = RigidTransform()
     X_ET.set_translation([0.1, 0, 0])
-    X_WE0 = zmq_client.get_current_ee_pose(frame_E)
+    X_WE0 = zmq_client.get_ee_pose_commanded(frame_E)
     X_WT0 = X_WE0.multiply(X_ET)
     X_WT1 = RigidTransform(X_WT0.rotation(),
                            X_WT0.translation() + np.array([0, 0.2, 0]))
diff --git a/src/plan_runner_client/zmq_client.py b/src/plan_runner_client/zmq_client.py
index 1b61f5b..71e3131 100644
--- a/src/plan_runner_client/zmq_client.py
+++ b/src/plan_runner_client/zmq_client.py
@@ -4,6 +4,7 @@
 import copy
 import logging
 
+import pydrake.multibody.tree
 import zmq
 import lcm
 
@@ -41,9 +42,9 @@ def __init__(self):
         self.lc = lcm.LCM()
         sub = self.lc.subscribe("IIWA_STATUS", self.sub_callback)
         sub.set_queue_capacity(1)
-        self.iiwa_position_measured = None
+        self.iiwa_status_msg = None
         self.msg_lock = threading.Lock()
-        self.t1 = threading.Thread(target=self.update_iiwa_position_measured)
+        self.t1 = threading.Thread(target=self.update_iiwa_status)
         self.t1.start()
 
         # TODO: Set up logger (should really do it elsewhere...)
@@ -59,7 +60,7 @@ def __init__(self):
         self.logger.info("Waiting for iiwa Status...")
         while True:
             self.msg_lock.acquire()
-            if self.iiwa_position_measured is not None:
+            if self.iiwa_status_msg is not None:
                 self.msg_lock.release()
                 break
 
@@ -70,20 +71,28 @@ def __init__(self):
     def sub_callback(self, channel, data):
         iiwa_status_msg = lcmt_iiwa_status.decode(data)
         self.msg_lock.acquire()
-        self.iiwa_position_measured = iiwa_status_msg.joint_position_measured
+        self.iiwa_status_msg = iiwa_status_msg
         self.msg_lock.release()
 
-    def update_iiwa_position_measured(self):
+    def update_iiwa_status(self):
         while True:
             self.lc.handle()
 
     def get_iiwa_position_measured(self):
-        iiwa_position_measured = np.array([])
+        q = np.array([])
         self.msg_lock.acquire()
-        if self.iiwa_position_measured:
-            iiwa_position_measured = np.array(self.iiwa_position_measured)
+        if self.iiwa_status_msg:
+            q = np.array(self.iiwa_status_msg.joint_position_measured)
         self.msg_lock.release()
-        return iiwa_position_measured
+        return q
+
+    def get_iiwa_position_commanded(self):
+        q = np.array([])
+        self.msg_lock.acquire()
+        if self.iiwa_status_msg:
+            q = np.array(self.iiwa_status_msg.joint_position_commanded)
+        self.msg_lock.release()
+        return q
 
 
 class PlanManagerZmqClient:
@@ -138,16 +147,27 @@ def get_plan_status(self):
         self.status_msg_lock.release()
         return status_msg
 
-    def get_current_ee_pose(self, frame_E):
-        context = self.plant.CreateDefaultContext()
+    def get_ee_pose_measured(self, frame_E: pydrake.multibody.tree.Frame,):
         q = self.iiwa_position_getter.get_iiwa_position_measured()
+        return self.get_ee_pose_from_joint_angles(frame_E, q)
+
+    def get_ee_pose_commanded(self, frame_E: pydrake.multibody.tree.Frame, ):
+        q = self.iiwa_position_getter.get_iiwa_position_commanded()
+        return self.get_ee_pose_from_joint_angles(frame_E, q)
+
+    def get_ee_pose_from_joint_angles(self, frame_E: pydrake.multibody.tree.Frame,
+                                      q: np.ndarray):
+        context = self.plant.CreateDefaultContext()
         self.plant.SetPositions(context, q)
         return self.plant.CalcRelativeTransform(
             context, self.plant.world_frame(), frame_E)
 
-    def get_current_joint_angles(self):
+    def get_joint_angles_measured(self):
         return self.iiwa_position_getter.get_iiwa_position_measured()
 
+    def get_joint_angles_commanded(self):
+        return self.iiwa_position_getter.get_iiwa_position_commanded()
+
     def send_plan(self, plan_msg):
         self.plan_msg_lock.acquire()
         self.last_plan_msg = copy.deepcopy(plan_msg)
@@ -155,12 +175,15 @@ def send_plan(self, plan_msg):
         self.plan_client.send(plan_msg.encode())
         msg = self.plan_client.recv()
         assert msg == b'plan_received'
-        print("plan received by server.")
+        self.iiwa_position_getter.logger.info("plan received by server.")
 
     def wait_for_plan_to_finish(self):
         # TODO: add timeout.
         while True:
             status_msg = self.get_plan_status()
+            if status_msg is None:
+                time.sleep(0.01)
+                continue
             self.plan_msg_lock.acquire()
             is_same_plan = (
                 self.last_plan_msg.utime == status_msg.utime)
@@ -175,7 +198,8 @@ def wait_for_plan_to_finish(self):
             if is_same_plan and (is_plan_finished or is_plan_error):
                 break
             time.sleep(0.01)
-        print("Final status:", self.plan_stats_dict[status_msg.status])
+        self.iiwa_position_getter.logger.info(
+            "Final status: " + self.plan_stats_dict[status_msg.status])
 
     def abort(self):
         self.abort_client.send(b"abort")
diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
index 3e5a89c..a3e2c7a 100644
--- a/src/plans/chicken_head_plan.cc
+++ b/src/plans/chicken_head_plan.cc
@@ -1,12 +1,25 @@
 #include "plans/chicken_head_plan.h"
 
-
+using drake::Vector6;
+using drake::math::RigidTransformd;
+using drake::manipulation::planner::internal::DoDifferentialInverseKinematics;
+using drake::manipulation::planner::DifferentialInverseKinematicsStatus;
 
 ChickenHeadPlan::ChickenHeadPlan(
-    const drake::math::RigidTransformd& X_WL7_0,
+    const drake::math::RigidTransformd &X_WL7_0, double duration,
+    double control_time_step,
+    const Eigen::Ref<const Eigen::VectorXd> &nominal_joint_angles,
     const drake::multibody::MultibodyPlant<double> *plant)
-    : PlanBase(plant), X_WCd_(X_WL7_0 * X_L7T_),
+    : PlanBase(plant), duration_(duration), X_WCd_(X_WL7_0 * X_L7T_),
       frame_L7_(plant->GetFrameByName("iiwa_link_7")) {
+  // DiffIk.
+  params_ = std::make_unique<
+      drake::manipulation::planner::DifferentialInverseKinematicsParameters>(
+      plant_->num_positions(), plant_->num_velocities());
+  params_->set_timestep(control_time_step);
+  params_->set_nominal_joint_position(nominal_joint_angles);
+  plant_context_ = plant_->CreateDefaultContext();
+
   // X_TC subscription.
   owned_lcm_ = std::make_unique<drake::lcm::DrakeLcm>();
   rpe_sub_ = std::make_unique<drake::lcm::Subscriber<drake::lcmt_robot_state>>(
@@ -15,20 +28,24 @@ ChickenHeadPlan::ChickenHeadPlan(
 }
 
 ChickenHeadPlan::~ChickenHeadPlan() {
-  if (sub_thread_.joinable()) {
-    sub_thread_.join();
-  }
+  stop_flag_ = true;
+  sub_thread_.join();
 }
 
-[[noreturn]] void ChickenHeadPlan::ReceiveRelativePose() const {
+void ChickenHeadPlan::ReceiveRelativePose() const {
+  int i = 0;
   while (true) {
     rpe_sub_->clear();
     drake::lcm::LcmHandleSubscriptionsUntil(
-        owned_lcm_.get(), [&]() { return rpe_sub_->count() > 0; });
-    const auto& X_TC_msg = rpe_sub_->message();
-    const auto& q_xyz = X_TC_msg.joint_position;
-    const auto Q_TC = Eigen::Quaterniond(q_xyz[0], q_xyz[1], q_xyz[2],
-                                         q_xyz[3]);
+        owned_lcm_.get(),
+        [&]() { return stop_flag_ or rpe_sub_->count() > 0; });
+    if (stop_flag_) {
+      break;
+    }
+    const auto &X_TC_msg = rpe_sub_->message();
+    const auto &q_xyz = X_TC_msg.joint_position;
+    const auto Q_TC =
+        Eigen::Quaterniond(q_xyz[0], q_xyz[1], q_xyz[2], q_xyz[3]);
     const auto p_TC = Eigen::Vector3d(q_xyz[4], q_xyz[5], q_xyz[6]);
     {
       std::lock_guard<std::mutex> lock(mutex_rpe_);
@@ -39,6 +56,39 @@ ChickenHeadPlan::~ChickenHeadPlan() {
 }
 
 void ChickenHeadPlan::Step(const State &state, double control_period, double t,
-          Command *cmd) const {
+                           Command *cmd) const {
+  RigidTransformd X_WTd;
+  {
+    std::lock_guard<std::mutex> lock(mutex_rpe_);
+    X_WTd = X_WCd_ * X_TC_.inverse();
+  }
 
-}
\ No newline at end of file
+  // DiffIk.
+  plant_->SetPositions(plant_context_.get(), state.q);
+  const auto &frame_W = plant_->world_frame();
+  const auto X_WT =
+      plant_->CalcRelativeTransform(*plant_context_, frame_W, frame_L7_) *
+      X_L7T_;
+  const Vector6<double> V_WT_desired =
+      drake::manipulation::planner::ComputePoseDiffInCommonFrame(
+          X_WT.GetAsIsometry3(), X_WTd.GetAsIsometry3()) /
+      params_->get_timestep();
+
+  Eigen::MatrixXd J_WT(6, plant_->num_velocities());
+  plant_->CalcJacobianSpatialVelocity(
+      *plant_context_, drake::multibody::JacobianWrtVariable::kV, frame_L7_,
+      X_L7T_.translation(), frame_W, frame_W, &J_WT);
+
+  const auto result = DoDifferentialInverseKinematics(
+      state.q, state.v, X_WT, J_WT,
+      drake::multibody::SpatialVelocity<double>(V_WT_desired), *params_);
+
+  // 3. Check for errors and integrate.
+  if (result.status != DifferentialInverseKinematicsStatus::kSolutionFound) {
+    cmd->q_cmd = NAN * Eigen::VectorXd::Zero(7);
+    spdlog::critical("DoDifferentialKinematics Failed to find a solution.");
+  } else {
+    cmd->q_cmd = state.q + control_period * result.joint_velocities.value();
+    cmd->tau_cmd = Eigen::VectorXd::Zero(7);
+  }
+}
diff --git a/src/plans/chicken_head_plan.h b/src/plans/chicken_head_plan.h
index c43865d..0e20908 100644
--- a/src/plans/chicken_head_plan.h
+++ b/src/plans/chicken_head_plan.h
@@ -3,6 +3,7 @@
 #include "plans/plan_base.h"
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_robot_state.hpp"
+#include "drake/manipulation/planner/differential_inverse_kinematics.h"
 #include <Eigen/Dense>
 
 /*
@@ -21,26 +22,35 @@
  */
 class ChickenHeadPlan : public PlanBase {
  public:
-  ChickenHeadPlan(const drake::math::RigidTransformd& X_WL7_0,
+  ChickenHeadPlan(const drake::math::RigidTransformd &X_WL7_0,
+                  double duration,
+                  double control_time_step,
+                  const Eigen::Ref<const Eigen::VectorXd> &nominal_joint_angles,
                   const drake::multibody::MultibodyPlant<double> *plant);
   ~ChickenHeadPlan() override;
 
   void Step(const State &state, double control_period, double t,
             Command *cmd) const override;
+  double duration() const override {return duration_;};
  private:
-  [[noreturn]] void ReceiveRelativePose() const;
-
+  const double duration_{0};
   // Relative transform between the frame L7 and the frame tool frame T.
   const drake::math::RigidTransformd X_L7T_ = drake::math::RigidTransformd(
       drake::math::RollPitchYawd(Eigen::Vector3d(0, -M_PI / 2, 0)),
       Eigen::Vector3d(0, 0, 0.255));
   const drake::math::RigidTransformd X_WCd_;
   const drake::multibody::Frame<double>& frame_L7_;
+  std::unique_ptr<drake::systems::Context<double>> plant_context_;
+  std::unique_ptr<
+  drake::manipulation::planner::DifferentialInverseKinematicsParameters>
+  params_;
 
   // Relative pose estimator subscription.
+  void ReceiveRelativePose() const;
   std::unique_ptr<drake::lcm::DrakeLcm> owned_lcm_;
   std::unique_ptr<drake::lcm::Subscriber<drake::lcmt_robot_state>> rpe_sub_;
   std::thread sub_thread_;
+  std::atomic<bool> stop_flag_{false};
   mutable std::mutex mutex_rpe_;
   mutable drake::math::RigidTransformd X_TC_;
-};
\ No newline at end of file
+};
diff --git a/src/plans/iiwa_plan_factory.cc b/src/plans/iiwa_plan_factory.cc
index 4b46864..cc8f723 100644
--- a/src/plans/iiwa_plan_factory.cc
+++ b/src/plans/iiwa_plan_factory.cc
@@ -4,6 +4,7 @@
 #include "drake/common/trajectories/piecewise_quaternion.h"
 #include "drake/multibody/parsing/parser.h"
 
+#include "chicken_head_plan.h"
 #include "joint_space_trajectory_plan.h"
 #include "task_space_trajectory_plan.h"
 
@@ -36,10 +37,14 @@ std::unique_ptr<PlanBase>
 IiwaPlanFactory::MakePlan(const drake::lcmt_robot_plan &msg_plan) const {
   // TODO: replace this with a better test and use an lcm type with an
   //  enum for plan types?
-  if (msg_plan.plan.at(0).joint_name.at(0) == "iiwa_joint_0") {
+  const auto first_joint_name =
+      std::string(msg_plan.plan.at(0).joint_name.at(0));
+  if (first_joint_name == "iiwa_joint_0") {
     return MakeJointSpaceTrajectoryPlan(msg_plan);
-  } else if (msg_plan.plan.at(0).joint_name.at(0) == "qw") {
+  } else if (first_joint_name == "qw") {
     return MakeTaskSpaceTrajectoryPlan(msg_plan);
+  } else if (first_joint_name == "qw_chicken") {
+    return MakeChickenHeadPlan(msg_plan);
   }
   throw std::runtime_error("error in plan lcm message.");
 }
@@ -52,7 +57,7 @@ std::unique_ptr<PlanBase> IiwaPlanFactory::MakeJointSpaceTrajectoryPlan(
   VectorXd t_knots(n_knots);
 
   for (int t = 0; t < n_knots; t++) {
-    t_knots[t] = static_cast<double>(msg_plan.plan.at(t).utime) / 1e6;
+    t_knots[t] = msg_plan.plan.at(t).utime / 1e6;
     for (int i = 0; i < n_q; i++) {
       q_knots(i, t) = msg_plan.plan.at(t).joint_position.at(i);
     }
@@ -70,11 +75,8 @@ std::unique_ptr<PlanBase> IiwaPlanFactory::MakeTaskSpaceTrajectoryPlan(
   int n_knots = msg_plan.num_states - 1;
 
   // X_ET.
-  const auto& q_xyz_ET = msg_plan.plan[n_knots].joint_position;
-  const auto Q_ET = Quaterniond(q_xyz_ET[0], q_xyz_ET[1], q_xyz_ET[2],
-                                q_xyz_ET[3]);
-  const auto p_EoTo_E = Eigen::Vector3d(q_xyz_ET[4], q_xyz_ET[5], q_xyz_ET[6]);
-  const auto X_ET = drake::math::RigidTransform<double>(Q_ET, p_EoTo_E);
+  const auto &q_xyz_ET = msg_plan.plan[n_knots].joint_position;
+  const auto X_ET = TransformFromRobotStateMsg(q_xyz_ET);
 
   // trajectory.
   vector<double> t_knots(n_knots);
@@ -83,9 +85,9 @@ std::unique_ptr<PlanBase> IiwaPlanFactory::MakeTaskSpaceTrajectoryPlan(
 
   for (int t = 0; t < n_knots; t++) {
     // Store time
-    t_knots[t] = static_cast<double>(msg_plan.plan.at(t).utime) / 1e6;
+    t_knots[t] = msg_plan.plan.at(t).utime / 1e6;
     // Store quaternions
-    const auto& q_xyz = msg_plan.plan.at(t).joint_position;
+    const auto &q_xyz = msg_plan.plan.at(t).joint_position;
     quat_knots[t] = Quaterniond(q_xyz[0], q_xyz[1], q_xyz[2], q_xyz[3]);
 
     // Store xyz positions.
@@ -108,14 +110,28 @@ std::unique_ptr<PlanBase> IiwaPlanFactory::MakeTaskSpaceTrajectoryPlan(
   const auto &frame_E =
       plant_->GetFrameByName(config_["robot_ee_body_name"].as<std::string>());
 
-  vector<double> nominal_joint_vector = 
-    config_["robot_nominal_joint"].as<vector<double>>();
-
-  Eigen::VectorXd nominal_joint= Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
-    nominal_joint_vector.data(), nominal_joint_vector.size());
-
+  // TODO(pang): it feels like the nominal joint angles should be part of the
+  //  plan definition, not in the config file.
+  const auto nominal_joint_vector =
+      config_["robot_nominal_joint"].as<vector<double>>();
+  auto nominal_joint = Eigen::Map<const Eigen::VectorXd>(
+      nominal_joint_vector.data(), nominal_joint_vector.size());
 
   return std::make_unique<TaskSpaceTrajectoryPlan>(
       std::move(quat_traj), std::move(xyz_traj), X_ET, plant_.get(), frame_E,
       config_["control_period"].as<double>(), nominal_joint);
 }
+
+[[nodiscard]] std::unique_ptr<PlanBase> IiwaPlanFactory::MakeChickenHeadPlan(
+    const drake::lcmt_robot_plan &msg_plan) const {
+  DRAKE_THROW_UNLESS(msg_plan.plan.size() == 2);
+  const double duration = msg_plan.plan.at(1).utime / 1e6;
+  const auto nominal_joint_vector =
+      config_["robot_nominal_joint"].as<vector<double>>();
+  auto nominal_joint = Eigen::Map<const Eigen::VectorXd>(
+      nominal_joint_vector.data(), nominal_joint_vector.size());
+  return std::make_unique<ChickenHeadPlan>(
+      TransformFromRobotStateMsg(msg_plan.plan[0].joint_position), duration,
+      config_["control_period"].as<double>(), nominal_joint,
+      plant_.get());
+}
diff --git a/src/plans/iiwa_plan_factory.h b/src/plans/iiwa_plan_factory.h
index ad35ad7..46e9de0 100644
--- a/src/plans/iiwa_plan_factory.h
+++ b/src/plans/iiwa_plan_factory.h
@@ -13,9 +13,25 @@ class IiwaPlanFactory : public PlanFactory {
   MakePlan(const drake::lcmt_robot_plan &msg_plan) const override;
 
 private:
+  // q_xyz.size() == 7. q_xyz[0:4] are the w, x, y and z components of the
+  // quaternion. q_xyz[4:7] are the x, y and z components of the translation.
+  template <class T>
+  static drake::math::RigidTransformd TransformFromRobotStateMsg(
+      const std::vector<T>& q_xyz);
   const YAML::Node &config_;
   [[nodiscard]] std::unique_ptr<PlanBase>
   MakeJointSpaceTrajectoryPlan(const drake::lcmt_robot_plan &msg_plan) const;
   [[nodiscard]] std::unique_ptr<PlanBase>
   MakeTaskSpaceTrajectoryPlan(const drake::lcmt_robot_plan &msg_plan) const;
+  [[nodiscard]] std::unique_ptr<PlanBase>
+  MakeChickenHeadPlan(const drake::lcmt_robot_plan &msg_plan) const;
 };
+
+template <class T>
+drake::math::RigidTransformd IiwaPlanFactory::TransformFromRobotStateMsg(
+    const std::vector<T>& q_xyz) {
+  DRAKE_THROW_UNLESS(q_xyz.size() == 7);
+  const auto Q = Eigen::Quaterniond(q_xyz[0], q_xyz[1], q_xyz[2], q_xyz[3]);
+  const auto p = Eigen::Vector3d(q_xyz[4], q_xyz[5], q_xyz[6]);
+  return {Q, p};
+}
diff --git a/src/plans/task_space_trajectory_plan.cc b/src/plans/task_space_trajectory_plan.cc
index 5db1e36..d3128bd 100644
--- a/src/plans/task_space_trajectory_plan.cc
+++ b/src/plans/task_space_trajectory_plan.cc
@@ -8,9 +8,6 @@ using drake::manipulation::planner::DifferentialInverseKinematicsResult;
 using drake::manipulation::planner::DifferentialInverseKinematicsStatus;
 using drake::manipulation::planner::internal::DoDifferentialInverseKinematics;
 
-using std::cout;
-using std::endl;
-
 TaskSpaceTrajectoryPlan::TaskSpaceTrajectoryPlan(
     drake::trajectories::PiecewiseQuaternionSlerp<double> quat_traj,
     drake::trajectories::PiecewisePolynomial<double> xyz_traj,
@@ -48,12 +45,12 @@ void TaskSpaceTrajectoryPlan::Step(const State &state, double control_period,
                                    X_WT_desired.GetAsIsometry3()) /
       params_->get_timestep();
 
-  MatrixX<double> J_WT(6, plant_->num_velocities());
+  Eigen::MatrixXd J_WT(6, plant_->num_velocities());
   plant_->CalcJacobianSpatialVelocity(
       *plant_context_, drake::multibody::JacobianWrtVariable::kV, frame_E_,
       X_ET_.translation(), frame_W, frame_W, &J_WT);
 
-  DifferentialInverseKinematicsResult result = DoDifferentialInverseKinematics(
+  const auto result = DoDifferentialInverseKinematics(
       state.q, state.v, X_WT, J_WT,
       drake::multibody::SpatialVelocity<double>(V_WT_desired), *params_);
 
@@ -63,10 +60,11 @@ void TaskSpaceTrajectoryPlan::Step(const State &state, double control_period,
     // go to error state.
     cmd->q_cmd = NAN * Eigen::VectorXd::Zero(7);
     // TODO(terry-suh): how do I tell the use that the state machine went to
-    // error because of this precise reason? Printing the error message here
-    // seems like a good start, but we'll need to handle this better.
-    std::cout << "DoDifferentialKinematics Failed to find a solution."
-              << std::endl;
+    //  error because of this precise reason? Printing the error message here
+    //  seems like a good start, but we'll need to handle this better.
+    //  (Pang): I think throwing exceptions here and have them handled at the
+    //   level of PlanManager is a good solution.
+    spdlog::critical("DoDifferentialKinematics Failed to find a solution.");
   } else {
     cmd->q_cmd = state.q + control_period * result.joint_velocities.value();
     cmd->tau_cmd = Eigen::VectorXd::Zero(7);
diff --git a/src/plans/task_space_trajectory_plan.h b/src/plans/task_space_trajectory_plan.h
index eec2699..7f6e067 100644
--- a/src/plans/task_space_trajectory_plan.h
+++ b/src/plans/task_space_trajectory_plan.h
@@ -38,7 +38,6 @@ class TaskSpaceTrajectoryPlan : public PlanBase {
 
   // frame of end-effector body + offset.
   const drake::multibody::Frame<double> &frame_E_;
-
   std::unique_ptr<drake::systems::Context<double>> plant_context_;
   std::unique_ptr<
       drake::manipulation::planner::DifferentialInverseKinematicsParameters>

From c096c455c27e3d428020561f46b3ab621492e838 Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Thu, 28 Oct 2021 22:40:39 -0400
Subject: [PATCH 04/10] Chicken head plan resists sinusoidal measurements.

---
 .../run_chicken_head_fake_msg_publisher.py    | 22 ++++++++
 src/examples/run_chicken_head_plan.py         |  6 ++-
 src/plan_runner_client/zmq_client.py          |  4 +-
 src/plans/chicken_head_plan.cc                | 50 ++++++++++++++++---
 src/plans/chicken_head_plan.h                 |  4 +-
 .../plan_manager_state_machine.cc             |  1 -
 6 files changed, 73 insertions(+), 14 deletions(-)
 create mode 100644 src/examples/run_chicken_head_fake_msg_publisher.py

diff --git a/src/examples/run_chicken_head_fake_msg_publisher.py b/src/examples/run_chicken_head_fake_msg_publisher.py
new file mode 100644
index 0000000..79ae02f
--- /dev/null
+++ b/src/examples/run_chicken_head_fake_msg_publisher.py
@@ -0,0 +1,22 @@
+import time
+import numpy as np
+
+from pydrake.all import DrakeLcm, RotationMatrix
+from pydrake.math import RollPitchYaw
+from drake import lcmt_robot_state
+
+channel_name = "X_TC"
+lcm = DrakeLcm()
+
+t0 = time.time()
+while True:
+    msg = lcmt_robot_state()
+    t = time.time()
+    msg.utime = int(t * 1e6)
+    msg.num_joints = 7
+    msg.joint_name = ["qw", "qx", "qy", "qz", "px", "py", "pz"]
+    R_TC = RollPitchYaw(0, 0, 0.2 * np.sin(t - t0)).ToRotationMatrix()
+    msg.joint_position = np.hstack([R_TC.ToQuaternion().wxyz(), np.zeros(3)])
+    lcm.Publish(channel_name, msg.encode())
+    time.sleep(0.01)
+
diff --git a/src/examples/run_chicken_head_plan.py b/src/examples/run_chicken_head_plan.py
index 0ca8940..5b09ca6 100644
--- a/src/examples/run_chicken_head_plan.py
+++ b/src/examples/run_chicken_head_plan.py
@@ -11,12 +11,14 @@
 X_WL7 = zmq_client.get_ee_pose_commanded(frame_L7)
 
 #%%
+# TODO: these joint names are getting ridiculous... we really need our own LCM types
+#  for plans.
 joint_names = ["qw_chicken", "qx_chicken", "qy_chicken", "qz_chicken", "px_chicken",
                "py_chicken", "pz_chicken"]
-t_knots = [0, 5.]
+t_knots = [0, 60.]
 
 msg_plan = lcmt_robot_plan()
-msg_plan.utime = round(time.time() * 1000)
+msg_plan.utime = round(time.time() * 1e6)
 msg_plan.num_states = 2
 
 for i in range(len(t_knots)):
diff --git a/src/plan_runner_client/zmq_client.py b/src/plan_runner_client/zmq_client.py
index 71e3131..7a25ab1 100644
--- a/src/plan_runner_client/zmq_client.py
+++ b/src/plan_runner_client/zmq_client.py
@@ -147,11 +147,11 @@ def get_plan_status(self):
         self.status_msg_lock.release()
         return status_msg
 
-    def get_ee_pose_measured(self, frame_E: pydrake.multibody.tree.Frame,):
+    def get_ee_pose_measured(self, frame_E: pydrake.multibody.tree.Frame):
         q = self.iiwa_position_getter.get_iiwa_position_measured()
         return self.get_ee_pose_from_joint_angles(frame_E, q)
 
-    def get_ee_pose_commanded(self, frame_E: pydrake.multibody.tree.Frame, ):
+    def get_ee_pose_commanded(self, frame_E: pydrake.multibody.tree.Frame):
         q = self.iiwa_position_getter.get_iiwa_position_commanded()
         return self.get_ee_pose_from_joint_angles(frame_E, q)
 
diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
index a3e2c7a..bf051b0 100644
--- a/src/plans/chicken_head_plan.cc
+++ b/src/plans/chicken_head_plan.cc
@@ -1,7 +1,9 @@
 #include "plans/chicken_head_plan.h"
+#include "drake/lcm/lcm_messages.h"
 
 using drake::Vector6;
 using drake::math::RigidTransformd;
+using drake::lcmt_robot_state;
 using drake::manipulation::planner::internal::DoDifferentialInverseKinematics;
 using drake::manipulation::planner::DifferentialInverseKinematicsStatus;
 
@@ -24,7 +26,7 @@ ChickenHeadPlan::ChickenHeadPlan(
   owned_lcm_ = std::make_unique<drake::lcm::DrakeLcm>();
   rpe_sub_ = std::make_unique<drake::lcm::Subscriber<drake::lcmt_robot_state>>(
       owned_lcm_.get(), "X_TC");
-  sub_thread_ = std::thread(&ChickenHeadPlan::ReceiveRelativePose, this);
+  sub_thread_ = std::thread(&ChickenHeadPlan::PoseIo, this);
 }
 
 ChickenHeadPlan::~ChickenHeadPlan() {
@@ -32,8 +34,26 @@ ChickenHeadPlan::~ChickenHeadPlan() {
   sub_thread_.join();
 }
 
-void ChickenHeadPlan::ReceiveRelativePose() const {
-  int i = 0;
+lcmt_robot_state GetStateMsgFromTransform(
+    const RigidTransformd& X_WC, int64_t utime) {
+  const auto Q_WC = X_WC.rotation().ToQuaternion();
+  const auto& p_WC = X_WC.translation();
+  lcmt_robot_state msg_X_WC{};
+  msg_X_WC.utime = utime;
+  msg_X_WC.num_joints = 7;
+  msg_X_WC.joint_name = {"qw", "qx", "qy", "qz", "x", "y", "z"};
+  msg_X_WC.joint_position = std::vector<float>(
+      {static_cast<float>(Q_WC.w()),
+       static_cast<float>(Q_WC.x()),
+       static_cast<float>(Q_WC.y()),
+       static_cast<float>(Q_WC.z()),
+       static_cast<float>(p_WC[0]),
+       static_cast<float>(p_WC[1]),
+       static_cast<float>(p_WC[2])});
+  return msg_X_WC;
+}
+
+void ChickenHeadPlan::PoseIo() const {
   while (true) {
     rpe_sub_->clear();
     drake::lcm::LcmHandleSubscriptionsUntil(
@@ -47,28 +67,40 @@ void ChickenHeadPlan::ReceiveRelativePose() const {
     const auto Q_TC =
         Eigen::Quaterniond(q_xyz[0], q_xyz[1], q_xyz[2], q_xyz[3]);
     const auto p_TC = Eigen::Vector3d(q_xyz[4], q_xyz[5], q_xyz[6]);
+
+    drake::math::RigidTransformd X_WC;
     {
       std::lock_guard<std::mutex> lock(mutex_rpe_);
       X_TC_.set_rotation(Q_TC);
       X_TC_.set_translation(p_TC);
+      X_WC = X_WT_ * X_TC_;
     }
+
+    // Publish X_WC.
+    drake::lcm::Publish(
+        owned_lcm_.get(), "X_WC", GetStateMsgFromTransform(X_WC, X_TC_msg.utime));
   }
 }
 
 void ChickenHeadPlan::Step(const State &state, double control_period, double t,
                            Command *cmd) const {
+  // Forward kinematics.
+  plant_->SetPositions(plant_context_.get(), state.q);
+  const auto &frame_W = plant_->world_frame();
+  const auto X_WT =
+      plant_->CalcRelativeTransform(*plant_context_, frame_W, frame_L7_) *
+      X_L7T_;
+
+  // New T_desired.
   RigidTransformd X_WTd;
   {
     std::lock_guard<std::mutex> lock(mutex_rpe_);
     X_WTd = X_WCd_ * X_TC_.inverse();
+    // Also Update X_WT_.
+    X_WT_ = X_WT;
   }
 
   // DiffIk.
-  plant_->SetPositions(plant_context_.get(), state.q);
-  const auto &frame_W = plant_->world_frame();
-  const auto X_WT =
-      plant_->CalcRelativeTransform(*plant_context_, frame_W, frame_L7_) *
-      X_L7T_;
   const Vector6<double> V_WT_desired =
       drake::manipulation::planner::ComputePoseDiffInCommonFrame(
           X_WT.GetAsIsometry3(), X_WTd.GetAsIsometry3()) /
@@ -91,4 +123,6 @@ void ChickenHeadPlan::Step(const State &state, double control_period, double t,
     cmd->q_cmd = state.q + control_period * result.joint_velocities.value();
     cmd->tau_cmd = Eigen::VectorXd::Zero(7);
   }
+
+  // Publish X_WC.
 }
diff --git a/src/plans/chicken_head_plan.h b/src/plans/chicken_head_plan.h
index 0e20908..5e71d39 100644
--- a/src/plans/chicken_head_plan.h
+++ b/src/plans/chicken_head_plan.h
@@ -19,6 +19,7 @@
  * The plan subscribes to Relative Pose Estimator and receives X_TC.
  * The goal is to have T track Td (Tool_desired), so that
  *  X_WTd * X_TC = X_WCd.
+ *  Also publishes X_WC on the LCM channel "X_WC".
  */
 class ChickenHeadPlan : public PlanBase {
  public:
@@ -46,11 +47,12 @@ class ChickenHeadPlan : public PlanBase {
   params_;
 
   // Relative pose estimator subscription.
-  void ReceiveRelativePose() const;
+  void PoseIo() const;
   std::unique_ptr<drake::lcm::DrakeLcm> owned_lcm_;
   std::unique_ptr<drake::lcm::Subscriber<drake::lcmt_robot_state>> rpe_sub_;
   std::thread sub_thread_;
   std::atomic<bool> stop_flag_{false};
   mutable std::mutex mutex_rpe_;
   mutable drake::math::RigidTransformd X_TC_;
+  mutable drake::math::RigidTransformd X_WT_;
 };
diff --git a/src/state_machine/plan_manager_state_machine.cc b/src/state_machine/plan_manager_state_machine.cc
index e0fde0a..02b31f3 100644
--- a/src/state_machine/plan_manager_state_machine.cc
+++ b/src/state_machine/plan_manager_state_machine.cc
@@ -55,7 +55,6 @@ bool PlanManagerStateBase::CommandHasError(
       cmd.q_cmd.array().isNaN().sum() or cmd.tau_cmd.array().isNaN().sum();
 
   bool is_too_far_away = (state.q - cmd.q_cmd).norm() > q_threshold;
-
   bool is_error = is_nan or is_too_far_away;
   if (is_error) {
     while (!state_machine->plans_.empty()) {

From 52563aac249e989e4be713ff8e1326ea9a8aefa2 Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Fri, 29 Oct 2021 01:10:06 -0400
Subject: [PATCH 05/10] Support exceptions in Plan::Step. Now handled in
 IiwaPlanManager.

---
 src/examples/zmq_client_example.py            |  1 +
 src/iiwa_plan_manager.cc                      | 85 ++++++++++---------
 src/iiwa_plan_manager.h                       | 15 ++--
 .../iiwa_plan_manager_system.cc               |  2 +-
 src/plan_runner_client/zmq_client.py          |  2 +-
 src/plans/chicken_head_plan.cc                |  3 +-
 src/plans/plan_base.h                         | 23 +++++
 src/plans/task_space_trajectory_plan.cc       | 10 +--
 .../plan_manager_state_machine.cc             | 26 +++---
 .../plan_manager_state_machine.h              | 25 ++++--
 src/state_machine/state_error.cc              | 15 +++-
 src/state_machine/state_error.h               |  8 +-
 src/state_machine/state_init.cc               |  8 +-
 src/state_machine/state_init.h                |  6 +-
 14 files changed, 137 insertions(+), 92 deletions(-)

diff --git a/src/examples/zmq_client_example.py b/src/examples/zmq_client_example.py
index 2cd9f6d..a060c97 100644
--- a/src/examples/zmq_client_example.py
+++ b/src/examples/zmq_client_example.py
@@ -76,6 +76,7 @@ def run_joint_space_plan_loop():
         stop_duration = np.random.rand() * duration
         time.sleep(stop_duration)
         zmq_client.abort()
+        zmq_client.wait_for_plan_to_finish()
         print("plan aborted after t = {}s".format(stop_duration))
 
         # # get current robot position.
diff --git a/src/iiwa_plan_manager.cc b/src/iiwa_plan_manager.cc
index cb41858..12213ed 100644
--- a/src/iiwa_plan_manager.cc
+++ b/src/iiwa_plan_manager.cc
@@ -17,6 +17,7 @@ using std::endl;
 
 IiwaPlanManager::IiwaPlanManager(YAML::Node config)
     : config_(std::move(config)),
+      lcm_cmd_channel_name_(config_["lcm_command_channel"].as<std::string>()),
       control_period_seconds_(config["control_period"].as<double>()) {
   double t_now_seconds =
       std::chrono::duration_cast<DoubleSeconds>(
@@ -50,18 +51,16 @@ void IiwaPlanManager::Run() {
 }
 
 void IiwaPlanManager::CalcCommandFromStatus() {
-  lcm_status_command_ = std::make_unique<lcm::LCM>();
-  auto sub = lcm_status_command_->subscribe(
-      config_["lcm_status_channel"].as<std::string>(),
-      &IiwaPlanManager::HandleIiwaStatus, this);
-  sub->setQueueCapacity(1);
+  owned_lcm_ = std::make_unique<drake::lcm::DrakeLcm>();
+  iiwa_status_sub_ =
+      std::make_unique<drake::lcm::Subscriber<drake ::lcmt_iiwa_status>>(
+          owned_lcm_.get(), config_["lcm_status_channel"].as<std::string>());
+  // LCM-driven loop.
   while (true) {
-    // >0 if a message was handled,
-    // 0 if the function timed out,
-    // <0 if an error occured.
-    if (lcm_status_command_->handleTimeout(10) < 0) {
-      break;
-    }
+    iiwa_status_sub_->clear();
+    drake::lcm::LcmHandleSubscriptionsUntil(
+        owned_lcm_.get(), [&]() { return iiwa_status_sub_->count() > 0; });
+    HandleIiwaStatus(iiwa_status_sub_->message());
   }
 }
 
@@ -180,19 +179,18 @@ std::vector<uint8_t> PrependLcmMsgWithChannel(const std::string &channel_name,
 }
 
 void IiwaPlanManager::HandleIiwaStatus(
-    const lcm::ReceiveBuffer *, const std::string &channel,
-    const drake::lcmt_iiwa_status *status_msg) {
+    const drake::lcmt_iiwa_status &status_msg) {
   const PlanBase *plan;
   auto t_now = std::chrono::high_resolution_clock::now();
-  const int num_joints = (*status_msg).num_joints;
-  State s(Eigen::Map<const VectorXd>(
-              (*status_msg).joint_position_measured.data(), num_joints),
-          Eigen::Map<const VectorXd>(
-              (*status_msg).joint_velocity_estimated.data(), num_joints),
-          Eigen::Map<const VectorXd>((*status_msg).joint_torque_external.data(),
+  const int num_joints = status_msg.num_joints;
+  State s(Eigen::Map<const VectorXd>(status_msg.joint_position_measured.data(),
+                                     num_joints),
+          Eigen::Map<const VectorXd>(status_msg.joint_velocity_estimated.data(),
+                                     num_joints),
+          Eigen::Map<const VectorXd>(status_msg.joint_torque_external.data(),
                                      num_joints));
   Command c;
-  bool command_has_error;
+  bool command_has_error{true};
   {
     // Lock state machine.
     std::lock_guard<std::mutex> lock(mutex_state_machine_);
@@ -215,7 +213,7 @@ void IiwaPlanManager::HandleIiwaStatus(
      *       - set state_machine.current_plan_start_time_seconds_ to nullptr.
      *       - change state to IDLE.
      */
-    plan = state_machine_->GetCurrentPlan(t_now, *status_msg);
+    plan = state_machine_->GetCurrentPlan(t_now, status_msg);
 
     /*
      * ReceiveNewStatusMsg.
@@ -230,36 +228,43 @@ void IiwaPlanManager::HandleIiwaStatus(
      * Error:
      *   - do nothing.
      */
-    state_machine_->ReceiveNewStatusMsg(*status_msg);
-
-    // Compute command.
-    if (plan) {
-      const double t_plan = state_machine_->GetCurrentPlanUpTime(t_now);
-      plan->Step(s, control_period_seconds_, t_plan, &c);
-    } else if (state_machine_->get_state_type() ==
-               PlanManagerStateTypes::kStateIdle) {
-      c.q_cmd = state_machine_->get_iiwa_position_command_idle();
-      c.tau_cmd = Eigen::VectorXd::Zero(num_joints);
-    } else {
-      // No commands are sent in state INIT or ERROR.
-      return;
-    }
+    state_machine_->ReceiveNewStatusMsg(status_msg);
+
+    try {
+      // Compute command.
+      if (plan) {
+        const double t_plan = state_machine_->GetCurrentPlanUpTime(t_now);
+        plan->Step(s, control_period_seconds_, t_plan, &c);
+      } else if (state_machine_->get_state_type() ==
+                 PlanManagerStateTypes::kStateIdle) {
+        c.q_cmd = state_machine_->get_iiwa_position_command_idle();
+        c.tau_cmd = Eigen::VectorXd::Zero(num_joints);
+      } else {
+        // No commands are sent in state INIT or ERROR.
+        return;
+      }
+
+      // Check command for error.
+      state_machine_->CheckCommandForError(s, c);
 
-    // Check command for error.
-    command_has_error = state_machine_->CommandHasError(s, c);
+      // If no exception has been thrown thus far, there is no error in command.
+      command_has_error = false;
+    } catch (PlanException &e) {
+      state_machine_->EnterErrorState();
+      spdlog::critical(e.what());
+    }
   }
 
   if (!command_has_error) {
     drake::lcmt_iiwa_command cmd_msg;
     cmd_msg.num_joints = num_joints;
     cmd_msg.num_torques = num_joints;
-    cmd_msg.utime = status_msg->utime;
+    cmd_msg.utime = status_msg.utime;
     for (int i = 0; i < num_joints; i++) {
       cmd_msg.joint_position.push_back(c.q_cmd[i]);
       cmd_msg.joint_torque.push_back(c.tau_cmd[i]);
     }
-    lcm_status_command_->publish(
-        config_["lcm_command_channel"].as<std::string>(), &cmd_msg);
+    drake::lcm::Publish(owned_lcm_.get(), lcm_cmd_channel_name_, cmd_msg);
     state_machine_->SetIiwaPositionCommandIdle(c.q_cmd);
   }
 }
diff --git a/src/iiwa_plan_manager.h b/src/iiwa_plan_manager.h
index 64ef92f..8fdba7c 100644
--- a/src/iiwa_plan_manager.h
+++ b/src/iiwa_plan_manager.h
@@ -6,7 +6,7 @@
 #include <zmq.hpp>
 
 #include "drake_lcmtypes/drake/lcmt_iiwa_status.hpp"
-#include "lcm/lcm-cpp.hpp"
+#include "drake/lcm/drake_lcm.h"
 
 #include "plans/iiwa_plan_factory.h"
 #include "state_machine/plan_manager_state_machine.h"
@@ -18,23 +18,24 @@ class IiwaPlanManager {
   void Run();
 
 private:
+  const YAML::Node config_;
+  const std::string lcm_cmd_channel_name_;
   const double control_period_seconds_;
   mutable std::mutex mutex_state_machine_;
   std::unique_ptr<PlanManagerStateMachine> state_machine_;
   std::unordered_map<std::string, std::thread> threads_;
   std::unique_ptr<IiwaPlanFactory> plan_factory_;
 
-  const YAML::Node config_;
   // This context is used in multiple threads. zmq context should be
   // thread-safe, according to their documentation...
   zmq::context_t zmq_ctx_;
 
   // Iiwa status + command thread.
-  std::unique_ptr<lcm::LCM> lcm_status_command_;
-  void CalcCommandFromStatus();
-  void HandleIiwaStatus(const lcm::ReceiveBuffer *rbuf,
-                        const std::string &channel,
-                        const drake::lcmt_iiwa_status *status_msg);
+  std::unique_ptr<drake::lcm::DrakeLcm> owned_lcm_;
+  std::unique_ptr<drake::lcm::Subscriber<drake::lcmt_iiwa_status>>
+    iiwa_status_sub_;
+  [[noreturn]] void CalcCommandFromStatus();
+  void HandleIiwaStatus(const drake::lcmt_iiwa_status& status_msg);
 
   // Printing thread.
   [[noreturn]] void PrintStateMachineStatus() const;
diff --git a/src/plan_manager_system/iiwa_plan_manager_system.cc b/src/plan_manager_system/iiwa_plan_manager_system.cc
index b5c5a9f..aacfe4c 100644
--- a/src/plan_manager_system/iiwa_plan_manager_system.cc
+++ b/src/plan_manager_system/iiwa_plan_manager_system.cc
@@ -100,7 +100,7 @@ void IiwaPlanManagerSystem::CalcIiwaCommand(
   }
 
   // Check command for error.
-  if (!state_machine_->CommandHasError(s, c)) {
+  if (!state_machine_->CheckCommandForError(s, c)) {
     const int num_joints = msg_iiwa_status.num_joints;
     msg_iiwa_command.num_joints = num_joints;
     msg_iiwa_command.num_torques = num_joints;
diff --git a/src/plan_runner_client/zmq_client.py b/src/plan_runner_client/zmq_client.py
index 7a25ab1..7d886d9 100644
--- a/src/plan_runner_client/zmq_client.py
+++ b/src/plan_runner_client/zmq_client.py
@@ -204,7 +204,7 @@ def wait_for_plan_to_finish(self):
     def abort(self):
         self.abort_client.send(b"abort")
         s = self.abort_client.recv_string()
-        print(s)
+        self.iiwa_position_getter.logger.info(s)
 
 
 class SchunkManager:
diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
index bf051b0..68381e0 100644
--- a/src/plans/chicken_head_plan.cc
+++ b/src/plans/chicken_head_plan.cc
@@ -117,8 +117,7 @@ void ChickenHeadPlan::Step(const State &state, double control_period, double t,
 
   // 3. Check for errors and integrate.
   if (result.status != DifferentialInverseKinematicsStatus::kSolutionFound) {
-    cmd->q_cmd = NAN * Eigen::VectorXd::Zero(7);
-    spdlog::critical("DoDifferentialKinematics Failed to find a solution.");
+    throw DiffIkException();
   } else {
     cmd->q_cmd = state.q + control_period * result.joint_velocities.value();
     cmd->tau_cmd = Eigen::VectorXd::Zero(7);
diff --git a/src/plans/plan_base.h b/src/plans/plan_base.h
index 59cc37d..fd5604d 100644
--- a/src/plans/plan_base.h
+++ b/src/plans/plan_base.h
@@ -26,6 +26,29 @@ struct Command {
   Eigen::VectorXd tau_cmd;
 };
 
+struct PlanException : public std::exception {
+  [[nodiscard]] const char* what() const noexcept override = 0;
+};
+
+struct NanException : PlanException {
+  [[nodiscard]] const char* what() const noexcept override {
+    return "Joint angle commands contain Nan.";
+  }
+};
+
+struct TooFarAwayException : PlanException {
+  [[nodiscard]] const char* what() const noexcept override {
+    return "Difference between commanded and measured joint angles are too "
+           "large.";
+  }
+};
+
+struct DiffIkException : PlanException {
+  [[nodiscard]] const char* what() const noexcept override {
+    return "Differential inverse kinematics failed to find a solution.";
+  }
+};
+
 class PlanBase {
 public:
   explicit PlanBase(const drake::multibody::MultibodyPlant<double> *plant)
diff --git a/src/plans/task_space_trajectory_plan.cc b/src/plans/task_space_trajectory_plan.cc
index d3128bd..90c3406 100644
--- a/src/plans/task_space_trajectory_plan.cc
+++ b/src/plans/task_space_trajectory_plan.cc
@@ -56,15 +56,7 @@ void TaskSpaceTrajectoryPlan::Step(const State &state, double control_period,
 
   // 3. Check for errors and integrate.
   if (result.status != DifferentialInverseKinematicsStatus::kSolutionFound) {
-    // Set the command to NAN so that state machine will detect downstream and
-    // go to error state.
-    cmd->q_cmd = NAN * Eigen::VectorXd::Zero(7);
-    // TODO(terry-suh): how do I tell the use that the state machine went to
-    //  error because of this precise reason? Printing the error message here
-    //  seems like a good start, but we'll need to handle this better.
-    //  (Pang): I think throwing exceptions here and have them handled at the
-    //   level of PlanManager is a good solution.
-    spdlog::critical("DoDifferentialKinematics Failed to find a solution.");
+    throw DiffIkException();
   } else {
     cmd->q_cmd = state.q + control_period * result.joint_velocities.value();
     cmd->tau_cmd = Eigen::VectorXd::Zero(7);
diff --git a/src/state_machine/plan_manager_state_machine.cc b/src/state_machine/plan_manager_state_machine.cc
index 02b31f3..c492587 100644
--- a/src/state_machine/plan_manager_state_machine.cc
+++ b/src/state_machine/plan_manager_state_machine.cc
@@ -48,22 +48,28 @@ void PlanManagerStateBase::QueueNewPlan(PlanManagerStateMachine *state_machine,
   throw std::runtime_error(error_msg);
 }
 
-bool PlanManagerStateBase::CommandHasError(
+void PlanManagerStateBase::CheckCommandForError(
     const State &state, const Command &cmd,
     PlanManagerStateMachine *state_machine, const double q_threshold) {
   bool is_nan =
       cmd.q_cmd.array().isNaN().sum() or cmd.tau_cmd.array().isNaN().sum();
+  if (is_nan) {
+    throw NanException();
+  }
 
-  bool is_too_far_away = (state.q - cmd.q_cmd).norm() > q_threshold;
-  bool is_error = is_nan or is_too_far_away;
-  if (is_error) {
-    while (!state_machine->plans_.empty()) {
-      // Delete all plans.
-      state_machine->plans_.pop();
-    }
-    ChangeState(state_machine, StateError::Instance());
+  const double dq_norm = (state.q - cmd.q_cmd).norm();
+  if (dq_norm > q_threshold) {
+    throw TooFarAwayException();
+  }
+}
+
+void PlanManagerStateBase::EnterErrorState(
+    PlanManagerStateMachine *state_machine) {
+  while (!state_machine->plans_.empty()) {
+    // Delete all plans.
+    state_machine->plans_.pop();
   }
-  return is_error;
+  ChangeState(state_machine, StateError::Instance());
 }
 
 bool PlanManagerStateBase::has_received_status_msg() const {
diff --git a/src/state_machine/plan_manager_state_machine.h b/src/state_machine/plan_manager_state_machine.h
index 141c0a6..0aef884 100644
--- a/src/state_machine/plan_manager_state_machine.h
+++ b/src/state_machine/plan_manager_state_machine.h
@@ -50,7 +50,9 @@ class PlanManagerStateMachine {
   // 1. Nans
   // 2. If cmd.q_cmd and state.q is too far away with a hard-coded threshold.
   //    This threshold is decided by a parameter in the config file.
-  bool CommandHasError(const State &state, const Command &cmd);
+  void CheckCommandForError(const State &state, const Command &cmd);
+
+  void EnterErrorState();
 
   // Empties the plans_ queue and sets the state to IDLE.
   void AbortAllPlans();
@@ -139,9 +141,11 @@ class PlanManagerStateBase {
   virtual void QueueNewPlan(PlanManagerStateMachine *state_machine,
                             std::unique_ptr<PlanBase> plan);
 
-  virtual bool CommandHasError(const State &state, const Command &cmd,
-                               PlanManagerStateMachine *state_machine,
-                               const double q_threshold);
+  virtual void CheckCommandForError(const State &state, const Command &cmd,
+                                    PlanManagerStateMachine *state_machine,
+                                    const double q_threshold);
+
+  virtual void EnterErrorState(PlanManagerStateMachine *state_machine);
 
   virtual void AbortAllPlans(PlanManagerStateMachine *state_machine);
 
@@ -163,6 +167,7 @@ class PlanManagerStateBase {
     return state_name_;
   };
 
+
 protected:
   explicit PlanManagerStateBase(std::string state_name)
       : state_name_(std::move(state_name)){};
@@ -244,10 +249,14 @@ PlanManagerStateMachine::set_current_plan_start_time(double t_now_seconds) {
   current_plan_start_time_seconds_ = std::make_unique<double>(t_now_seconds);
 }
 
-inline bool PlanManagerStateMachine::CommandHasError(const State &state,
-                                                     const Command &cmd) {
-  return state_->CommandHasError(state, cmd, this,
-                                 config_["q_threshold"].as<double>());
+inline void PlanManagerStateMachine::CheckCommandForError(const State &state,
+                                                          const Command &cmd) {
+  state_->CheckCommandForError(state, cmd, this,
+                               config_["q_threshold"].as<double>());
+}
+
+inline void PlanManagerStateMachine::EnterErrorState() {
+  state_->EnterErrorState(this);
 }
 
 inline void PlanManagerStateMachine::AbortAllPlans() {
diff --git a/src/state_machine/state_error.cc b/src/state_machine/state_error.cc
index fd0d97b..b3c4a69 100644
--- a/src/state_machine/state_error.cc
+++ b/src/state_machine/state_error.cc
@@ -11,10 +11,17 @@ PlanManagerStateBase *StateError::Instance() {
   return instance_;
 }
 
-bool StateError::CommandHasError(const State &state, const Command &cmd,
-                     PlanManagerStateMachine *state_machine,
-                     const double q_threshold) {
-  std::string error_msg = "CommandHasError should not be called in state ";
+void StateError::CheckCommandForError(const State &state, const Command &cmd,
+                                      PlanManagerStateMachine *state_machine,
+                                      const double q_threshold) {
+  std::string error_msg = "CheckCommandForError should not be called in state ";
+  error_msg += get_state_name();
+  error_msg += ".";
+  throw std::runtime_error(error_msg);
+}
+
+void StateError::EnterErrorState(PlanManagerStateMachine *state_machine) {
+  std::string error_msg = "EnterErrorState should not be called in state ";
   error_msg += get_state_name();
   error_msg += ".";
   throw std::runtime_error(error_msg);
diff --git a/src/state_machine/state_error.h b/src/state_machine/state_error.h
index b42eed4..4867a6a 100644
--- a/src/state_machine/state_error.h
+++ b/src/state_machine/state_error.h
@@ -8,9 +8,11 @@ class StateError : public PlanManagerStateBase {
   void QueueNewPlan(PlanManagerStateMachine *state_machine,
                     std::unique_ptr<PlanBase> plan) override;
 
-  bool CommandHasError(const State &state, const Command &cmd,
-                       PlanManagerStateMachine *state_machine,
-                       const double q_threshold) override;
+  void CheckCommandForError(const State &state, const Command &cmd,
+                            PlanManagerStateMachine *state_machine,
+                            const double q_threshold) override;
+
+  void EnterErrorState(PlanManagerStateMachine *state_machine) override;
 
   void PrintCurrentState(const PlanManagerStateMachine *state_machine,
                          double t_now_seconds) const override;
diff --git a/src/state_machine/state_init.cc b/src/state_machine/state_init.cc
index 86ebe00..1ac9c32 100644
--- a/src/state_machine/state_init.cc
+++ b/src/state_machine/state_init.cc
@@ -27,10 +27,10 @@ void StateInit::QueueNewPlan(PlanManagerStateMachine *state_machine,
                "Received plan is discarded.");
 }
 
-bool StateInit::CommandHasError(const State &state, const Command &cmd,
-                                PlanManagerStateMachine *state_machine,
-                                const double q_threshold) {
-  std::string error_msg = "CommandHasError should not be called in state ";
+void StateInit::CheckCommandForError(const State &state, const Command &cmd,
+                                     PlanManagerStateMachine *state_machine,
+                                     const double q_threshold) {
+  std::string error_msg = "CheckCommandForError should not be called in state ";
   error_msg += get_state_name();
   error_msg += ".";
   throw std::runtime_error(error_msg);
diff --git a/src/state_machine/state_init.h b/src/state_machine/state_init.h
index f32c601..f928324 100644
--- a/src/state_machine/state_init.h
+++ b/src/state_machine/state_init.h
@@ -14,9 +14,9 @@ class StateInit : public PlanManagerStateBase {
   void QueueNewPlan(PlanManagerStateMachine *state_machine,
                     std::unique_ptr<PlanBase> plan) override;
 
-  bool CommandHasError(const State &state, const Command &cmd,
-                       PlanManagerStateMachine *state_machine,
-                       const double q_threshold) override;
+  void CheckCommandForError(const State &state, const Command &cmd,
+                            PlanManagerStateMachine *state_machine,
+                            const double q_threshold) override;
 
   void PrintCurrentState(const PlanManagerStateMachine *manager,
                          double t_now_seconds) const override;

From 7f99abb1d30cebf80e2fdb75fa409b5b7edf4726 Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Fri, 29 Oct 2021 18:23:07 -0400
Subject: [PATCH 06/10] Ditch DiffIk in ChickenHeadPlan, now it's buttery
 smooth.

---
 src/plans/chicken_head_plan.cc | 84 ++++++++++++++++++----------------
 src/plans/chicken_head_plan.h  | 14 ++++--
 src/plans/iiwa_plan_factory.cc |  3 --
 3 files changed, 53 insertions(+), 48 deletions(-)

diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
index 68381e0..5b204ad 100644
--- a/src/plans/chicken_head_plan.cc
+++ b/src/plans/chicken_head_plan.cc
@@ -1,26 +1,24 @@
 #include "plans/chicken_head_plan.h"
 #include "drake/lcm/lcm_messages.h"
 
-using drake::Vector6;
-using drake::math::RigidTransformd;
 using drake::lcmt_robot_state;
-using drake::manipulation::planner::internal::DoDifferentialInverseKinematics;
+using drake::Vector6;
 using drake::manipulation::planner::DifferentialInverseKinematicsStatus;
+using drake::manipulation::planner::internal::DoDifferentialInverseKinematics;
+using drake::math::RigidTransformd;
+using drake::math::RotationMatrixd;
+using Eigen::Vector3d;
+using Eigen::VectorXd;
 
 ChickenHeadPlan::ChickenHeadPlan(
     const drake::math::RigidTransformd &X_WL7_0, double duration,
-    double control_time_step,
-    const Eigen::Ref<const Eigen::VectorXd> &nominal_joint_angles,
     const drake::multibody::MultibodyPlant<double> *plant)
     : PlanBase(plant), duration_(duration), X_WCd_(X_WL7_0 * X_L7T_),
       frame_L7_(plant->GetFrameByName("iiwa_link_7")) {
   // DiffIk.
-  params_ = std::make_unique<
-      drake::manipulation::planner::DifferentialInverseKinematicsParameters>(
-      plant_->num_positions(), plant_->num_velocities());
-  params_->set_timestep(control_time_step);
-  params_->set_nominal_joint_position(nominal_joint_angles);
   plant_context_ = plant_->CreateDefaultContext();
+  solver_ = std::make_unique<drake::solvers::GurobiSolver>();
+  result_ = std::make_unique<drake::solvers::MathematicalProgramResult>();
 
   // X_TC subscription.
   owned_lcm_ = std::make_unique<drake::lcm::DrakeLcm>();
@@ -34,21 +32,18 @@ ChickenHeadPlan::~ChickenHeadPlan() {
   sub_thread_.join();
 }
 
-lcmt_robot_state GetStateMsgFromTransform(
-    const RigidTransformd& X_WC, int64_t utime) {
+lcmt_robot_state GetStateMsgFromTransform(const RigidTransformd &X_WC,
+                                          int64_t utime) {
   const auto Q_WC = X_WC.rotation().ToQuaternion();
-  const auto& p_WC = X_WC.translation();
+  const auto &p_WC = X_WC.translation();
   lcmt_robot_state msg_X_WC{};
   msg_X_WC.utime = utime;
   msg_X_WC.num_joints = 7;
   msg_X_WC.joint_name = {"qw", "qx", "qy", "qz", "x", "y", "z"};
   msg_X_WC.joint_position = std::vector<float>(
-      {static_cast<float>(Q_WC.w()),
-       static_cast<float>(Q_WC.x()),
-       static_cast<float>(Q_WC.y()),
-       static_cast<float>(Q_WC.z()),
-       static_cast<float>(p_WC[0]),
-       static_cast<float>(p_WC[1]),
+      {static_cast<float>(Q_WC.w()), static_cast<float>(Q_WC.x()),
+       static_cast<float>(Q_WC.y()), static_cast<float>(Q_WC.z()),
+       static_cast<float>(p_WC[0]), static_cast<float>(p_WC[1]),
        static_cast<float>(p_WC[2])});
   return msg_X_WC;
 }
@@ -56,9 +51,9 @@ lcmt_robot_state GetStateMsgFromTransform(
 void ChickenHeadPlan::PoseIo() const {
   while (true) {
     rpe_sub_->clear();
-    drake::lcm::LcmHandleSubscriptionsUntil(
-        owned_lcm_.get(),
-        [&]() { return stop_flag_ or rpe_sub_->count() > 0; });
+    drake::lcm::LcmHandleSubscriptionsUntil(owned_lcm_.get(), [&]() {
+      return stop_flag_ or rpe_sub_->count() > 0;
+    });
     if (stop_flag_) {
       break;
     }
@@ -77,8 +72,8 @@ void ChickenHeadPlan::PoseIo() const {
     }
 
     // Publish X_WC.
-    drake::lcm::Publish(
-        owned_lcm_.get(), "X_WC", GetStateMsgFromTransform(X_WC, X_TC_msg.utime));
+    drake::lcm::Publish(owned_lcm_.get(), "X_WC",
+                        GetStateMsgFromTransform(X_WC, X_TC_msg.utime));
   }
 }
 
@@ -100,28 +95,37 @@ void ChickenHeadPlan::Step(const State &state, double control_period, double t,
     X_WT_ = X_WT;
   }
 
-  // DiffIk.
-  const Vector6<double> V_WT_desired =
-      drake::manipulation::planner::ComputePoseDiffInCommonFrame(
-          X_WT.GetAsIsometry3(), X_WTd.GetAsIsometry3()) /
-      params_->get_timestep();
+  const auto &R_WT = X_WT.rotation();
+  const Vector3d e_xyz = X_WTd.translation() - X_WT.translation();
+  const RotationMatrixd R_TTd = R_WT.inverse() * X_WTd.rotation();
+  Vector6<double> V_WT_desired;
+  V_WT_desired.head(3) =
+      R_WT * (kp_rotation_ * R_TTd.ToQuaternion().vec().array()).matrix();
+  V_WT_desired.tail(3) = kp_translation_ * e_xyz.array();
 
-  Eigen::MatrixXd J_WT(6, plant_->num_velocities());
   plant_->CalcJacobianSpatialVelocity(
       *plant_context_, drake::multibody::JacobianWrtVariable::kV, frame_L7_,
-      X_L7T_.translation(), frame_W, frame_W, &J_WT);
+      X_L7T_.translation(), frame_W, frame_W, &Jv_WTq_W_);
 
-  const auto result = DoDifferentialInverseKinematics(
-      state.q, state.v, X_WT, J_WT,
-      drake::multibody::SpatialVelocity<double>(V_WT_desired), *params_);
+  const int nq = plant_->num_velocities();
+  auto prog = drake::solvers::MathematicalProgram();
+  auto dq = prog.NewContinuousVariables(nq, "dq");
+
+  const Eigen::MatrixXd &A = Jv_WTq_W_;
+  const Eigen::VectorXd b = V_WT_desired * control_period;
+  // minimize ||A * dq - b||^2 + 0.01 * ||dq||^2
+
+  prog.AddQuadraticCost(
+      (A.transpose() * A + 0.01 * Eigen::MatrixXd::Identity(nq, nq)),
+      -A.transpose() * b, dq);
+
+  solver_->Solve(prog, {}, {}, result_.get());
 
   // 3. Check for errors and integrate.
-  if (result.status != DifferentialInverseKinematicsStatus::kSolutionFound) {
-    throw DiffIkException();
+  if (result_->is_success()) {
+    cmd->q_cmd = state.q + result_->GetSolution(dq);
+    cmd->tau_cmd = Eigen::VectorXd::Zero(nq);
   } else {
-    cmd->q_cmd = state.q + control_period * result.joint_velocities.value();
-    cmd->tau_cmd = Eigen::VectorXd::Zero(7);
+    throw DiffIkException();
   }
-
-  // Publish X_WC.
 }
diff --git a/src/plans/chicken_head_plan.h b/src/plans/chicken_head_plan.h
index 5e71d39..d31a6ef 100644
--- a/src/plans/chicken_head_plan.h
+++ b/src/plans/chicken_head_plan.h
@@ -4,6 +4,8 @@
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_robot_state.hpp"
 #include "drake/manipulation/planner/differential_inverse_kinematics.h"
+#include "drake/solvers/gurobi_solver.h"
+#include "drake/solvers/mathematical_program.h"
 #include <Eigen/Dense>
 
 /*
@@ -25,8 +27,6 @@ class ChickenHeadPlan : public PlanBase {
  public:
   ChickenHeadPlan(const drake::math::RigidTransformd &X_WL7_0,
                   double duration,
-                  double control_time_step,
-                  const Eigen::Ref<const Eigen::VectorXd> &nominal_joint_angles,
                   const drake::multibody::MultibodyPlant<double> *plant);
   ~ChickenHeadPlan() override;
 
@@ -41,10 +41,14 @@ class ChickenHeadPlan : public PlanBase {
       Eigen::Vector3d(0, 0, 0.255));
   const drake::math::RigidTransformd X_WCd_;
   const drake::multibody::Frame<double>& frame_L7_;
+
+  // DiffIk
+  const Eigen::Array3d kp_translation_{100, 100, 100};
+  const Eigen::Array3d kp_rotation_{50, 50, 50};
+  mutable Eigen::MatrixXd Jv_WTq_W_{6, 7};
   std::unique_ptr<drake::systems::Context<double>> plant_context_;
-  std::unique_ptr<
-  drake::manipulation::planner::DifferentialInverseKinematicsParameters>
-  params_;
+  std::unique_ptr<drake::solvers::GurobiSolver> solver_;
+  std::unique_ptr<drake::solvers::MathematicalProgramResult> result_;
 
   // Relative pose estimator subscription.
   void PoseIo() const;
diff --git a/src/plans/iiwa_plan_factory.cc b/src/plans/iiwa_plan_factory.cc
index cc8f723..c697f78 100644
--- a/src/plans/iiwa_plan_factory.cc
+++ b/src/plans/iiwa_plan_factory.cc
@@ -128,10 +128,7 @@ std::unique_ptr<PlanBase> IiwaPlanFactory::MakeTaskSpaceTrajectoryPlan(
   const double duration = msg_plan.plan.at(1).utime / 1e6;
   const auto nominal_joint_vector =
       config_["robot_nominal_joint"].as<vector<double>>();
-  auto nominal_joint = Eigen::Map<const Eigen::VectorXd>(
-      nominal_joint_vector.data(), nominal_joint_vector.size());
   return std::make_unique<ChickenHeadPlan>(
       TransformFromRobotStateMsg(msg_plan.plan[0].joint_position), duration,
-      config_["control_period"].as<double>(), nominal_joint,
       plant_.get());
 }

From d16b35f0824d2ec9ba0e26e4f5ce53c014df3a6b Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Fri, 29 Oct 2021 18:28:35 -0400
Subject: [PATCH 07/10] Faster time out in drake LCM subscription.

---
 src/plans/chicken_head_plan.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
index 5b204ad..c5c9f11 100644
--- a/src/plans/chicken_head_plan.cc
+++ b/src/plans/chicken_head_plan.cc
@@ -53,7 +53,7 @@ void ChickenHeadPlan::PoseIo() const {
     rpe_sub_->clear();
     drake::lcm::LcmHandleSubscriptionsUntil(owned_lcm_.get(), [&]() {
       return stop_flag_ or rpe_sub_->count() > 0;
-    });
+    }, 10);
     if (stop_flag_) {
       break;
     }

From b2ce5613c6a3d0f9a58bd536ba9aac56a9952a5b Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Fri, 29 Oct 2021 18:42:37 -0400
Subject: [PATCH 08/10] Fix DrakeSystem version of PlanRunner.

---
 .../iiwa_plan_manager_system.cc               | 99 +++++++++++--------
 1 file changed, 57 insertions(+), 42 deletions(-)

diff --git a/src/plan_manager_system/iiwa_plan_manager_system.cc b/src/plan_manager_system/iiwa_plan_manager_system.cc
index aacfe4c..f847f0d 100644
--- a/src/plan_manager_system/iiwa_plan_manager_system.cc
+++ b/src/plan_manager_system/iiwa_plan_manager_system.cc
@@ -44,11 +44,11 @@ IiwaPlanManagerSystem::IiwaPlanManagerSystem(YAML::Node config)
 void IiwaPlanManagerSystem::CalcIiwaCommand(
     const drake::systems::Context<double> &context,
     drake::lcmt_iiwa_command *cmd) const {
-  const auto &msg_iiwa_status =
+  const auto &status_msg =
       get_iiwa_status_input_port().Eval<lcmt_iiwa_status>(context);
   const auto &msg_robot_plan =
       get_robot_plan_input_port().Eval<lcmt_robot_plan>(context);
-  auto &msg_iiwa_command = *cmd;
+  auto &cmd_msg = *cmd;
   // Handle new robot plan messages.
   if (msg_robot_plan.num_states > 0 and
       msg_robot_plan.utime != last_robot_plan_utime_) {
@@ -59,57 +59,72 @@ void IiwaPlanManagerSystem::CalcIiwaCommand(
   }
 
   // Handle new iiwa status messages.
-  if (msg_iiwa_status.num_joints == 0) {
+  if (status_msg.num_joints == 0) {
     return;
   }
 
   const double t_now = context.get_time();
-  auto plan = state_machine_->GetCurrentPlan(t_now, msg_iiwa_status);
-  state_machine_->ReceiveNewStatusMsg(msg_iiwa_status);
+  auto plan = state_machine_->GetCurrentPlan(t_now, status_msg);
+  state_machine_->ReceiveNewStatusMsg(status_msg);
 
-  State s(
-      Eigen::Map<const VectorXd>(msg_iiwa_status.joint_position_measured.data(),
-                                 msg_iiwa_status.num_joints),
-      Eigen::Map<const VectorXd>(
-          msg_iiwa_status.joint_velocity_estimated.data(),
-          msg_iiwa_status.num_joints),
-      Eigen::Map<const VectorXd>(msg_iiwa_status.joint_torque_external.data(),
-                                 msg_iiwa_status.num_joints));
+  State s(Eigen::Map<const VectorXd>(status_msg.joint_position_measured.data(),
+                                     status_msg.num_joints),
+          Eigen::Map<const VectorXd>(status_msg.joint_velocity_estimated.data(),
+                                     status_msg.num_joints),
+          Eigen::Map<const VectorXd>(status_msg.joint_torque_external.data(),
+                                     status_msg.num_joints));
   Command c;
 
-  if (plan) {
-    const double t_plan = state_machine_->GetCurrentPlanUpTime(t_now);
-    plan->Step(s, control_period_seconds_, t_plan, &c);
-  } else if (state_machine_->get_state_type() ==
-             PlanManagerStateTypes::kStateIdle) {
-    c.q_cmd = state_machine_->get_iiwa_position_command_idle();
-    c.tau_cmd = Eigen::VectorXd::Zero(msg_iiwa_status.num_joints);
-  } else {
-    // In state INIT or ERROR.
-    // Send an empty iiwa command message with utime = -1.
-    // Behavior with mock_station_simulation:
-    //  throws with error message:
-    //   IiwaCommandReceiver expected num_joints = 7, but received 0.
-    // TODO: confirm the behavior of drake-iiwa-driver (real robot).
-    //  What I think will happen:
-    //  throws if msg_iiwa_command.num_joints != 7, unless msg_iiwa_command
-    //  .utime == -1, in which case no command is sent to the robot.
-    msg_iiwa_command = lcmt_iiwa_command();
-    msg_iiwa_command.utime = -1;
-    return;
+  bool command_has_error{true};
+  // Refer to iiwa_plan_manager.cc for more detailed documentation.
+  plan = state_machine_->GetCurrentPlan(t_now, status_msg);
+  state_machine_->ReceiveNewStatusMsg(status_msg);
+
+  try {
+    // Compute command.
+    if (plan) {
+      const double t_plan = state_machine_->GetCurrentPlanUpTime(t_now);
+      plan->Step(s, control_period_seconds_, t_plan, &c);
+    } else if (state_machine_->get_state_type() ==
+               PlanManagerStateTypes::kStateIdle) {
+      c.q_cmd = state_machine_->get_iiwa_position_command_idle();
+      c.tau_cmd = Eigen::VectorXd::Zero(status_msg.num_joints);
+    } else {
+      // In state INIT or ERROR.
+      // Send an empty iiwa command message with utime = -1.
+      // Behavior with mock_station_simulation:
+      //  throws with error message:
+      //   IiwaCommandReceiver expected num_joints = 7, but received 0.
+      // TODO: confirm the behavior of drake-iiwa-driver (real robot).
+      //  What I think will happen:
+      //  throws if cmd_msg.num_joints != 7, unless cmd_msg
+      //  .utime == -1, in which case no command is sent to the robot.
+      cmd_msg = lcmt_iiwa_command();
+      cmd_msg.utime = -1;
+      return;
+    }
+
+    // Check command for error.
+    state_machine_->CheckCommandForError(s, c);
+
+    // If no exception has been thrown thus far, there is no error in command.
+    command_has_error = false;
+  } catch (PlanException &e) {
+    state_machine_->EnterErrorState();
+    spdlog::critical(e.what());
   }
 
   // Check command for error.
-  if (!state_machine_->CheckCommandForError(s, c)) {
-    const int num_joints = msg_iiwa_status.num_joints;
-    msg_iiwa_command.num_joints = num_joints;
-    msg_iiwa_command.num_torques = num_joints;
-    msg_iiwa_command.utime = msg_iiwa_status.utime;
-    msg_iiwa_command.joint_position.resize(num_joints);
-    msg_iiwa_command.joint_torque.resize(num_joints);
+  if (!command_has_error) {
+    const int num_joints = status_msg.num_joints;
+    cmd_msg.num_joints = num_joints;
+    cmd_msg.num_torques = num_joints;
+    cmd_msg.utime = status_msg.utime;
+    cmd_msg.joint_position.resize(num_joints);
+    cmd_msg.joint_torque.resize(num_joints);
     for (int i = 0; i < num_joints; i++) {
-      msg_iiwa_command.joint_position[i] = c.q_cmd[i];
-      msg_iiwa_command.joint_torque[i] = c.tau_cmd[i];
+      cmd_msg.joint_position[i] = c.q_cmd[i];
+      cmd_msg.joint_torque[i] = c.tau_cmd[i];
     }
   }
 }

From c17821fcbbad1dfed4cf67d876beb0a0e6b50392 Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Fri, 29 Oct 2021 18:51:34 -0400
Subject: [PATCH 09/10] Include Threads as a dependency of robot_plans.

---
 src/plans/CMakeLists.txt      | 2 +-
 src/plans/chicken_head_plan.h | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/plans/CMakeLists.txt b/src/plans/CMakeLists.txt
index e770f01..0e5e83d 100644
--- a/src/plans/CMakeLists.txt
+++ b/src/plans/CMakeLists.txt
@@ -3,7 +3,7 @@ add_library(robot_plans
         joint_space_trajectory_plan.h joint_space_trajectory_plan.cc
         task_space_trajectory_plan.h task_space_trajectory_plan.cc
         chicken_head_plan.h chicken_head_plan.cc)
-target_link_libraries(robot_plans drake::drake)
+target_link_libraries(robot_plans drake::drake Threads::Threads)
 target_include_directories(robot_plans PUBLIC ${PROJECT_SOURCE_DIR}/src)
 
 add_library(iiwa_plan_factory iiwa_plan_factory.h iiwa_plan_factory.cc
diff --git a/src/plans/chicken_head_plan.h b/src/plans/chicken_head_plan.h
index d31a6ef..b902115 100644
--- a/src/plans/chicken_head_plan.h
+++ b/src/plans/chicken_head_plan.h
@@ -3,7 +3,6 @@
 #include "plans/plan_base.h"
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_robot_state.hpp"
-#include "drake/manipulation/planner/differential_inverse_kinematics.h"
 #include "drake/solvers/gurobi_solver.h"
 #include "drake/solvers/mathematical_program.h"
 #include <Eigen/Dense>

From ab65d4ae64dc30a163456af32cffd04da4aa0c3b Mon Sep 17 00:00:00 2001
From: Pang <pangtao@mit.edu>
Date: Fri, 29 Oct 2021 18:56:03 -0400
Subject: [PATCH 10/10] Remove references to DiffIk in chicken head plan.

---
 src/plans/chicken_head_plan.cc | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/plans/chicken_head_plan.cc b/src/plans/chicken_head_plan.cc
index c5c9f11..cff3c1b 100644
--- a/src/plans/chicken_head_plan.cc
+++ b/src/plans/chicken_head_plan.cc
@@ -3,8 +3,6 @@
 
 using drake::lcmt_robot_state;
 using drake::Vector6;
-using drake::manipulation::planner::DifferentialInverseKinematicsStatus;
-using drake::manipulation::planner::internal::DoDifferentialInverseKinematics;
 using drake::math::RigidTransformd;
 using drake::math::RotationMatrixd;
 using Eigen::Vector3d;