Example #1
0
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()
Example #2
0
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()
def run(joint_index, save_file_base):
    lcm = LCM(robot_lcm_url)
    status_sub = LcmMessagePoll(lcm, status_channel, lcmt_panda_status)
    events = []
    print(f"joint_index: {joint_index}")

    print("Waiting for first message...")
    lcm.handle()

    count = 0
    dt_des = 0.005

    print("Received! Starting")
    t_abs_start = time.time()
    t = 0.0
    t_transient_start = 3.0
    t_transient_end = 0.25

    t_sweep = 10.0
    T_sec_min = 1.0
    T_sec_max = 5.0

    v = None

    try:
        while True:
            lcm_handle_all(lcm)
            status = status_sub.get()
            if status is None:
                continue

            t_prev = t
            t = time.time() - t_abs_start
            s = min(t / t_transient_start, 1.0)
            dt = t - t_prev

            v = np.array(status.joint_velocity)
            assert v.shape == (ndof,), v.shape

            # T_sec = np.array([4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]) / 4
            s_chirp = min(t / t_sweep, 1.0)
            if s_chirp >= 1.0:
                raise KeyboardInterrupt
            T_sec = T_sec_min + (1 - s_chirp) * (T_sec_max - T_sec_min)
            print(T_sec)
            w = 2 * np.pi / T_sec
            v_max = np.zeros(ndof)
            v_max[joint_index] = 1.0
            # v_max = np.array([0.5, 1.5, 0.5, 1.0, 1.0, 1.5, 1.5])

            vd = np.zeros(ndof)
            vd[:] = s * v_max * np.sin(w * t)

            cmd = make_command(vd)
            lcm.publish(cmd_channel, cmd.encode())
            events.append((status, cmd))

            count += 1

            t_next = t + dt_des
            while time.time() - t_abs_start < t_next:
                time.sleep(dt_des / 100)

    except KeyboardInterrupt:
        # Wind down.
        assert v is not None
        v0 = v
        vd = np.zeros(ndof)

        t_abs_start = time.time()
        t = 0.0
        while t < t_transient_end:
            lcm_handle_all(lcm)
            status = status_sub.get()
            if status is None:
                continue

            t = time.time() - t_abs_start
            s = min(t / t_transient_end, 1.0)
            vd = v0 * (1 - s)
            cmd = make_command(vd)
            lcm.publish(cmd_channel, cmd.encode())
            events.append((status, cmd))

            t_next = t + dt_des
            while time.time() - t_abs_start < t_next:
                time.sleep(dt_des / 100)

    finally:
        # Save data.
        file = expanduser(f"~/data/panda/tracking/{save_file_base}.pkl")
        with open(file, "wb") as f:
            pickle.dump(events, f)
        print(f"Wrote: {file}")
Example #4
0
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()