Beispiel #1
0
    def sample_rp_from_ee(self, env, rs_param, ee_pose):
        """
            Using Inverse Kinematics to solve all possible robot armpose and basepose that reaches this ee_pose

            robot_pose: list of full robot pose with format of(basepose, backHeight, lArmPose, lGripper, rArmPose, rGripper)
        """
        ee_pos, ee_rot = ee_pose[0], ee_pose[1]
        base_raidus = np.linspace(-1, 1, num=BASE_SAMPLE_SIZE)
        poses = list(itertools.product(base_raidus, base_raidus))
        # sample
        dummy_body = OpenRAVEBody(env, rs_param.name, PR2())
        possible_robot_pose = []
        for pos in poses:
            bp = pos + ee_pos[:2]
            vec = ee_pos[:2] - bp
            vec = vec / np.linalg.norm(vec)
            theta = math.atan2(vec[1], vec[0])
            bp = np.array([bp[0], bp[1], theta])
            dummy_body.set_pose(bp)
            arm_poses = robot_body.ik_arm_pose(ee_pos, ee_rot)
            for arm_pose in arm_poses:
                possible_robot_pose.append(
                    (bp, ee_arm_poses[0], rs_param.lArmPose, rs_param.lGripper, ee_arm_poses[1:], rs_param.rGripper)
                )

        import ipdb

        ipdb.set_trace()
        dummy_body.delete()
        return possible_robot_pose
Beispiel #2
0
    def sample_rp_from_ee(self, env, rs_param, ee_pose):
        """
            Using Inverse Kinematics to solve all possible robot armpose and basepose that reaches this ee_pose

            robot_pose: list of full robot pose with format of(basepose, backHeight, lArmPose, lGripper, rArmPose, rGripper)
        """
        ee_pos, ee_rot = ee_pose[0], ee_pose[1]
        base_raidus = np.linspace(-1, 1, num=BASE_SAMPLE_SIZE)
        poses = list(itertools.product(base_raidus, base_raidus))
        # sample
        dummy_body = OpenRAVEBody(env, rs_param.name, PR2())
        possible_robot_pose = []
        for pos in poses:
            bp = pos + ee_pos[:2]
            vec = ee_pos[:2] - bp
            vec = vec / np.linalg.norm(vec)
            theta = math.atan2(vec[1], vec[0])
            bp = np.array([bp[0], bp[1], theta])
            dummy_body.set_pose(bp)
            arm_poses = robot_body.ik_arm_pose(ee_pos, ee_rot)
            for arm_pose in arm_poses:
                possible_robot_pose.append(
                    (bp, ee_arm_poses[0], rs_param.lArmPose, rs_param.lGripper,
                     ee_arm_poses[1:], rs_param.rGripper))

        import ipdb
        ipdb.set_trace()
        dummy_body.delete()
        return possible_robot_pose
Beispiel #3
0
    def sample_rp_from_ee(t, plan, ee_poses, robot):
        target_pose = target.value[:, 0]
        dummy_body = OpenRAVEBody(plan.env, robot.name+'_dummy', robot.geom)
        possible_rps = []
        for ee_pose in ee_poses:
            base_pose = sampling.get_col_free_base_pose_around_target(t, plan, ee_pose.value, robot).reshape((1,3))
            ik_arm_poses = dummy_body.ik_arm_pose(ee_pose[0], ee_pose[1])
            cur_arm_poses = np.c_[robot.backHeight, robot.rArmPose].reshape((8,))
            chosen_pose = sampling.closest_arm_pose(ik_arm_poses, cur_arm_poses)
            torso_pose, arm_pose = chosen_pose[0], chosen_pose[1:]
            possible_rp = np.hstack((base_pose, torso_pose, robot.lArmPose[:,t], robot.lGripper[:,t], rArmPose, robot.rGripper[:,t]))
            possible_rps.append(possible_rp)

        dummy_body.delete()
        if len(possible_rps) <= 0:
            import ipdb; ipdb.set_trace()

        return possible_rps
Beispiel #4
0
    def sample_rp_from_ee(t, plan, ee_poses, robot):
        target_pose = target.value[:, 0]
        dummy_body = OpenRAVEBody(plan.env, robot.name + '_dummy', robot.geom)
        possible_rps = []
        for ee_pose in ee_poses:
            base_pose = sampling.get_col_free_base_pose_around_target(
                t, plan, ee_pose.value, robot).reshape((1, 3))
            ik_arm_poses = dummy_body.ik_arm_pose(ee_pose[0], ee_pose[1])
            cur_arm_poses = np.c_[robot.backHeight, robot.rArmPose].reshape(
                (8, ))
            chosen_pose = sampling.closest_arm_pose(ik_arm_poses,
                                                    cur_arm_poses)
            torso_pose, arm_pose = chosen_pose[0], chosen_pose[1:]
            possible_rp = np.hstack(
                (base_pose, torso_pose, robot.lArmPose[:, t],
                 robot.lGripper[:, t], rArmPose, robot.rGripper[:, t]))
            possible_rps.append(possible_rp)

        dummy_body.delete()
        if len(possible_rps) <= 0:
            print "something went wrong"
            # import ipdb; ipdb.set_trace()

        return possible_rps