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 new file mode 100644 index 0000000..5b09ca6 --- /dev/null +++ b/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() 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..a060c97 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])) @@ -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()), control_period_seconds_(config["control_period"].as()) { double t_now_seconds = std::chrono::duration_cast( @@ -50,18 +51,16 @@ void IiwaPlanManager::Run() { } void IiwaPlanManager::CalcCommandFromStatus() { - lcm_status_command_ = std::make_unique(); - auto sub = lcm_status_command_->subscribe( - config_["lcm_status_channel"].as(), - &IiwaPlanManager::HandleIiwaStatus, this); - sub->setQueueCapacity(1); + owned_lcm_ = std::make_unique(); + iiwa_status_sub_ = + std::make_unique>( + owned_lcm_.get(), config_["lcm_status_channel"].as()); + // 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 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( - (*status_msg).joint_position_measured.data(), num_joints), - Eigen::Map( - (*status_msg).joint_velocity_estimated.data(), num_joints), - Eigen::Map((*status_msg).joint_torque_external.data(), + const int num_joints = status_msg.num_joints; + State s(Eigen::Map(status_msg.joint_position_measured.data(), + num_joints), + Eigen::Map(status_msg.joint_velocity_estimated.data(), + num_joints), + Eigen::Map(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 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(), &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 #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 state_machine_; std::unordered_map threads_; std::unique_ptr 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_status_command_; - void CalcCommandFromStatus(); - void HandleIiwaStatus(const lcm::ReceiveBuffer *rbuf, - const std::string &channel, - const drake::lcmt_iiwa_status *status_msg); + std::unique_ptr owned_lcm_; + std::unique_ptr> + 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..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 &context, drake::lcmt_iiwa_command *cmd) const { - const auto &msg_iiwa_status = + const auto &status_msg = get_iiwa_status_input_port().Eval(context); const auto &msg_robot_plan = get_robot_plan_input_port().Eval(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(msg_iiwa_status.joint_position_measured.data(), - msg_iiwa_status.num_joints), - Eigen::Map( - msg_iiwa_status.joint_velocity_estimated.data(), - msg_iiwa_status.num_joints), - Eigen::Map(msg_iiwa_status.joint_torque_external.data(), - msg_iiwa_status.num_joints)); + State s(Eigen::Map(status_msg.joint_position_measured.data(), + status_msg.num_joints), + Eigen::Map(status_msg.joint_velocity_estimated.data(), + status_msg.num_joints), + Eigen::Map(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_->CommandHasError(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]; } } } diff --git a/src/plan_runner_client/zmq_client.py b/src/plan_runner_client/zmq_client.py index 1b61f5b..7d886d9 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,12 +198,13 @@ 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") s = self.abort_client.recv_string() - print(s) + self.iiwa_position_getter.logger.info(s) class SchunkManager: diff --git a/src/plans/CMakeLists.txt b/src/plans/CMakeLists.txt index a5a711c..0e5e83d 100644 --- a/src/plans/CMakeLists.txt +++ b/src/plans/CMakeLists.txt @@ -1,8 +1,9 @@ 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) -target_link_libraries(robot_plans drake::drake) + 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 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.cc b/src/plans/chicken_head_plan.cc new file mode 100644 index 0000000..cff3c1b --- /dev/null +++ b/src/plans/chicken_head_plan.cc @@ -0,0 +1,129 @@ +#include "plans/chicken_head_plan.h" +#include "drake/lcm/lcm_messages.h" + +using drake::lcmt_robot_state; +using drake::Vector6; +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, + const drake::multibody::MultibodyPlant *plant) + : PlanBase(plant), duration_(duration), X_WCd_(X_WL7_0 * X_L7T_), + frame_L7_(plant->GetFrameByName("iiwa_link_7")) { + // DiffIk. + plant_context_ = plant_->CreateDefaultContext(); + solver_ = std::make_unique(); + result_ = std::make_unique(); + + // X_TC subscription. + owned_lcm_ = std::make_unique(); + rpe_sub_ = std::make_unique>( + owned_lcm_.get(), "X_TC"); + sub_thread_ = std::thread(&ChickenHeadPlan::PoseIo, this); +} + +ChickenHeadPlan::~ChickenHeadPlan() { + stop_flag_ = true; + sub_thread_.join(); +} + +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( + {static_cast(Q_WC.w()), static_cast(Q_WC.x()), + static_cast(Q_WC.y()), static_cast(Q_WC.z()), + static_cast(p_WC[0]), static_cast(p_WC[1]), + static_cast(p_WC[2])}); + return msg_X_WC; +} + +void ChickenHeadPlan::PoseIo() const { + while (true) { + rpe_sub_->clear(); + drake::lcm::LcmHandleSubscriptionsUntil(owned_lcm_.get(), [&]() { + return stop_flag_ or rpe_sub_->count() > 0; + }, 10); + 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]); + + drake::math::RigidTransformd X_WC; + { + std::lock_guard 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 lock(mutex_rpe_); + X_WTd = X_WCd_ * X_TC_.inverse(); + // Also Update X_WT_. + X_WT_ = X_WT; + } + + 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 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(); + + plant_->CalcJacobianSpatialVelocity( + *plant_context_, drake::multibody::JacobianWrtVariable::kV, frame_L7_, + X_L7T_.translation(), frame_W, frame_W, &Jv_WTq_W_); + + 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_->is_success()) { + cmd->q_cmd = state.q + result_->GetSolution(dq); + cmd->tau_cmd = Eigen::VectorXd::Zero(nq); + } else { + throw DiffIkException(); + } +} diff --git a/src/plans/chicken_head_plan.h b/src/plans/chicken_head_plan.h new file mode 100644 index 0000000..b902115 --- /dev/null +++ b/src/plans/chicken_head_plan.h @@ -0,0 +1,61 @@ +#include + +#include "plans/plan_base.h" +#include "drake/lcm/drake_lcm.h" +#include "drake/lcmt_robot_state.hpp" +#include "drake/solvers/gurobi_solver.h" +#include "drake/solvers/mathematical_program.h" +#include + +/* + * 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. + * Also publishes X_WC on the LCM channel "X_WC". + */ +class ChickenHeadPlan : public PlanBase { + public: + ChickenHeadPlan(const drake::math::RigidTransformd &X_WL7_0, + double duration, + const drake::multibody::MultibodyPlant *plant); + ~ChickenHeadPlan() override; + + void Step(const State &state, double control_period, double t, + Command *cmd) const override; + double duration() const override {return duration_;}; + private: + 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& 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> plant_context_; + std::unique_ptr solver_; + std::unique_ptr result_; + + // Relative pose estimator subscription. + void PoseIo() const; + std::unique_ptr owned_lcm_; + std::unique_ptr> rpe_sub_; + std::thread sub_thread_; + std::atomic stop_flag_{false}; + mutable std::mutex mutex_rpe_; + mutable drake::math::RigidTransformd X_TC_; + mutable drake::math::RigidTransformd X_WT_; +}; diff --git a/src/plans/iiwa_plan_factory.cc b/src/plans/iiwa_plan_factory.cc index b7059d0..c697f78 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" @@ -17,6 +18,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>(1e-3); auto parser = drake::multibody::Parser(plant_.get()); @@ -33,10 +37,14 @@ std::unique_ptr 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."); } @@ -49,7 +57,7 @@ std::unique_ptr IiwaPlanFactory::MakeJointSpaceTrajectoryPlan( VectorXd t_knots(n_knots); for (int t = 0; t < n_knots; t++) { - t_knots[t] = static_cast(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); } @@ -67,11 +75,8 @@ std::unique_ptr 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(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 t_knots(n_knots); @@ -80,9 +85,9 @@ std::unique_ptr IiwaPlanFactory::MakeTaskSpaceTrajectoryPlan( for (int t = 0; t < n_knots; t++) { // Store time - t_knots[t] = static_cast(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. @@ -105,14 +110,25 @@ std::unique_ptr IiwaPlanFactory::MakeTaskSpaceTrajectoryPlan( const auto &frame_E = plant_->GetFrameByName(config_["robot_ee_body_name"].as()); - vector nominal_joint_vector = - config_["robot_nominal_joint"].as>(); - - Eigen::VectorXd nominal_joint= Eigen::Map( - 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>(); + auto nominal_joint = Eigen::Map( + nominal_joint_vector.data(), nominal_joint_vector.size()); return std::make_unique( std::move(quat_traj), std::move(xyz_traj), X_ET, plant_.get(), frame_E, config_["control_period"].as(), nominal_joint); } + +[[nodiscard]] std::unique_ptr 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>(); + return std::make_unique( + TransformFromRobotStateMsg(msg_plan.plan[0].joint_position), duration, + 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 + static drake::math::RigidTransformd TransformFromRobotStateMsg( + const std::vector& q_xyz); const YAML::Node &config_; [[nodiscard]] std::unique_ptr MakeJointSpaceTrajectoryPlan(const drake::lcmt_robot_plan &msg_plan) const; [[nodiscard]] std::unique_ptr MakeTaskSpaceTrajectoryPlan(const drake::lcmt_robot_plan &msg_plan) const; + [[nodiscard]] std::unique_ptr + MakeChickenHeadPlan(const drake::lcmt_robot_plan &msg_plan) const; }; + +template +drake::math::RigidTransformd IiwaPlanFactory::TransformFromRobotStateMsg( + const std::vector& 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/plan_base.h b/src/plans/plan_base.h index 15c072b..fd5604d 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 -// 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 contact_normal; // in world frame? -}; - struct State { State() = default; State(const Eigen::Ref &q, @@ -23,7 +15,6 @@ struct State { Eigen::VectorXd q; Eigen::VectorXd v; Eigen::VectorXd tau_ext; - std::optional> contact_results; }; struct Command { @@ -35,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 *plant) @@ -48,5 +62,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 const *const plant_{nullptr}; }; diff --git a/src/plans/task_space_trajectory_plan.cc b/src/plans/task_space_trajectory_plan.cc index 0251929..90c3406 100644 --- a/src/plans/task_space_trajectory_plan.cc +++ b/src/plans/task_space_trajectory_plan.cc @@ -1,15 +1,30 @@ #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 quat_traj, + drake::trajectories::PiecewisePolynomial xyz_traj, + drake::math::RigidTransformd X_ET, + const drake::multibody::MultibodyPlant *plant, + const drake::multibody::Frame &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,38 +34,29 @@ 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 V_WT_desired = - ComputePoseDiffInCommonFrame( - X_WT.GetAsIsometry3(), X_WT_desired.GetAsIsometry3()) / - params_->get_timestep(); - - MatrixX 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); + ComputePoseDiffInCommonFrame(X_WT.GetAsIsometry3(), + X_WT_desired.GetAsIsometry3()) / + params_->get_timestep(); + 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(V_WT_desired), *params_); // 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. - std::cout << "DoDifferentialKinematics Failed to find a solution." - << std::endl; + 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/task_space_trajectory_plan.h b/src/plans/task_space_trajectory_plan.h index b7e2520..7f6e067 100644 --- a/src/plans/task_space_trajectory_plan.h +++ b/src/plans/task_space_trajectory_plan.h @@ -19,19 +19,8 @@ class TaskSpaceTrajectoryPlan : public PlanBase { drake::trajectories::PiecewisePolynomial xyz_traj, drake::math::RigidTransformd X_ET, const drake::multibody::MultibodyPlant *plant, - const drake::multibody::Frame &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), nominal_joint_position_(nominal_joint_position) { - - 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 &frame_E, double control_time_step, + Eigen::VectorXd nominal_joint_position); ~TaskSpaceTrajectoryPlan() override = default; @@ -46,11 +35,9 @@ class TaskSpaceTrajectoryPlan : public PlanBase { const drake::trajectories::PiecewiseQuaternionSlerp quat_traj_; const drake::trajectories::PiecewisePolynomial xyz_traj_; const drake::math::RigidTransformd X_ET_; - const Eigen::VectorXd nominal_joint_position_; // frame of end-effector body + offset. const drake::multibody::Frame &frame_E_; - std::unique_ptr> plant_context_; std::unique_ptr< drake::manipulation::planner::DifferentialInverseKinematicsParameters> diff --git a/src/state_machine/plan_manager_state_machine.cc b/src/state_machine/plan_manager_state_machine.cc index e0fde0a..c492587 100644 --- a/src/state_machine/plan_manager_state_machine.cc +++ b/src/state_machine/plan_manager_state_machine.cc @@ -48,23 +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; + const double dq_norm = (state.q - cmd.q_cmd).norm(); + if (dq_norm > q_threshold) { + throw TooFarAwayException(); + } +} - 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()); +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 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(t_now_seconds); } -inline bool PlanManagerStateMachine::CommandHasError(const State &state, - const Command &cmd) { - return state_->CommandHasError(state, cmd, this, - config_["q_threshold"].as()); +inline void PlanManagerStateMachine::CheckCommandForError(const State &state, + const Command &cmd) { + state_->CheckCommandForError(state, cmd, this, + config_["q_threshold"].as()); +} + +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 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 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;