ChickenHead Plan.

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)
+

src/examples/run_chicken_head_plan.py

@@ -0,0 +1,35 @@
+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)
+
+#%%
+# 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, 60.]
+
+msg_plan = lcmt_robot_plan()
+msg_plan.utime = round(time.time() * 1e6)
+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()

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()

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]))
@@ -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.

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);
   }
 }

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;