示例#1
0
 def __convert_to_name_vector(self, name_list):
     name_vector = NameVector()
     for name in name_list:
         name_vector.append(name)
     return name_vector
示例#2
0
class SuhanMotionPlannerManager:
    def __init__(self, argv, is_real=False):
        self.working_directory = '/home/dyros/suhan_motion_planning'
        self.debug_file_prefix = self.working_directory + '/logs/'
        self.data_file_prefix = self.working_directory + '/database/'
        self.is_real = is_real  # True #to skip 'is_initial_joint_updated' when not using real robot
        self.real_arm_num = 3
        self.arm_num = 3
        moveit_commander.roscpp_initialize(argv)
        self.is_initial_joint_updated = [
            False, False, False, False, False, False, False, False, False,
            False, False, False, False, False, False, False, False, False,
            False, False, False
        ]
        self.planner = SuhanMotionPlanner()
        self.planner.set_debug_file_prefix(self.debug_file_prefix)
        self.max_velocity_scaling_factor = 0.2  #0.9
        self.max_acceleration_scaling_factor = 1.0  #0.35
        self.need_to_update_current_state = True
        self.run_fast = True
        self.obj_list = []

        self.joint_states = np.zeros(7 * self.arm_num)
        if self.real_arm_num == 2:
            self.joint_states[7 * 2:7 * 3] = np.array(
                [0, 0, 0, -pi / 3, 0, pi / 3, pi / 4])
            self.is_initial_joint_updated[7 * 2:7 * 3] = [
                True, True, True, True, True, True, True
            ]
        self.joint_subscriber = rospy.Subscriber('/panda_dual/joint_states',
                                                 JointState,
                                                 self.__joint_state_callback)
        # TF
        self.listener = TransformListener()
        self.transformer = TransformerROS()
        self.broadcaster = TransformBroadcaster()
        # Scene Object lists
        # TODO: add objects
        # self.object_data_dict = {'pin_1':[mesh_url, grasp_url, pos, quat],}
        self.object_lists = ['pin_1', 'pin_2']
        self.last_object_pose = {}
        self.arm_names = ['panda_left', 'panda_right', 'panda_top']
        arm = NameMap()
        obj = NameMap()
        arm['panda_left'] = 0
        arm['panda_right'] = 1
        arm['panda_top'] = 2
        obj['dumbbell'] = 0
        self.joint_map = {}
        for i in range(7):
            for name in self.arm_names:
                self.joint_map[name + '_joint' +
                               str(i + 1)] = i + 7 * arm[name]
        print(self.joint_map)

        self.hand_touch_links = {}
        self.hand_touch_links['panda_left'] = NameVector()
        self.hand_touch_links['panda_right'] = NameVector()
        self.hand_touch_links['panda_top'] = NameVector()
        self.hand_touch_links['panda_left'].append('panda_left_hand')
        self.hand_touch_links['panda_left'].append('panda_left_leftfinger')
        self.hand_touch_links['panda_left'].append('panda_left_rightfinger')
        self.hand_touch_links['panda_left'].append('panda_left_link7')
        self.hand_touch_links['panda_left'].append('panda_left_link8')
        self.hand_touch_links['panda_right'].append('panda_right_hand')
        self.hand_touch_links['panda_right'].append(
            'panda_right_finger_left_link')
        self.hand_touch_links['panda_right'].append(
            'panda_right_finger_right_link')
        self.hand_touch_links['panda_right'].append('panda_right_link7')
        self.hand_touch_links['panda_right'].append('panda_right_link8')
        self.hand_touch_links['panda_top'].append('panda_top_hand')
        self.hand_touch_links['panda_top'].append('panda_top_finger_left_link')
        self.hand_touch_links['panda_top'].append(
            'panda_top_finger_right_link')
        self.hand_touch_links['panda_top'].append('panda_top_link7')
        self.hand_touch_links['panda_top'].append('panda_top_link8')

        self.collision_allowed_ids = NameVector()
        self.collision_allowed_ids.append('ikea_stefan_side_left_0')

        eye = np.array([0, 0, 0, 1])
        reversed_eye = np.array([1, 0, 0, 0])
        q_arm = np.array([
            0, 0, 0, -pi / 3, 0, pi / 3, pi / 4, 0, 0, 0, -pi / 3, 0, pi / 3,
            pi / 4, 0, 0, 0, -pi / 3, 0, pi / 3, pi / 4
        ])
        ee_pos = np.array([0, 0, 0.103])
        # ee_pos_R = np.array([0, 0, 0.083])
        ee_pos_R = np.array([0, 0, 0.078])  # for test 11-12 khc
        ee_quat = np.array([0, 0, 0, 1])

        # planner needs to be initialized
        self.planner.set_environment(arm, obj)
        self.planner.set_sigma(0.4)
        self.planner.set_max_ik_trials(10000)
        self.planner.set_max_planning_time(60)
        self.planner.set_sample_rviz(True)

        self.planner.set_robot_transform('panda_left',
                                         np.array([0, 0.3, 1.006]), eye)
        self.planner.set_robot_transform('panda_right',
                                         np.array([0, -0.3, 1.006]), eye)
        self.planner.set_robot_transform('panda_top',
                                         np.array([1.35, 0.3, 1.006]),
                                         np.array([0, 0, 1, 0]))

        # pos, quat = self.__convert_to_vec_quat(T_DXL_CTR)
        # print (pos, quat)

        self.planner.set_end_effector_frame('panda_left', ee_pos, ee_quat)
        self.planner.set_end_effector_frame('panda_right', ee_pos_R, ee_quat)
        self.planner.set_end_effector_frame('panda_top', ee_pos_R, ee_quat)
        self.planner.set_end_effector_margin('panda_left',
                                             np.array([0, 0, 0.03]), eye)
        self.planner.set_end_effector_margin('panda_right',
                                             np.array([0, 0, 0.03]), eye)
        self.planner.set_end_effector_margin('panda_top',
                                             np.array([0, 0, 0.03]), eye)
        self.long_planner = 'rrt_connect'  #
        self.planner.set_chomp_postprocess(False)
        # self.long_planner = 'rrt_connect' #
        # self.planner.set_planner('ait_star')

        # Calib dasta with closed chain calibration with 2.09e-5 eval
        calib_dh_left = np.array(
            [[
                0.000474560429981023, 0.000483166682165302,
                -0.00304883355188383, 0.00148667086907321
            ],
             [
                 -3.69828865924539e-05, -0.000288069909956647,
                 -0.00812428761844092, 0.00421567136144437
             ],
             [
                 -0.000154357131719552, -0.0010921364777817,
                 0.00031894496234845, -0.0030474925191138
             ],
             [
                 -0.000117600404870226, 0.000712982958085577,
                 -0.00571261767823764, 0.00176867969486185
             ],
             [
                 -0.00058993701921134, -0.000326649645213642,
                 0.00939394386245098, 0.00123723772258799
             ],
             [
                 -0.000433705606644922, -0.000293762477507038,
                 -0.0156742348127345, -0.00529320945025931
             ],
             [
                 -0.000589815315429364, 6.2389274666678e-05,
                 0.0291501803388187, 0.00113202442328629
             ]])

        calib_dh_right = np.array(
            [[
                -0.00153055068483914, -0.000772485919009139,
                -0.00374187596482555, -0.00183369895482027
            ],
             [
                 0.000163834922530543, -0.000212890734425727,
                 -0.0041768339596184, 0.00292223805919776
             ],
             [
                 -2.60271767179549e-05, -0.00100930869860036,
                 0.00208915153420725, -0.00362801030623702
             ],
             [
                 0.0002126348171224, 0.00108679894872676, 9.81965015663654e-05,
                 0.00292912798333623
             ],
             [
                 -0.00136529072609946, -5.62799006356164e-05,
                 0.0139799357258803, -0.00122374898174726
             ],
             [
                 -0.000502587880406569, 0.000336192838117574,
                 -0.0139808833528828, -0.00268675457630875
             ],
             [
                 -0.00104647166032287, 0.000135297170834114,
                 0.00498364620994882, 0.000349775306996936
             ]])

        calib_dh_top = np.array(
            [[
                -0.000171539356569936, 0.000591551783574421,
                0.00119525002639617, -0.00650097874689066
            ],
             [
                 -0.000330274824097498, -0.000124214443868683,
                 7.10962210595555e-05, 0.00166835357174836
             ],
             [
                 -0.00027921463294522, -0.000683991542242173,
                 0.00203760570771006, -0.0018819778569208
             ],
             [
                 -0.000355247972007034, 0.000891508331427601,
                 -0.00513318787489872, 0.000168196477584221
             ],
             [
                 0.000190817101859644, -0.000635118518697479,
                 0.00445732420517835, -0.00167223312645046
             ],
             [
                 -0.000244350284995939, -0.000209543585115151,
                 0.000118913154576129, -0.00356076901549127
             ],
             [
                 -0.000484192538997231, 4.41640702632187e-05,
                 -0.0155483388661319, 0.000602582057747216
             ]])

        self.planner.set_calibration_data('panda_left', calib_dh_left)
        self.planner.set_calibration_data('panda_right', calib_dh_right)
        self.planner.set_calibration_data('panda_top', calib_dh_top)
        # let's add some collision meshes for the objects
        # Note: Use URL (package:// ... )
        # also, grasp candidates should be given.
        # Note: YOU SHOULD USE PATH NOT URL (use get_path_from_url function)
        # TODO: use this

        self.planner.add_box(np.array([0.65, 1.0, 0.2]), 'table',
                             np.array([0.65, 0.0, 1.1]), np.array([0, 0, 0,
                                                                   1]))
        self.obj_list.append('table')
        self.planner.add_box(np.array([0.5, 0.08, 0.1]), 'obstacle',
                             np.array([0.65, 0.1, 1.3]), np.array([0, 0, 0,
                                                                   1]))
        self.obj_list.append('obstacle')

        # for the first state
        self.planner.set_start_arm_states(q_arm)
        # self.planner.print_start_state()
        # for the visualization
        # q_arm[7:14] = np.array([-0.516055, 0.377285, 0.567762, -2.48491, -2.54167, 1.81389, 2.01595])
        self.planner.update_arm_states(q_arm)
        # print('q_arm: ', q_arm)
        # self.planner.print_current_collision_infos()
        # self.planner.enable_tf_broadcasting()

        self.planning_scene = self.planner.get_planning_scene_collision_check()
        self.ik_task = self.planner.get_ik_task()
        self.object_ik_task = self.planner.get_object_ik_task()
        self.assembly_ik_task = self.planner.get_assembly_ik_task()
        self.dual_arm_task = self.planner.get_dual_arm_task()
        self.dual_arm_task.set_delta(0.25)
        # self.dual_arm_task.set_range(0.5)

        self.dual_arm_task.set_max_trials(10000)
        self.dual_arm_task.set_cost_limit(100.0)
        self.dual_arm_task.set_enable_sample_viz(True)
        self.triple_ik_task = self.planner.get_triple_ik_task()
        self.orientation_constrained_task = self.planner.get_orientation_constrained_task(
        )

        self.orientation_constrained_task.set_enable_sample_viz(False)
        self.orientation_constrained_task.set_cost_limit(7.0)

        self.ik_task.set_grad_rate(5e-2)
        self.ik_task.set_grad_stop_threshold(3e-2)
        self.ik_task.set_max_opt_trials(20)
        self.ik_task.set_cost_limit(5.0)
        self.ik_task.set_enable_sample_viz(True)

        self.object_ik_task.set_grad_rate(5e-2)
        self.object_ik_task.set_grad_stop_threshold(2e-2)
        self.object_ik_task.set_max_trials(10000)
        self.object_ik_task.set_max_opt_trials(20)
        self.object_ik_task.set_cost_limit(7.0)
        self.object_ik_task.set_enable_sample_viz(True)

        self.assembly_ik_task.set_grad_rate(5e-2)
        self.assembly_ik_task.set_grad_stop_threshold(2e-2)
        self.assembly_ik_task.set_max_opt_trials(20)
        self.assembly_ik_task.set_cost_limit(5.0)
        self.assembly_ik_task.set_enable_sample_viz(True)

        self.triple_ik_task.set_grad_rate(5e-2)
        self.triple_ik_task.set_grad_stop_threshold(2e-2)
        self.triple_ik_task.set_max_opt_trials(20)
        self.triple_ik_task.set_cost_limit(8.0)
        self.triple_ik_task.set_enable_sample_viz(True)

        self.controller_not_working = False

        self.start_q_threshold = 0.087266444  # rad

        if self.is_real:
            is_completed = False
            while is_completed is False and rospy.is_shutdown() is False:
                is_completed = True
                for value in self.is_initial_joint_updated:
                    if value is False:
                        is_completed = False
                print(
                    'waiting for joint states. please use SuhanMotionPlannerManager(sys.argv, is_real=False) if you are using this in just sim mode.'
                )
                rospy.sleep(0.1)
        else:
            self.joint_states = np.array([
                0, 0, 0, -pi / 3, 0, pi / 3, pi / 4, 0, 0, 0, -pi / 3, 0,
                pi / 3, pi / 4, 0, 0, 0, -pi / 3, 0, pi / 3, pi / 4
            ])
        # else:
        #     self.joint_states = np.array([])

    def __del__(self):
        self.save_ik_solutions()
        moveit_commander.roscpp_shutdown()

    def __joint_state_callback(self, data):
        for i in range(0, len(joint_names)):
            for j in range(0, len(data.name)):
                if joint_names[i] == data.name[j]:
                    self.joint_states[i] = data.position[j]
                    self.is_initial_joint_updated[i] = True
        # print(self.joint_states)

    def __robot_error_callback(self, subscriber):
        # print("test_subscribe")
        if subscriber.data is True:
            self.controller_not_working = True

    def __update_current_state(self):
        # TODO: implement this code (insepct TF, /panda_dual/joint_states)
        # Noise option (?)
        self.planner.update_arm_states(self.joint_states)
        # self.planner.publish_planning_scene_msg()

        if self.need_to_update_current_state is False:
            return
        self.planner.set_start_arm_states(self.joint_states)
        for col_object in self.object_lists:
            # get obj pose
            # pos = ?
            # quat = ?
            # self.planner.update_object_states(col_object, pos, quat)
            # self.planner.set_start_object_states(col_object, pos, quat)
            pass
        pass
        self.print_current_collision_info()

    def __convert_to_name_vector(self, name_list):
        name_vector = NameVector()
        for name in name_list:
            name_vector.append(name)
        return name_vector

    def __convert_to_param_vector(self, param_list):
        param_vector = ParamVector()
        for param in param_list:
            param_vector.append(param)
        return param_vector

    def __convert_to_vec_quat(self, matrix):
        pos = np.empty(3)
        pos[0:3] = matrix[0:3, 3].T
        quat = quaternion_from_matrix(matrix)
        return pos, quat

    def __load_pickles(self, name):
        if name == None:
            return None
        file_name = self.data_file_prefix + name + '.pkl'
        if os.path.isfile(file_name):
            with open(file_name, 'rb') as f:
                return pickle.load(f)
        else:
            return None

    def __path_to_trajectory(self, path):
        self.planner.set_solved_path(path)
        self.planner.time_parameterize(self.max_velocity_scaling_factor,
                                       self.max_acceleration_scaling_factor)
        trajectory = to_trajectory_msgs(self.planner)
        self.reset_collision_all_arm()
        return trajectory

    def __process_pickles(self, path):
        if path is None:
            raise RuntimeError(
                'Why did you call me even if the path is empty? Please modify the codes'
            )

        start_q = path[0]
        diff = np.linalg.norm(self.joint_states - start_q)
        print('process_pickle - diff:', diff)
        if (diff < self.start_q_threshold):
            self.planner.add_start_to_path(self.joint_states)
            new_path = np.concatenate(([self.joint_states], path), axis=0)
            return self.__path_to_trajectory(new_path)

        print(
            'the initial q of the given joint path is too far away from the current state'
        )
        print('self.joint_states', self.joint_states)
        print('start_q', start_q)

        print('try to plan')
        for i in range(5):
            print(' - try', i + 1)
            r = self.planner.solve_for_joint_pose_all(start_q)
            if r == True:
                print('okay we planner the safe motion to the start')
                path_pre = self.planner.get_solved_path()
                new_path = np.concatenate((path_pre, path), axis=0)

                return self.__path_to_trajectory(new_path)

        raise RuntimeError('plan failed (cannot move to saved pickle)')

    def __save_pickles(self, name, data):
        if name == None:
            return

        with open(self.data_file_prefix + name + '.pkl', 'wb') as f:
            pickle.dump(data, f)

    def __check_safe_diff(self, q1, q2):
        d = LA.norm(q1 - q2)
        if d > 0.1:
            return False
        return True

    def __set_planner_for_coarse_motion(self, name):
        if name is None or self.run_fast:
            self.planner.set_planner('rrt_connect')
        else:
            self.planner.set_planner(self.long_planner)

    def check_robot_error(self):
        self.controller_not_working = False
        sub_once = None
        sub_once = rospy.Subscriber('/panda_dual/panda_left/has_error', Bool,
                                    self.__robot_error_callback, sub_once)
        sub_once = None
        sub_once = rospy.Subscriber('/panda_dual/panda_right/has_error', Bool,
                                    self.__robot_error_callback, sub_once)
        sub_once = None
        sub_once = rospy.Subscriber('/panda_dual/panda_top/has_error', Bool,
                                    self.__robot_error_callback, sub_once)
        rospy.sleep(0.05)
        # sub_once.unregister()
        if self.controller_not_working:
            print('-------------------------------------------------')
            print('controller not working ... attempting UnlockError')
            print('-------------------------------------------------')
            UnlockErrorSM().execute()
            rospy.sleep(0.7)

    def update_joint_states(self):
        self.planner.update_arm_states(self.joint_states)
        self.planner.set_start_arm_states(self.joint_states)

    def display_joint_and_env(self):
        while rospy.is_shutdown() is False:
            self.planner.update_arm_states(self.joint_states)
            self.planner.set_start_arm_states(self.joint_states)
            self.planner.publish_planning_scene_msg()
            rospy.sleep(0.1)

    def display_joint_and_env_given(self, joint):
        while rospy.is_shutdown() is False:
            self.planner.update_arm_states(joint)
            self.planner.set_start_arm_states(joint)
            self.planner.publish_planning_scene_msg()
            rospy.sleep(0.1)

    def set_planning_time_default(self):
        self.planner.set_sigma(0.3)
        self.planner.set_max_ik_trials(20000)
        self.planner.set_max_planning_time(10)

    def disable_collision_with_arm(self, arm_name, object_id, enable):
        for i in range(7):
            self.change_collision('{0}_link{1}'.format(arm_name, i + 1),
                                  object_id, enable)
        self.change_collision(arm_name + '_hand', object_id, enable)
        if arm_name != 'panda_left':
            self.change_collision(arm_name + '_finger_left_link', object_id,
                                  enable)
            self.change_collision(arm_name + '_finger_right_link', object_id,
                                  enable)

    def set_collision_arm(self, arm_name):
        not_col_obs = self.planning_scene.get_all_attached_objects()
        for obj in self.obj_list:
            if obj in not_col_obs:
                continue
            for i in range(7):
                self.change_collision('{0}_link{1}'.format(arm_name, i + 1),
                                      obj, True)
            self.change_collision(arm_name + '_hand', obj, True)
            if arm_name != 'panda_left':
                self.change_collision(arm_name + '_finger_left_link', obj,
                                      True)
                self.change_collision(arm_name + '_finger_right_link', obj,
                                      True)

    def reset_collision_arm(self, arm_name):
        for obj in self.obj_list:
            for i in range(7):
                self.change_collision('{0}_link{1}'.format(arm_name, i + 1),
                                      obj, False)
            self.change_collision(arm_name + '_hand', obj, True)
            if arm_name != 'panda_left':
                self.change_collision(arm_name + '_finger_left_link', obj,
                                      False)
                self.change_collision(arm_name + '_finger_right_link', obj,
                                      False)

    def reset_collision_all_arm(self):
        arm_list = ['panda_left', 'panda_right', 'panda_top']
        for arm in arm_list:
            for obj in self.obj_list:
                for i in range(7):
                    self.change_collision('{0}_link{1}'.format(arm, i + 1),
                                          obj, False)
                self.change_collision(arm + '_hand', obj, False)
                if arm != 'panda_left':
                    self.change_collision(arm + '_finger_left_link', obj,
                                          False)
                    self.change_collision(arm + '_finger_right_link', obj,
                                          False)

    def set_fixed_arm(self, arm_name):
        self.set_collision_arm(arm_name)
        self.planner.set_arm_fix(arm_name, True)

    def reset_fixed_arm(self, arm_name):
        print('reset_fixed_arm1')
        self.reset_collision_arm(arm_name)
        self.planner.set_arm_fix(arm_name, False)

    def reset_fixed_arm_all(self):
        self.reset_collision_all_arm()
        self.planner.reset_fix()

    def get_kinematics(self, arm_name):
        self.__update_current_state()
        pos = np.empty(3)
        quat = np.empty(4)
        self.planner.get_arm_ee_pose(arm_name, pos, quat)
        return pos, quat

    # transform w(base) - 0(robot_base) - 7(flange) - e(end_effector) - o(furniture_base)
    def calc_obj_pos(self, arm_name, object_id, grasp_param):
        T_w0_ = np.eye(4)
        if arm_name == 'panda_left':
            T_w0_[0:3, 3] = np.array([0.0, 0.3, 1.0])
        elif arm_name == 'panda_right':
            T_w0_[0:3, 3] = np.array([0.0, -0.3, 1.0])
        elif arm_name == 'panda_top':
            T_w0_[0, 0] = -1.0
            T_w0_[1, 1] = -1.0
            T_w0_[0:3, 3] = np.array([1.35, 0.3, 1.0])
        p_07_, q_07_ = self.get_kinematics(arm_name)
        T_07_ = quaternion_matrix(q_07_)
        T_07_[0:3, 3] = p_07_
        T_7e_ = np.eye(4)
        T_7e_[0:3, 3] = np.array([0.0, 0.0, 0.0775])
        p_oe_, q_oe_ = self.get_grasp_pose(object_id, grasp_param)
        T_oe_ = quaternion_matrix(q_oe_)
        T_oe_[0:3, 3] = p_oe_
        T_wo_ = np.dot(T_w0_, np.dot(np.dot(T_07_, T_7e_),
                                     np.linalg.inv(T_oe_)))
        quat = quaternion_from_matrix(T_wo_)
        pos = translation_from_matrix(T_wo_)
        return pos, quat

    # def calc_obj_pos(self,arm_name,object_id, grasp_index, grasp_param):
    #     T_w0_ = np.eye(4)
    #     if arm_name == 'panda_left':
    #         T_w0_[0:3,3] = np.array([0.0, 0.3, 1.0])
    #     elif arm_name == 'panda_right':
    #         T_w0_[0:3,3] = np.array([0.0, -0.3, 1.0])
    #     elif arm_name == 'panda_top':
    #         T_w0_[0,0] = -1.0
    #         T_w0_[1,1] = -1.0
    #         T_w0_[0:3,3] = np.array([1.35, 0.3, 1.0])
    #     p_07_, q_07_ = self.get_kinematics(arm_name)
    #     T_07_ = quaternion_matrix(q_07_)
    #     T_07_[0:3,3] = p_07_
    #     T_7e_ = np.eye(4)
    #     T_7e_[0:3,3] = np.array([0.0, 0.0, 0.0775])
    #     p_oe_, q_oe_ = self.get_grasp_pose(object_id, grasp_index, grasp_param)
    #     T_oe_ = quaternion_matrix(q_oe_)
    #     T_oe_[0:3,3] = p_oe_
    #     T_wo_ = np.dot(T_w0_,np.dot(np.dot(T_07_,T_7e_),np.linalg.inv(T_oe_)))
    #     quat = quaternion_from_matrix(T_wo_)
    #     pos = translation_from_matrix(T_wo_)
    #     return pos, quat

    # transform w(base) - 0(robot_base) - 7(flange) - e(end_effector) - o(furniture_base)
    def calc_obj_pos_point(self, arm_name, T_oe_):
        T_w0_ = np.eye(4)
        if arm_name == 'panda_left':
            T_w0_[0:3, 3] = np.array([0.0, 0.3, 1.0])
        elif arm_name == 'panda_right':
            T_w0_[0:3, 3] = np.array([0.0, -0.3, 1.0])
        elif arm_name == 'panda_top':
            T_w0_[0, 0] = -1.0
            T_w0_[1, 1] = -1.0
            T_w0_[0:3, 3] = np.array([1.35, 0.3, 1.0])
        p_07_, q_07_ = self.get_kinematics(arm_name)
        T_07_ = quaternion_matrix(q_07_)
        T_07_[0:3, 3] = p_07_
        T_7e_ = np.eye(4)
        T_7e_[0:3, 3] = np.array([0.0, 0.0, 0.0825])
        T_wo_ = np.dot(T_w0_, np.dot(np.dot(T_07_, T_7e_),
                                     np.linalg.inv(T_oe_)))
        quat = quaternion_from_matrix(T_wo_)
        pos = translation_from_matrix(T_wo_)
        return pos, quat

    def update_object_states(self, object_list):
        for obj in object_list:
            self.update_object_states_each(obj[0], obj[1], obj[2])

    def update_box_states(self, box_list):
        for box in box_list:
            self.planner.update_object_states(box[0], box[1], box[2])

    def get_last_object_state(self, object_name):
        return self.last_object_pose[object_name]

    def save_ik_solutions(self):
        self.planner.save_ik_solutions()

    def attach_object(self, arm_name, object_id, is_arm=True):
        self.__update_current_state()
        self.planner.attach_object(object_id, arm_name + '_hand',
                                   self.hand_touch_links[arm_name])
        self.planner.publish_planning_scene_msg()

    def detach_object(self, arm_name, object_id):
        self.__update_current_state()
        self.planner.detach_object(object_id, arm_name + '_hand')

    def remove_collision_object(self, name1):
        self.__update_current_state()
        self.planner.remove_collision_object(name1)

    # change collision state with name1 and name2 / ignore collision when True
    def change_collision(self, name1, name2, allowed):
        # self.__update_current_state()
        self.planner.change_collision(name1, name2, allowed)

    # change collision state with name1 and all the other elements that were used in this function / ignore collision when True
    def change_collisions(self, name1, allowed):
        # self.__update_current_state()
        self.planner.change_collisions(name1, self.collision_allowed_ids,
                                       allowed)
        if allowed:
            no_overlap_ = True
            for i in range(len(self.collision_allowed_ids)):
                if self.collision_allowed_ids[i] == name1:
                    no_overlap_ = False
            if no_overlap_: self.collision_allowed_ids.append(name1)
        else:
            for i in range(len(self.collision_allowed_ids)):
                if self.collision_allowed_ids[i] == name1:
                    self.collision_allowed_ids.erase(
                        self.collision_allowed_ids.begin() + i)
                    break

    # change collision state with name1 and all the other elements / ignore collision when True
    def change_collisions_all(self, name1, allowed):
        self.__update_current_state()
        self.planner.change_collisions_all(name1, allowed)
        if allowed:
            no_overlap_ = True
            for i in range(len(self.collision_allowed_ids)):
                if self.collision_allowed_ids[i] == name1:
                    no_overlap_ = False
            if no_overlap_: self.collision_allowed_ids.append(name1)
        else:
            for i in range(len(self.collision_allowed_ids)):
                if self.collision_allowed_ids[i] == name1:
                    self.collision_allowed_ids.erase(
                        self.collision_allowed_ids.begin() + i)
                    break

    def print_current_collision_info(self):
        self.planner.print_current_collision_infos()

    def make_follow_trajectory_goal(self, trajectory):
        if self.is_real == False:
            display_from_trajectory_msg(self.planner, trajectory,
                                        self.joint_states, self.joint_map)
            q = np.array(trajectory.points[-1].positions)
            for i in range(len(trajectory.joint_names)):
                idx = self.joint_map[trajectory.joint_names[i]]
                self.joint_states[idx] = q[i]

        return_goal = FollowJointTrajectoryGoal()
        return_goal.trajectory = trajectory
        return return_goal

    def plan_joint_pose(self, arm_name, joint_pose, name=None):
        if self.is_real: self.check_robot_error()
        self.__update_current_state()
        path = self.__load_pickles(name)
        if path is not None:
            return self.__process_pickles(path)
            # self.planner.set_solved_path(path)
            # self.planner.add_start_to_path(self.joint_states)
            # self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
            # trajectory = to_trajectory_msgs(self.planner)
            # self.reset_collision_all_arm()
            # return trajectory
        self.__update_current_state()
        self.__set_planner_for_coarse_motion(name)
        r = self.planner.solve_for_joint_pose(arm_name, joint_pose)
        if r == False:
            raise RuntimeError('plan failed')
        self.planner.time_parameterize(self.max_velocity_scaling_factor,
                                       self.max_acceleration_scaling_factor)

        path = self.planner.get_solved_path()
        self.planner.update_arm_states(path[-1])
        self.planner.set_start_arm_states(path[-1])

        trajectory = to_trajectory_msgs(self.planner)

        self.__save_pickles(name, path)
        self.reset_collision_all_arm()
        return trajectory

    def plan_given_path(self, path, name=None):
        if self.is_real: self.check_robot_error()
        self.__update_current_state()
        path = self.__load_pickles(name)
        if path is not None:
            return self.__process_pickles(path)
            # self.planner.set_solved_path(path)
            # self.planner.add_start_to_path(self.joint_states)
            # self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
            # trajectory = to_trajectory_msgs(self.planner)
            # return trajectory

        self.planner.set_solved_path(path)
        self.planner.add_start_to_path(self.joint_states)
        self.planner.time_parameterize(self.max_velocity_scaling_factor,
                                       self.max_acceleration_scaling_factor)
        trajectory = to_trajectory_msgs(self.planner)
        self.__save_pickles(name, path)
        return trajectory

    def plan_pickle(self, name):
        if self.is_real: self.check_robot_error()
        self.__update_current_state()
        path = self.__load_pickles(name)
        if path is not None:
            return self.__process_pickles(path)
            # self.planner.set_solved_path(path)
            # self.planner.add_start_to_path(self.joint_states)
            # self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
            # trajectory = to_trajectory_msgs(self.planner)
            # return trajectory

        raise RuntimeError('plan failed')

    def plan_target_pose(self, arm_name, target_pos, target_quat, name=None):
        if self.is_real: self.check_robot_error()
        self.__update_current_state()
        path = self.__load_pickles(name)
        if path is not None:
            return self.__process_pickles(path)
            # self.planner.set_solved_path(path)
            # self.planner.add_start_to_path(self.joint_states)
            # self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
            # trajectory = to_trajectory_msgs(self.planner)
            # self.reset_collision_all_arm()
            # return trajectory

        self.__update_current_state()
        self.__set_planner_for_coarse_motion(name)
        r = self.planner.solve_for_ik_pose(arm_name, target_pos, target_quat)
        if r == False:
            raise RuntimeError('plan failed')
        self.planner.time_parameterize(self.max_velocity_scaling_factor,
                                       self.max_acceleration_scaling_factor)
        trajectory = to_trajectory_msgs(self.planner)

        path = self.planner.get_solved_path()
        self.__save_pickles(name, path)
        self.reset_collision_all_arm()
        return trajectory

    # CAUTION: this resets all fixed arms
    def plan_dual_arm_pose(self,
                           arm_names,
                           object_id,
                           desired_pos,
                           desired_quat,
                           name=None):
        if self.is_real: self.check_robot_error()
        self.__update_current_state()
        path = self.__load_pickles(name)
        if path is not None:
            return self.__process_pickles(path)
            # self.planner.set_solved_path(path)
            # self.planner.add_start_to_path(self.joint_states)
            # self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
            # trajectory = to_trajectory_msgs(self.planner)
            # self.reset_collision_all_arm()
            # return trajectory
        self.__update_current_state()
        pos, quat = self.last_object_pose[object_id]

        self.dual_arm_task.set_arm_names(
            self.__convert_to_name_vector(arm_names))
        print('joints:', self.joint_states)
        self.dual_arm_task.set_start_and_goal(self.joint_states, pos, quat,
                                              desired_pos, desired_quat)
        self.dual_arm_task.set_timeout_limit(360.0)
        r = self.dual_arm_task.solve()

        if r == False:
            raise RuntimeError('plan failed')

        path = self.dual_arm_task.get_path()
        self.planner.set_solved_path(path)
        self.planner.time_parameterize(0.05, 0.2)

        # for dual traj
        print(arm_names)
        for arm in self.arm_names:
            if arm in arm_names:
                continue
            self.planner.set_arm_fix(arm, True)
            print('fix', arm)
        trajectory = to_trajectory_msgs(self.planner)
        # reset
        self.planner.reset_fix()
        self.__save_pickles(name, path)
        self.reset_collision_all_arm()
        return trajectory

    # CAUTION: this resets all fixed arms
    def plan_dual_arm_pose2(self,
                            arm_names,
                            object_id,
                            desired_pos,
                            desired_quat,
                            name=None):
        if self.is_real: self.check_robot_error()
        self.__update_current_state()
        path = self.__load_pickles(name)
        if path is not None:
            self.planner.set_solved_path(path)
            self.planner.add_start_to_path(self.joint_states)
            self.planner.time_parameterize(
                self.max_velocity_scaling_factor,
                self.max_acceleration_scaling_factor)
            trajectory = to_trajectory_msgs(self.planner)
            self.reset_collision_all_arm()
            return trajectory

        pos, quat = self.last_object_pose[object_id]

        self.dual_arm_task.set_arm_names(
            self.__convert_to_name_vector(arm_names))
        self.dual_arm_task.set_start_and_goal(self.joint_states, pos, quat,
                                              desired_pos, desired_quat)
        r = self.dual_arm_task.solve2()

        if r == False:
            raise RuntimeError('plan failed')

        path = self.dual_arm_task.get_path()
        self.planner.set_solved_path(path)
        self.planner.time_parameterize(self.max_velocity_scaling_factor,
                                       self.max_acceleration_scaling_factor)

        # for dual traj
        for arm in arm_names:
            if arm in self.arm_names:
                continue
            self.planner.set_arm_fix(arm, True)
        trajectory = to_trajectory_msgs(self.planner)
        # reset
        self.planner.reset_fix()
        self.__save_pickles(name, path)
        self.reset_collision_all_arm()
        return trajectory

    def display_path(self, dt=0.01):
        display(self.planner, dt)

    def get_moved_joint_pose(self, arm_name, pos, quat, q0):
        q_out = np.zeros(7)
        suc = self.planner.get_moved_joints(arm_name, pos, quat, q0, q_out)
        print('q0', q0)
        print('q_', q_out)
        if suc is False:
            print("Fail")
            return None
        else:
            return q_out

    def reproject_path(self, path_file, tol, iteration, idx, offset):
        tt = []
        with open(path_file, "r") as file:
            for line in file.readlines():
                split_line = line.split(" ")
                if (len(split_line) > 21):
                    split_line.pop(-1)
                if (len(split_line) < 21):
                    continue
                for i in range(21):
                    split_line[i] = float(split_line[i])
                tt.append(split_line)

        path = np.array(tt)
        self.planner.set_solved_path(path)
        self.__update_current_state()
        print(offset)
        success = self.planner.reproject_path(self.joint_states, tol,
                                              iteration, idx, offset)
        if success is True:
            print("PROJECTION SUCCESS!!")
        else:
            print("PROJECTION FAILED")

        self.planner.time_parameterize(self.max_velocity_scaling_factor,
                                       self.max_acceleration_scaling_factor)
        trajectory = to_trajectory_msgs(self.planner)
        return trajectory