class JsonManipulatorTrajRunner: def __init__(self, filename): with open(filename) as fp: self.plan = json.load(fp) self._lcm = LCM() self._lcm.subscribe("EXECUTION_STATUS", self._kuka_plan_runner_status_handler) self.wsg_utime = 0 self.torque_mode = 0 self.cur_node_id = 0 self.plan_started = False self.node_completed = False def run_plan(self): if len(self.plan): print("Node", self.cur_node_id) self._publish_node_traj(self.plan[self.cur_node_id]) while True: self._lcm.handle() if self.node_completed: self.node_completed = False self.cur_node_id += 1 if self.cur_node_id >= len(self.plan): print("Plan Completed") break print("Node", self.cur_node_id) self._publish_node_traj(self.plan[self.cur_node_id]) def _publish_node_traj(self, node): msg = dict_to_lcmt_manipulator_traj(node) if USE_TORQUE: msg.dim_torques = 7 else: msg.dim_torques = 0 if msg.n_time_steps: self._lcm.publish("COMMITTED_ROBOT_PLAN", msg.encode()) print("Trajectory Published!") def _kuka_plan_runner_status_handler(self, channel, msg): print("Kuka Traj Completed") self.node_completed = True def run_plan_lcm_wrapper(self): self._lcm.subscribe("START_PLAN", self._start_plan_handler) while (not self.plan_started): self._lcm.handle() def _start_plan_handler(self, channel, msg): self.plan_started = True print("Starting plan") self.run_plan()
class ConveyorBeltManipReactiveMotionPlanRunner(BasicManipMotionPlanRunner): def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = ConveyorBeltManipReactiveQuery(geo_setup_file) self._node = None with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def update_object_state(self, object_state): self._query.update_object_state(object_state)
class ReactiveMultiWPStaionaryManipMotionPlanRunner(MultiWPStaionaryManipMotionPlanRunner): def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = StationaryMultiWPManipReactiveQuery(geo_setup_file) self._node = None self._traj_perturbed = False with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def update_object_state(self, object_state): self._query.update_object_state(object_state)
def lcm_mode(): import_lcm_python() lc = LCM() subscription = lc.subscribe('VALVE', valve_handler) print 'Subscribed to the VAVLE channel, waiting for command...' while True: lc.handle()
class MultiWPStaionaryManipMotionPlanRunner(BasicManipMotionPlanRunner): def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = StationaryMultiWPManipQuery(geo_setup_file) self._node = None self._traj_perturbed = False with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def _generate_move_query(self, node, op_name): """ Generates Multi WP move query for traj op Args: node: tree search node op_name: (string) name of operation Returns: msg: (lcmt_multi_wp_manip_query) """ if node.parent_traj: q_prev = node.parent_traj["states"][-1] gripper_width_prev = node.parent_traj["gripper_width"][-1] else: q_prev = self._traj_setup["init_iiwa_pos"] gripper_width_prev = self._traj_setup["gripper_open_width"] if node.parent is None: print("q_prev:", q_prev) input("") print("Parent EE:", node.parent.final_ee) if (node.parent.final_ee is not None) and ( "init_offset_wp" in self._traj_setup[op_name]): # self._traj_setup[op_name]["init_offset_wp"] is not None): print("Adding lift waypoint") print("Lift:", self._traj_setup[op_name]["init_offset_wp"]) wp = (np.array(node.parent.final_ee) + np.array(self._traj_setup[op_name]["init_offset_wp"])).tolist() ee_desired_list = [wp] time_horizon_list = [self._traj_setup[op_name]["init_offset_time"]] constrain_orientation_list = [self._traj_setup[op_name]["init_offset_constrain_orientation"]] else: ee_desired_list = [] time_horizon_list = [] constrain_orientation_list = [] ee_desired_list.extend(self._query.get_desired_ee_pos(node)) node.final_ee = ee_desired_list[-1] time_horizon_list.extend(self._traj_setup[op_name]["time_horizon"]) node.time = node.parent.time + sum(time_horizon_list) constrain_orientation_list.extend(self._traj_setup[op_name]["constrain_orientation"]) print("Prev q:", q_prev) print("DESIRED EE:", ee_desired_list) msg = lcmt_multi_wp_manip_query() msg.name = node.action.name[1:-1].split(" ")[0] msg.dim_q = 7 msg.n_wp = len(ee_desired_list) msg.option = node.option ################################################################################ # Setup false bypass ik msg.bypass_ik = False msg.q_goal = [] for i in range(msg.n_wp): msg.q_goal.append([0 for j in range(msg.dim_q)]) ################################################################################ if node.option=="ddp": msg.time_step = self._traj_setup["ddp_time_step"] elif node.option=="admm": msg.time_step = self._traj_setup["admm_time_step"] msg.time_horizon = time_horizon_list msg.prev_gripper_width = gripper_width_prev msg.desired_ee = ee_desired_list msg.prev_q = q_prev msg.gripper_force = self._traj_setup["gripper_force"] msg.constrain_orientation = constrain_orientation_list ################################################################################ # set object states op = node.action.name[1:-1] if (node.parent.final_ee is not None and (op.split(" ")[0]=="move-to-free-table" or op.split(" ")[0]=="move-to-goal-table")): object_name = op.split(" ")[2] vel_obj = [0 for _ in range(6)] xyz_iiwa = [-0.2, 0, 0] msg.use_object = True q_obj_init = self._query.get_q_object_init(object_name, node.parent.time, xyz_iiwa) q_obj_list = [] for i in range(msg.n_wp): if i == 0: ee_init = node.parent.final_ee q_init = q_obj_init else: ee_init = msg.desired_ee[i-1] q_init = q_obj_list[i-1] ee_final = msg.desired_ee[i] q_obj_list.append(self._query.get_q_object_final(q_init, ee_init, ee_final)) msg.q_init_object = [] msg.q_init_object.extend(q_obj_init) msg.q_init_object.extend(vel_obj) msg.q_desired_object = [] for i in range(msg.n_wp): q_obj_final = [] q_obj_final.extend(q_obj_list[i]) q_obj_final.extend(vel_obj) msg.q_desired_object.append(q_obj_final) print("q_init_object", msg.q_init_object) print("q_desired_object", msg.q_desired_object) else: msg.use_object = False msg.q_init_object = [0 for _ in range(13)] msg.q_desired_object = [] for i in range(msg.n_wp): msg.q_desired_object.append([0 for _ in range(13)]) ################################################################################ return msg
class MotionQueryHandler: def __init__(self, query_file, ddp_traj_file): with open(query_file) as fp: data = json.load(fp) self._queries = [] for d in data: try: self._queries.append(dict_to_lcmt_multi_wp_manip_query(d)) except: self._queries.append(None) with open(ddp_traj_file) as fp: self._ddp_traj = json.load(fp) self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._query_results_handler) self._cur_query = None self._completed = False self.trajs = [] self.q_idx = 0 def _query_results_handler(self, channel, msg): print("message received.") data = lcmt_manipulator_traj.decode(msg) print("Cost:", data.cost) if not data.n_time_steps: print("IK Infeasible") elif data.cost == float('inf') or math.isnan( data.cost) or data.cost <= 1: if self._cur_query.time_step / 2 >= MIN_TIME_STEP: self._cur_query.time_step /= 2 self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("Decreasing time step to", self._cur_query.time_step) return else: print("Minimum time step reached. Trajectory cannot be found.") self.trajs.append(self._ddp_traj[self.q_idx]) self._completed = True return self.trajs.append(lcmt_manipulator_traj_to_dict(data)) self._completed = True def Run(self): self.q_idx = 0 total_time = 0 for self.q_idx in range(len(self._queries)): self._cur_query = self._queries[self.q_idx] if self._cur_query is None: print( "No move query in this action. Keeping original trajectory" ) self.trajs.append(self._ddp_traj[self.q_idx]) continue if self._cur_query.time_step > MAX_TIME_STEP: self._cur_query.time_step = MAX_TIME_STEP print("Computing ADMM " + str(self.q_idx + 1) + "/" + str(len(self._queries))) print("Action: " + self._cur_query.name) self._completed = False start_time = time.time() self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("LCM Query Published...") while not self._completed: self._lcm.handle() end_time = time.time() total_time += end_time - start_time print("Computation time:", end_time - start_time) print("Total computation time:", total_time) self._save_trajs() def run_single_traj(self, q_idx): self._cur_query = self._queries[q_idx] print("Action: " + self._cur_query.name) if self._cur_query.time_step > MAX_TIME_STEP: self._cur_query.time_step = MAX_TIME_STEP if self._cur_query is None: print("No move query in this action. Keeping original trajectory") self.trajs.append(self._ddp_traj[q_idx]) else: print("Computing ADMM " + str(q_idx + 1) + "/" + str(len(self._queries))) self._completed = False start_time = time.time() self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("LCM Query Published...") while not self._completed: self._lcm.handle() self._save_trajs() def run_action_names(self, action_name): self.q_idx = 0 total_time = 0 for self.q_idx in range(len(self._queries)): if self._queries[self.q_idx] is not None and self._queries[ self.q_idx].name.startswith(action_name): self._cur_query = self._queries[self.q_idx] else: self._cur_query = None if self._cur_query is None: print( "No move query in this action. Keeping original trajectory" ) self.trajs.append(self._ddp_traj[self.q_idx]) continue if self._cur_query.time_step > MAX_TIME_STEP: self._cur_query.time_step = MAX_TIME_STEP print("Computing ADMM " + str(self.q_idx + 1) + "/" + str(len(self._queries))) print("Action: " + self._cur_query.name) self._completed = False start_time = time.time() self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("LCM Query Published...") while not self._completed: self._lcm.handle() end_time = time.time() total_time += end_time - start_time print("Computation time:", end_time - start_time) print("Total computation time:", total_time) self._save_trajs() def _save_trajs(self): print("ADMM completed, saving results") filename = "/home/zhigen/code/drake/manipulation_tamp/results/admm_traj" + datetime.now( ).strftime("%Y%m%dT%H%M%S") + ".json" with open(filename, 'w') as out: json.dump(self.trajs, out)
class BasicManipMotionPlanRunner(object): """ Runs Motion Plan for conveyor belt domain """ def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = ConveyorBeltManipQuery(geo_setup_file) self._node = None self._traj_perturbed = False with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def _results_handler(self, channel, msg): print("message received.") data = lcmt_manipulator_traj.decode(msg) print("Cost:", data.cost) if not data.n_time_steps: print("IK Infeasible") elif data.cost == float('inf') or math.isnan( data.cost) or data.cost == 0: if self._node.move_query.time_step / 2 >= self._traj_setup[ "min_time_step"]: self._node.move_query.time_step /= 2 self._lcm.publish("TREE_SEARCH_QUERY", self._node.move_query.encode()) print("Decreasing time step to", self._node.move_query.time_step) return # ans = input("Trajectory not found. Try Again? [yes/no]") # while not (ans == "yes" or ans == "no"): # ans = input("Please input yes or no...") # if ans =="yes": # self._lcm.publish("TREE_SEARCH_QUERY", self._node.move_query.encode()) # print("Decreasing time step to", # self._node.move_query.time_step) # return # elif ans == "no": # print("Traj Op terminated by user. Trajectory cannot be found.") elif not self._traj_perturbed: self._node.move_query.time_step = self._traj_setup[ "admm_time_step"] self._perturb_desired_ee(self._node, sigma=0.005) self._lcm.publish("TREE_SEARCH_QUERY", self._node.move_query.encode()) print("Perturbed desired ee, reset time step") return else: print("Minimum time step reached. Trajectory cannot be found.") self.results = lcmt_manipulator_traj_to_dict(data) self.completed = True def run(self, node): """ returns results of query for conveyor belt manipulation domain Args: node: (PddlTampNode) Returns: results: (Dict) containing trajectory info """ self._node = node op_name = node.action.name[1:-1].split(" ")[0] obj_name = node.action.name[1:-1].split(" ")[2] if node.parent is not None: node.processed_object = node.parent.processed_object.copy() node.discrete_cost = node.parent.discrete_cost if obj_name not in node.processed_object: node.processed_object.append(obj_name) if "base_object_cost" in self._traj_setup: obj_name = node.action.name[1:-1].split(" ")[2] alpha = self._traj_setup["base_object_cost"]["alpha"] base_obj_cost = self._traj_setup["base_object_cost"][obj_name] discrete_obj_cost = base_obj_cost * (alpha**len( node.processed_object)) node.discrete_cost += discrete_obj_cost print("Adding New discrete cost", obj_name, discrete_obj_cost) print("Total discrete cost", node.discrete_cost) self.completed = False if self._traj_setup[op_name]["traj_op"]: if node.move_query is None: node.move_query = self._generate_move_query(node, op_name) self._lcm.publish("TREE_SEARCH_QUERY", node.move_query.encode()) print("LCM Query Published...") else: if isinstance(self._traj_setup[op_name]["time_horizon"], list): node.time = node.parent.time + sum( self._traj_setup[op_name]["time_horizon"]) else: node.time = node.parent.time + self._traj_setup[op_name][ "time_horizon"] self.results = self._generate_traj(node, op_name) self.completed = True timeout = 5000 while (True): if self.completed: return self.results rfds, wfds, efds = select.select([self._lcm.fileno()], [], [], timeout) if rfds: self._lcm.handle() def _perturb_desired_ee(self, node, sigma=0.01): if node.move_query is None: print("Warning: no move query available") return if isinstance(node.move_query.desired_ee, list): for ee in node.move_query.desired_ee: for element in ee: element += random.gauss(0, sigma) else: for element in node.move_query.desired_ee: element += random.gauss(0, sigma) self._traj_perturbed = True def _generate_move_query(self, node, op_name): if node.parent_traj: q_prev = node.parent_traj["states"][-1] gripper_width_prev = node.parent_traj["gripper_width"][-1] else: q_prev = self._traj_setup["init_iiwa_pos"] gripper_width_prev = self._traj_setup["gripper_open_width"] if node.parent is None: print("q_prev:", q_prev) input("") ee_desired = self._query.get_desired_ee_pos(node) print("Prev q:", q_prev) print("DESIRED EE:", ee_desired) msg = lcmt_motion_plan_query() msg.name = node.action.name[1:-1].split(" ")[0] msg.dim_q = 7 if node.option == "ddp": msg.level = 1 msg.time_step = self._traj_setup["ddp_time_step"] elif node.option == "admm": msg.level = 2 msg.time_step = self._traj_setup["admm_time_step"] msg.wait_time = self._traj_setup[op_name]["wait_time"] msg.time_horizon = self._traj_setup[op_name]["time_horizon"] msg.prev_gripper_width = gripper_width_prev msg.desired_ee = ee_desired msg.prev_q = q_prev msg.gripper_force = self._traj_setup["gripper_force"] node.final_ee = ee_desired return msg def _generate_traj(self, node, op_name): if isinstance(self._traj_setup[op_name]["time_horizon"], list): time_horizon = sum(self._traj_setup[op_name]["time_horizon"]) else: time_horizon = self._traj_setup[op_name]["time_horizon"] node.final_ee = node.parent.final_ee time_step = self._traj_setup["manual_time_step"] if self._traj_setup[op_name]["gripper_open"] < 0: gripper_width = self._traj_setup["gripper_close_width"] elif self._traj_setup[op_name]["gripper_open"] > 0: gripper_width = self._traj_setup["gripper_open_width"] else: if node.parent_traj: gripper_width = node.parent_traj["gripper_width"][-1] else: gripper_width = self._traj_setup["init_gripper_width"] msg = dict() msg["n_time_steps"] = int(time_horizon / time_step) msg["dim_torques"] = 0 msg["dim_states"] = self._traj_setup["n_joints"] msg["cost"] = self._traj_setup[op_name]["cost"] cur_time = 0 msg["times_sec"] = [] msg["states"] = [] msg["torques"] = [] msg["gripper_width"] = [] msg["gripper_force"] = [] if node.parent_traj: q = node.parent_traj["states"][-1] else: q = self._traj_setup["init_iiwa_pos"] for _ in range(msg["n_time_steps"]): msg["states"].append(q) msg["torques"].append([]) msg["gripper_width"].append(gripper_width) msg["gripper_force"].append(self._traj_setup["gripper_force"]) msg["times_sec"].append(cur_time) cur_time += time_step return msg
def handle_lcm(): lc = LCM() subscription = lc.subscribe('ARM', arm_handler) print 'Subscribed to the ARM channel, waiting for command...' while True: lc.handle()