예제 #1
0
def Optimizer(r_grasp, PAM_r, PAM_s, object_s, object_f, object_params, phi,
              r_max, walls, obstacles, obstacles_PAM, current_leg, n, n_p,
              v_max, force_max, legs, dt):
    """ This function finds the best for both the target object and the robot. Here, we assign 4 different type of costs. (1) cost of changing legs if needed. (2) cost of robot motion to the starting state of the planned path. (3) cost of object manipulation, which is based on object's path. (4) cost of predicted re-positionings needed to execute the manipulation plan."""
    global action_push_pull, PAM_goal, grasping_goal, object_path_planned, PAM_path_planned
    # assigning cost of changing from one leg to another based on the distance to the desired pose
    cost_ChangeLeg = 1
    dz_final = np.sqrt((object_s.x - object_f.x)**2 +
                       (object_s.y - object_f.y)**2)
    if dz_final < 1:
        cost_ChangeLeg = 4000
    elif dz_final < 2:
        cost_ChangeLeg = 10000
    else:
        cost_ChangeLeg = 4000

    # assigning weight for cost of predicted repositioning and cost of robot motion
    w_cost_reposition = 300
    w_cost_motion = 100

    # finding object's leg cordinates
    object_leg = find_corners(object_s.x, object_s.y, object_s.phi,
                              object_params[7], object_params[8])

    # initialization (initializeing cost to infinity)
    cost = [float('inf'), float('inf'), float('inf'), float('inf')]
    cost_legchange = [0, 0, 0, 0]
    cost_PAM = [[0, 0], [0, 0], [0, 0], [0, 0]]
    cost_manipulation = [0, 0, 0, 0]
    cost_motion = [0, 0, 0, 0]
    force = [0, 0, 0, 0]
    path = [[[], []], [[], []], [[], []], [[], []]]
    planned_path_w = [[], [], [], []]
    PAM_g = [[[0, 0, 0], [0, 0, 0]], [[0, 0, 0], [0, 0, 0]],
             [[0, 0, 0], [0, 0, 0]], [[0, 0, 0], [0, 0, 0]]]
    command = [[], [], [], []]
    des = [[], [], [], [], []]
    PAM_goal = state()

    # find the nominal trajectory for manipulation
    theta = nominal_traj([object_s.x, object_s.y, object_s.phi],
                         [object_f.x, object_f.y, object_f.phi], v_max, walls,
                         obstacles, n, dt)

    # itterate through each leg to find the leg with minimum cost
    for leg in range(4):
        phi_linear = theta
        psi_linear = [theta[k] + phi[leg] for k in range(len(theta))]
        # find the cost and required force for manipulation for the leg
        force[leg], cost_manipulation[leg], planned_path_w[leg], command[
            leg], des = OptTraj([
                object_s.x, object_s.y, object_s.phi, object_s.xdot,
                object_s.ydot, object_s.phidot
            ], [
                object_f.x, object_f.y, object_f.phi, object_f.xdot,
                object_f.ydot, object_f.phidot
            ], v_max, walls, obstacles, object_params[0:4], object_params[4:7],
                                phi_linear, psi_linear, force_max, r_max[leg],
                                n, dt, object_leg[leg])
        # adding cost of changing leg
        if leg != current_leg:
            cost_legchange[leg] = cost_ChangeLeg
            # adding cost of PAM motion to PAM goal pose
            phi0 = np.arctan2(object_leg[leg][1] - object_s.y,
                              object_leg[leg][0] - object_s.x)
            # finding the better option between pulling and pushing for each leg, with the same manipulation plan
            for push_pull in [0, 1]:
                PAM_g[leg][push_pull] = [
                    r_grasp * np.cos(phi0) * np.sign(push_pull * 2 - 1) +
                    object_leg[leg][0],
                    r_grasp * np.sin(phi0) * np.sign(push_pull * 2 - 1) +
                    object_leg[leg][1], np.pi * push_pull + phi0
                ]
                cost_PAM[leg][push_pull], path[leg][
                    push_pull], command_pam, goal_orientation = OptPath(
                        [PAM_s.x, PAM_s.y, PAM_s.phi], PAM_g[leg][push_pull],
                        walls, obstacles_PAM, n_p, dt)
                if cost_PAM[leg][push_pull] != float("inf"):
                    PAM_s_sim = copy.deepcopy(PAM_s)
                    PAM_s_sim.x, PAM_s_sim.y, PAM_s_sim.phi = [
                        PAM_r * np.cos(phi0) * np.sign(push_pull * 2 - 1) +
                        object_leg[leg][0],
                        PAM_r * np.sin(phi0) * np.sign(push_pull * 2 - 1) +
                        object_leg[leg][1], np.pi * push_pull + phi0
                    ]
                    # adding cost of predicted re-positionings
                    n_transition = traj_simulation(copy.deepcopy(PAM_s_sim),
                                                   copy.deepcopy(object_s),
                                                   force[leg], legs, leg,
                                                   command[leg])
                    cost_PAM[leg][
                        push_pull] += w_cost_reposition * n_transition
            cost_motion[leg] += min(cost_PAM[leg]) * w_cost_motion
            action_push_pull[leg] = np.argmin(cost_PAM[leg])
        else:
            phi0 = np.arctan2(force[leg][0][1], force[leg][0][0])
            for push_pull in [0, 1]:
                PAM_g[leg][push_pull] = [
                    r_grasp * np.cos(phi0) * np.sign(push_pull * 2 - 1) +
                    object_leg[leg][0],
                    r_grasp * np.sin(phi0) * np.sign(push_pull * 2 - 1) +
                    object_leg[leg][1], np.pi * push_pull + phi0
                ]
    cost = [
        cost_legchange[leg] + cost_motion[leg] + cost_manipulation[leg]
        for leg in range(4)
    ]

    if min(cost) < float("inf"):
        [min_index, min_value] = [np.argmin(cost), min(cost)]
        # Finding the grasping goal pose based on the selected plan
        phi0 = np.arctan2(object_leg[min_index][1] - object_s.y,
                          object_leg[min_index][0] - object_s.x)
        grasping_goal = [
            PAM_r * np.cos(phi0) * np.sign(action_push_pull[min_index] * 2 - 1)
            + object_leg[min_index][0],
            PAM_r * np.sin(phi0) * np.sign(action_push_pull[min_index] * 2 - 1)
            + object_leg[min_index][1],
            np.pi * action_push_pull[min_index] + phi0
        ]
        PAM_goal = state()
        PAM_goal.x, PAM_goal.y, PAM_goal.phi = PAM_g[min_index][
            action_push_pull[min_index]]
        object_path_planned = Path()
        object_path_planned.header.frame_id = 'frame_0'
        for i in range(len(planned_path_w[min_index])):
            pose = PoseStamped()
            pose.pose.position.x = planned_path_w[min_index][i][0]
            pose.pose.position.y = planned_path_w[min_index][i][1]
            pose.pose.position.z = 0
            object_path_planned.poses.append(pose)

        PAM_path_planned = Path()
        PAM_path_planned.header.frame_id = 'frame_0'
        if min_index != current_leg:
            for i in range(len(path[min_index][action_push_pull[min_index]])):
                pose = PoseStamped()
                pose.pose.position.x, pose.pose.position.y, pose.pose.orientation.z = path[
                    min_index][action_push_pull[min_index]][i]
                PAM_path_planned.poses.append(pose)
    else:
        min_index = 5
        min_value = float("inf")
    if 0 < min_index and min_index <= 4:
        force_d = force[min_index][0]
    else:
        force_d = [0, 0, 0]

    return cost, min_index, force_d, PAM_goal, grasping_goal, object_path_planned, PAM_path_planned
예제 #2
0
def callback(data):
    """ This is the main function that runs the experiment. Based on the state of the robot and target object and the goal state for the object, it determines which mode it is in and finds the proper command in each mode as follows: (0) stay put. (1) robot motion to the manipulation pre-grasp state. (2) robot motion to grasp the object. (3) manipulation of the object. (4) re-position. (5) success."""
    r = rospy.Rate(10)
    global near_pam_goal, time_run_start, target_object, total_error_position, total_error_orientation, total_time, env, first_3, object_params, obss, r_max, phi, PAM_path_actual, object_path_actual, sys_mode, initials, vis, old_bed_state, old_walker_state, old_blue_chair_state, grasping_goal, PAM_goal, object_path_planned, found_solution, object_r, vis_bag, PAM_path_planned, desired_force, boolean, best_leg, transition_mode, transition, manipulation_object
    if sys_mode != 5:
        pose_aux_p = PoseStamped()
        pose_aux_p.pose.position.x, pose_aux_p.pose.position.y, pose_aux_p.pose.position.z = [
            data.PAM_state.x, data.PAM_state.y, 0
        ]
        PAM_path_actual.poses.append(pose_aux_p)

        gripper_pose = [
            data.PAM_state.x + env.r_gripper * np.cos(data.PAM_state.phi),
            data.PAM_state.y + env.r_gripper * np.sin(data.PAM_state.phi)
        ]  # calculates the gripper pose from PAM pose
        phi_quarter = Find_phi_quarter(
            data.PAM_state.phi
        )  #defines in which quarter is the PAM orientation

        newObs = [
            data.bed_state.x, data.bed_state.y, data.bed_state.phi, 2, 1
        ]  #adds the bed as obstacle for the target object
        obss = [newObs]
        newObs = [
            data.cart_state.x, data.cart_state.y, data.cart_state.phi, 1, 0.3
        ]
        obss.append(newObs)
        newObs = [
            data.gray_chair_state.x, data.gray_chair_state.y,
            data.gray_chair_state.phi, 0.4, 0.4
        ]
        obss.append(newObs)

        if env.manipulation_object == "walker":
            target_object = data.walker_state
            obss.append([
                data.blue_chair_state.x, data.blue_chair_state.y,
                data.blue_chair_state.phi, 0.6, 0.55
            ])  #adds the blue_chair as an obstacle for target object
        elif env.manipulation_object == "blue_chair":
            target_object = data.blue_chair_state
            obss.append([
                data.walker_state.x, data.walker_state.y,
                data.walker_state.phi, 0.6, 0.5
            ])  #adds the walker as an obstacle for target object
        obss_PAM = copy.copy(obss)
        obss_PAM.append(
            [target_object.x, target_object.y, target_object.phi, 0.6,
             0.5])  #adds the target object as an obstacle for PAM
        legs, current_leg = FindLeg(gripper_pose, target_object,
                                    object_params)  #return the grasped leg
        obstacles = define_obs(obss, object_r +
                               env.PAM_r)  #final obstacles for the object
        obstacles_PAM = define_obs(obss_PAM,
                                   env.PAM_r)  #final obstacles for PAM

        input_d = controlInput()  #input to the low-level controller
        vel = [0, 0]
        command = []
        nominal_path_p = []
        change = detectChange(
            data.walker_state, data.bed_state, data.blue_chair_state,
            old_blue_chair_state, old_bed_state, old_walker_state
        )  # checks if the environemtn (currently only including the bed and walker position) has been changed
        if (target_object.x - env.object_goal.x)**2 + (
                target_object.y -
                env.object_goal.y)**2 <= 3 * env.epsilon and (
                    target_object.phi -
                    env.object_goal.phi)**2 <= 2 * env.epsilon_phi * 10:
            sys_mode = 5
            desired = [0, 0]

        elif transition:
            rospy.loginfo("transition:")
            rospy.loginfo(transition)
            if transition_mode == 0:  #move backward from current leg
                if ((legs[best_leg][0] - gripper_pose[0]) *
                    (legs[best_leg][0] - gripper_pose[0]) +
                    (legs[best_leg][1] - gripper_pose[1]) *
                    (legs[best_leg][1] - gripper_pose[1])) >= 0.1:
                    transition_mode = 1
                    command = [[0, 0, 1]]
                else:
                    command = [[-0.04, 0, 1]]
                desired = [command[0][0], command[0][1]]
            elif transition_mode == 1:  #turn towards the new goal pose
                found_solution = 0
                transition = False
                command = [[0, 0, 1]]
                desired = [command[0][0], command[0][1]]
                transition_mode = 0

        else:
            if (change or found_solution == 0) and not transition:
                pose_aux = PoseStamped()
                pose_aux.pose.position.x, pose_aux.pose.position.y, pose_aux.pose.position.z = [
                    target_object.x, target_object.y, 0
                ]
                object_path_actual.poses.append(pose_aux)

                env.n_p = 15
                min_value, best_leg, force, PAM_goal, grasping_goal, object_path_planned, PAM_path_planned = Optimizer(
                    env.r_grasp, env.PAM_r, data.PAM_state, target_object,
                    env.object_goal, object_params, phi, r_max, env.walls,
                    obstacles, obstacles_PAM, current_leg, env.n, env.n_p,
                    env.v_max, env.force_max, legs, env.dt
                )  #runs the whole optimization again both for object and PAM to find the best manipulation plan
                if min(min_value) == float(
                        "inf"):  #inf means the infeasible optimization
                    found_solution = 0  #runs until it finds a soluion
                    command = [[0, 0, 1]]
                    desired = [command[0][0], command[0][1]]
                else:
                    found_solution = 1
                    desired_force.pose.x, desired_force.pose.y, desired_force.pose.phi, desired_force.magnitude = legs[
                        best_leg][0], legs[best_leg][1], np.arctan2(
                            force[1],
                            force[0]), np.sqrt(force[0]**2 + force[1]**2)
                    desired = [0, 0]
            if found_solution == 1 and not transition:
                if current_leg == best_leg:  #(data.PAM_state.x-grasping_goal[0])**2+(data.PAM_state.y-grasping_goal[1])**2 <= 0.01: #and (data.PAM_state.phi-grasping_goal[2])**2 <= 0.01:
                    phi_close = np.pi + data.PAM_state.phi - target_object.phi - best_leg * np.pi / 2
                    if phi_close < 0:
                        phi_close += 2 * np.pi
                    if phi_close >= 2 * np.pi:
                        phi_close -= 2 * np.pi
                    if (phi_close >= env.phi_limit[1]
                            or phi_close <= env.phi_limit[0]):
                        sys_mode = 4
                        transition = True
                        phi_0 = np.arctan2(
                            legs[best_leg][1] - target_object.y,
                            legs[best_leg][0] -
                            target_object.x)  #desired_force.pose.phi
                        phi_force = -np.pi / 2 + phi_0 - target_object.phi + best_leg * np.pi / 2
                        if phi_force <= env.phi_limit[
                                1] or phi_force >= env.phi_limit[0]:
                            push_pull = 1
                        else:
                            push_pull = 0
                        PAM_goal = state()
                        PAM_goal.x, PAM_goal.y, PAM_goal.phi = [
                            env.r_grasp * np.cos(phi_0) *
                            np.sign(push_pull * 2 - 1) + legs[best_leg][0],
                            env.r_grasp * np.sin(phi_0) *
                            np.sign(push_pull * 2 - 1) + legs[best_leg][1],
                            np.pi * push_pull + phi_0
                        ]
                        command = [[0, 0, 1]]
                        desired = [command[0][0], command[0][1]]
                    else:
                        sys_mode = 3
                        if first_3:
                            desired = [0, 0]
                            first_3 = False
                        else:
                            phi_linear = nominal_traj([
                                target_object.x, target_object.y,
                                target_object.phi
                            ], [
                                env.object_goal.x, env.object_goal.y,
                                env.object_goal.phi
                            ], env.v_max, env.walls, obstacles, env.n, env.dt)
                            psi_linear = [
                                phi_linear[k] + phi[current_leg] - np.pi / 2
                                for k in range(len(phi_linear))
                            ]
                            force, cost, planned_path_w, vel, des = OptTraj(
                                [
                                    target_object.x, target_object.y,
                                    target_object.phi, target_object.xdot,
                                    target_object.ydot, target_object.phidot
                                ], [
                                    env.object_goal.x, env.object_goal.y,
                                    env.object_goal.phi, env.object_goal.xdot,
                                    env.object_goal.ydot,
                                    env.object_goal.phidot
                                ], env.v_max, env.walls, obstacles,
                                object_params[0:4], object_params[4:7],
                                phi_linear, psi_linear, env.force_max,
                                r_max[current_leg], env.n, env.dt,
                                gripper_pose)
                            # Finding robot commands in local frame because it is less confusing
                            phi_vel = np.arctan2(
                                vel[1][1], vel[1][0]) - data.PAM_state.phi
                            if phi_vel < -np.pi:
                                phi_vel += 2 * np.pi
                            if phi_vel >= np.pi:
                                phi_vel -= 2 * np.pi
                            if phi_vel >= -np.pi / 2 and phi_vel < np.pi / 2:
                                vel_sign = 1
                                if phi_vel >= 0:
                                    omega_sign = 1
                                else:
                                    omega_sign = -1
                            else:
                                vel_sign = -1
                                if phi_vel >= np.pi / 2:
                                    omega_sign = 1
                                else:
                                    omega_sign = -1
                            phi_ICC = np.abs(np.pi / 2 - np.abs(phi_vel))
                            middle_point = [
                                gripper_pose[0] + vel[1][0] * env.dt / 2,
                                gripper_pose[1] + vel[1][1] * env.dt / 2
                            ]
                            m1 = -1 / (vel[1][1] / vel[1][0])
                            b1 = middle_point[1] - m1 * middle_point[0]
                            m2 = -1 / np.tan(data.PAM_state.phi)
                            b2 = data.PAM_state.y - m2 * data.PAM_state.x
                            ICC = [(b2 - b1) / (m1 - m2),
                                   (m1 * b2 - m2 * b1) / (m1 - m2)]
                            r_R = np.sqrt((data.PAM_state.x - ICC[0])**2 +
                                          (data.PAM_state.y - ICC[1])**2)
                            theta_1 = np.arctan2(gripper_pose[1] - ICC[1],
                                                 gripper_pose[0] - ICC[0])
                            theta_2 = np.arctan2(
                                gripper_pose[1] + vel[1][1] * env.dt - ICC[1],
                                gripper_pose[0] + vel[1][0] * env.dt - ICC[0])
                            omega_R = np.abs(theta_2 - theta_1) / env.dt
                            vel_R = omega_R * r_R
                            command = [[
                                vel_sign * vel_R, omega_sign * omega_R, 1
                            ]]
                            desired = [command[0][0], command[0][1], vel[1]]
                            object_path_planned = Path()
                            object_path_planned.header.frame_id = 'frame_0'

                            for i in range(len(planned_path_w)):
                                pose = PoseStamped()
                                pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = [
                                    planned_path_w[i][0], planned_path_w[i][1],
                                    0
                                ]
                                object_path_planned.poses.append(pose)
                            desired_force.pose.x, desired_force.pose.y, desired_force.pose.phi, desired_force.magnitude = legs[
                                best_leg][0], legs[best_leg][1], np.arctan2(
                                    force[0][1],
                                    force[0][0]), np.sqrt(force[0][0]**2 +
                                                          force[0][1]**2)
                        r.sleep()
                elif (data.PAM_state.x - grasping_goal[0])**2 + (
                        data.PAM_state.y - grasping_goal[1]
                )**2 <= env.r_grasp**2 + env.epsilon and (
                        data.PAM_state.phi -
                        grasping_goal[2])**2 <= env.epsilon_phi:
                    sys_mode = 2
                    command = [[0.06, 0, 1]]
                    desired = [command[0][0], command[0][1]]
                    first_3 = True

                elif near_pam_goal and (data.PAM_state.x - PAM_goal.x)**2 + (
                        data.PAM_state.y - PAM_goal.y)**2 <= env.epsilon and (
                            data.PAM_state.phi -
                            PAM_goal.phi)**2 >= env.epsilon_phi:
                    PAM_goal.phi = np.arctan2(
                        legs[best_leg][1] - data.PAM_state.y,
                        legs[best_leg][0] - data.PAM_state.x)
                    grasping_goal[2] = np.arctan2(
                        legs[best_leg][1] - data.PAM_state.y,
                        legs[best_leg][0] - data.PAM_state.x)
                    boolean = boolean + 1
                    sys_mode = 1
                    rospy.loginfo(sys_mode + 0.5)
                    if boolean < 10:
                        if PAM_goal.phi - data.PAM_state.phi > np.pi:
                            dphi = PAM_goal.phi - data.PAM_state.phi - 2 * np.pi
                        elif PAM_goal.phi - data.PAM_state.phi < -np.pi:
                            dphi = 2 * np.pi + PAM_goal.phi - data.PAM_state.phi
                        else:
                            dphi = PAM_goal.phi - data.PAM_state.phi
                        if dphi > 0:
                            desired = [0.01, 1]
                        else:
                            desired = [0.01, -1]
                    else:
                        desired = [0, 0]
                        boolean = 1

                else:
                    sys_mode = 1
                    if (data.PAM_state.x - PAM_goal.x)**2 + (
                            data.PAM_state.y -
                            PAM_goal.y)**2 <= env.epsilon / 2:
                        near_pam_goal = True
                    else:
                        near_pam_goal = False
                    cost_PAM, path, command, goal_orientation = OptPath([
                        data.PAM_state.x, data.PAM_state.y, data.PAM_state.phi
                    ], [PAM_goal.x, PAM_goal.y, PAM_goal.phi
                        ], env.walls, obstacles_PAM, env.n_p, env.dt)
                    PAM_path_planned = Path()
                    PAM_path_planned.header.frame_id = 'frame_0'
                    for i in range(len(path)):
                        pose = PoseStamped()
                        pose.pose.position.x, pose.pose.position.y, pose.pose.orientation.z = path[
                            i]
                        PAM_path_planned.poses.append(pose)

                    if len(command) == 0:
                        desired = [0, 0]
                    else:
                        dz_final = np.sqrt(
                            (PAM_path_planned.poses[-1].pose.position.x -
                             PAM_goal.x)**2 +
                            (PAM_path_planned.poses[-1].pose.position.y -
                             PAM_goal.y)**2)
                        if dz_final < 0.01 and (
                                PAM_path_planned.poses[-1].pose.orientation.z -
                                PAM_goal.phi)**2 < 0.01 and env.n_p > 3:
                            env.n_p -= 1  #recude the horizon when it is close enough to the goal. this removes the lower velocity effect of being close to the goal with high n_p
                        elif env.n_p <= 20:
                            env.n_p += 1  # increase n_p if it moves far from the goal and due to low n_p it cannot plan well
                        desired = [command[0][0],
                                   command[0][1]]  #v and w velocity of gripper

            if found_solution == 0:
                desired = [0, 0]
                sys_mode = 0

        rospy.loginfo("sys_mode")
        rospy.loginfo(sys_mode)
        vel = dynamicModelRoomba(
            desired
        )  #converts v and w velocity of gripper into angular and translational velocity of Roomba
        rospy.loginfo("desired:")
        rospy.loginfo(desired)
        total_time_run = rospy.get_time() - time_run_start
        rospy.loginfo("time_run:")
        rospy.loginfo(total_time_run)
        rospy.loginfo(
            "***************************************************************************"
        )
        if sys_mode == 5:
            success_status = 1
            total_error_position = np.sqrt(
                (target_object.x - env.object_goal.x)**2 +
                (target_object.y - env.object_goal.y)**2)
            total_error_orientation = np.sqrt(
                (target_object.phi - env.object_goal.phi)**2) * 180 / np.pi
            rospy.loginfo("Success :)")
            rospy.loginfo("error:")
            rospy.loginfo(total_error_position)
            rospy.loginfo(total_error_orientation)
            rospy.loginfo(
                "***************************************************************************"
            )
        elif total_time_run >= 450:
            success_status = 0
            total_error_position = np.sqrt(
                (target_object.x - env.object_goal.x)**2 +
                (target_object.y - env.object_goal.y)**2)
            total_error_orientation = np.sqrt(
                (target_object.phi - env.object_goal.phi)**2) * 180 / np.pi
            rospy.loginfo("Fail :(")
            rospy.loginfo("error:")
            rospy.loginfo(total_error_position)
            rospy.loginfo(total_error_orientation)
            rospy.loginfo(
                "***************************************************************************"
            )

        PAM_path_predicted = path_from_command(command, data.PAM_state)
        old_bed_state = copy.copy(data.bed_state)
        old_walker_state = copy.copy(data.walker_state)
        old_blue_chair_state = copy.copy(data.blue_chair_state)

        input_d.desired_input, input_d.mode = vel, sys_mode
        desiredInput.publish(input_d)

        vis.plot(desired_force, PAM_goal, env.object_goal, PAM_path_predicted,
                 object_path_actual, object_path_planned, PAM_path_actual,
                 PAM_path_planned)
def run_simulation(system):
    """ This is the main function that runs the simulation. Based on the state of the robot and target object and the goal state for the object, it determines which mode it is and finds the proper command in each mode as follows: (0) stay put. (1) robot motion to the manipulation pre-grasp state. (2) robot motion to grasp the object. (3) manipulation of the object. (4) re-position. (5) success."""
    r = rospy.Rate(5)
    global time_run_start, target_object, total_error_position, total_error_orientation, total_time, env, object_params, object_params_sim, obss, r_max, phi, PAM_path_actual, object_path_actual, sys_mode, vis, bed_state, old_object_state, grasping_goal, PAM_goal, object_path_planned, found_solution, object_r, PAM_path_planned, desired_force, boolean, best_leg, transition_mode, transition, blue_chair_state, des, ind

    pose_aux_p = PoseStamped()
    pose_aux_p.pose.position.x, pose_aux_p.pose.position.y, pose_aux_p.pose.position.z = [
        system.PAM_state.x, system.PAM_state.y, 0
    ]
    PAM_path_actual.poses.append(pose_aux_p)

    gripper_pose = [
        system.PAM_state.x + env.r_gripper * np.cos(system.PAM_state.phi),
        system.PAM_state.y + env.r_gripper * np.sin(system.PAM_state.phi)
    ]  # calculates the gripper pose from PAM pose
    phi_quarter = Find_phi_quarter(
        system.PAM_state.phi)  #defines in which quarter is the PAM orientation

    newObs = [env.bed_state.x, env.bed_state.y, env.bed_state.phi, 2,
              1]  #adds the bed as obstacle for the target object
    obss = [newObs]
    newObs = [env.cart_state.x, env.cart_state.y, env.cart_state.phi, 1, 0.3]
    obss.append(newObs)
    newObs = [
        env.gray_chair_state.x, env.gray_chair_state.y,
        env.gray_chair_state.phi, 0.4, 0.4
    ]
    obss.append(newObs)

    if env.manipulation_object == "walker":
        target_object = system.walker_state
        obss.append([
            system.blue_chair_state.x, system.blue_chair_state.y,
            system.blue_chair_state.phi, 0.6, 0.55
        ])  #adds the blue_chair as an obstacle for target object
    elif env.manipulation_object == "blue_chair":
        target_object = system.blue_chair_state
        obss.append([
            system.walker_state.x, system.walker_state.y,
            system.walker_state.phi, 0.54, 0.36
        ])  #adds the walker as an obstacle for target object
    elif env.manipulation_object == "gray_chair":
        target_object = system.gray_chair_state
        obss.append([
            system.blue_chair_state.x, system.blue_chair_state.y,
            system.blue_chair_state.phi, 0.6, 0.55
        ])  #adds the blue_chair as an obstacle for target object
    elif env.manipulation_object == "cart":
        target_object = system.cart_state
        obss.append([
            system.blue_chair_state.x, system.blue_chair_state.y,
            system.blue_chair_state.phi, 0.6, 0.55
        ])  #adds the walker as an obstacle for target object

    obss_PAM = copy.copy(obss)
    obss_PAM.append(
        [target_object.x, target_object.y, target_object.phi, 1.001,
         0.3])  #adds the target object as an obstacle for PAM
    legs, current_leg = FindLeg(gripper_pose, target_object,
                                object_params)  #return the grasped leg
    obstacles = define_obs(obss, object_r +
                           env.PAM_r)  #final obstacles for the object
    obstacles_PAM = define_obs(obss_PAM, env.PAM_r)  #final obstacles for PAM

    input_d = controlInput()  #input to the low-level controller
    vel = [0, 0]
    command = []
    change = detectChange(
        target_object, old_object_state
    )  # checks if the environment (currently only including the bed and walker position) has been changed

    if (target_object.x - env.object_goal.x)**2 + (
            target_object.y - env.object_goal.y)**2 <= env.epsilon and (
                target_object.phi -
                env.object_goal.phi)**2 <= env.epsilon_phi * 10:
        sys_mode = 5
        desired = [0, 0]
        total_error_position = np.sqrt(
            (target_object.x - env.object_goal.x)**2 +
            (target_object.y - env.object_goal.y)**2)
        total_error_orientation = np.sqrt(
            (target_object.phi - env.object_goal.phi)**2) * 180 / np.pi
        rospy.loginfo("Success :)")
        rospy.loginfo("total_time")
        rospy.loginfo(total_time)
        rospy.loginfo("error:")
        rospy.loginfo(total_error_position)
        rospy.loginfo(total_error_orientation)

    elif transition:
        rospy.loginfo("transition_mode:")
        rospy.loginfo(transition_mode)
        if transition_mode == 0:  #move backward from current leg
            target_object.phidot = 0
            target_object.xdot = 0
            target_object.ydot = 0
            if ((legs[best_leg][0] - gripper_pose[0]) *
                (legs[best_leg][0] - gripper_pose[0]) +
                (legs[best_leg][1] - gripper_pose[1]) *
                (legs[best_leg][1] - gripper_pose[1])) >= 0.15:
                transition_mode = 1
                command = [[0, 0, 1]]
            else:
                command = [[-0.04, 0, 1]]
            desired = [command[0][0], command[0][1]]
        elif transition_mode == 1:  # move towards the new goal pose
            if (system.PAM_state.x - PAM_goal.x)**2 + (system.PAM_state.y -
                                                       PAM_goal.y)**2 >= 0.002:
                cost_PAM, path, command, goal_orientation = OptPath(
                    [
                        system.PAM_state.x, system.PAM_state.y,
                        system.PAM_state.phi
                    ], [PAM_goal.x, PAM_goal.y, PAM_goal.phi], env.walls,
                    obstacles_PAM, env.n_p, env.dt)
                if command != []:
                    PAM_path_planned = Path()
                    PAM_path_planned.header.frame_id = 'frame_0'
                    for i in range(len(path)):
                        pose = PoseStamped()
                        pose.pose.position.x, pose.pose.position.y, pose.pose.orientation.z = path[
                            i]
                        PAM_path_planned.poses.append(pose)

                    if len(command) == 0:
                        desired = [0, 0]
                    else:
                        dz_final = np.sqrt(
                            (PAM_path_planned.poses[-1].pose.position.x -
                             PAM_goal.x)**2 +
                            (PAM_path_planned.poses[-1].pose.position.y -
                             PAM_goal.y)**2)
                        if dz_final < 0.05 and (
                                PAM_path_planned.poses[-1].pose.orientation.z -
                                PAM_goal.phi)**2 < 0.01 and env.n_p > 3:
                            env.n_p -= 1  #recude the horizon when it is close enough to the goal. this removes the lower velocity effect of being close to the goal with high n_p
                        elif env.n_p <= 20:
                            env.n_p += 1  # increase n_p if it moves far from the goal and due to low n_p it cannot plan well
                        if command[0][0]**2 <= 0.01 and command[0][
                                1]**2 <= 0.01:
                            desired = [command[1][0], command[1][1]]
                        else:
                            desired = [command[0][0], command[0][1]
                                       ]  #v and w velocity of gripper
                else:
                    boolean = boolean + 1
                    rospy.loginfo(sys_mode + 0.5)
                    if boolean < 3:
                        rospy.loginfo("goal_orientation:")
                        rospy.loginfo(goal_orientation)
                        rospy.loginfo("system.PAM_state.phi:")
                        rospy.loginfo(system.PAM_state.phi)
                        if goal_orientation - system.PAM_state.phi > 0:
                            desired = [0.0001, 1]
                        else:
                            desired = [0.0001, -1]
                    else:
                        desired = [0, 0]
                        boolean = 1
            else:
                transition_mode = 2
                desired = [0, 0]

        elif transition_mode == 2:  #turn towards the leg
            phi_grasp = np.arctan2(legs[best_leg][1] - system.PAM_state.y,
                                   legs[best_leg][0] - system.PAM_state.x)
            if (phi_grasp - system.PAM_state.phi) * (
                    phi_grasp - system.PAM_state.phi) >= 0.005:
                boolean = boolean + 1
                if boolean < 3:
                    if PAM_goal.phi - system.PAM_state.phi > 0:
                        desired = [0.0001, 1]
                    else:
                        desired = [0.0001, -1]
                else:
                    desired = [0, 0]
                    boolean = 1
            else:
                transition_mode = 3
                desired = [0, 0]
        else:  #re-grasp
            if current_leg == best_leg:
                transition = False
                transition_mode = 0
                desired = [0, 0]
            else:
                command = [[0.04, 0, 1]]
                desired = [command[0][0], command[0][1]]
    else:
        if (found_solution == 0) and not transition:
            rospy.loginfo("change")

            pose_aux = PoseStamped()
            pose_aux.pose.position.x, pose_aux.pose.position.y, pose_aux.pose.position.z = [
                target_object.x, target_object.y, 0
            ]
            object_path_actual.poses.append(pose_aux)

            env.n_p = 20
            min_value, best_leg, force, PAM_goal, grasping_goal, object_path_planned, PAM_path_planned, des = Optimizer(
                env.r_grasp, env.PAM_r, system.PAM_state, target_object,
                env.object_goal, object_params, phi, r_max, env.walls,
                obstacles, obstacles_PAM, current_leg, env.n, env.n_p,
                env.v_max, env.force_max, legs, env.dt
            )  #runs the whole optimization again both for object and PAM to find the best manipulation plan
            ind = 0
            if min(min_value) == float(
                    "inf"):  #inf means the infeasible optimization
                found_solution = 0  #runs until it finds a soluion
                command = [[0, 0, 1]]
                desired = [command[0][0], command[0][1]]
            else:
                found_solution = 1
                desired_force.pose.x, desired_force.pose.y, desired_force.pose.phi, desired_force.magnitude = legs[
                    best_leg][0], legs[best_leg][1], np.arctan2(
                        force[1], force[0]), np.sqrt(force[0]**2 + force[1]**2)
                desired = [0, 0]
        else:
            if current_leg == best_leg:  #(system.PAM_state.x-grasping_goal[0])**2+(system.PAM_state.y-grasping_goal[1])**2 <= 0.01: #and (system.PAM_state.phi-grasping_goal[2])**2 <= 0.01:

                phi_close = np.pi + system.PAM_state.phi - target_object.phi - best_leg * np.pi / 2
                if phi_close < 0:
                    phi_close += 2 * np.pi
                if phi_close >= 2 * np.pi:
                    phi_close -= 2 * np.pi
                if (phi_close >= env.phi_limit[1]
                        or phi_close <= env.phi_limit[0]):
                    sys_mode = 4
                    transition = True
                    phi_0 = np.arctan2(
                        legs[best_leg][1] - target_object.y,
                        legs[best_leg][0] -
                        target_object.x)  #desired_force.pose.phi
                    phi_force = -np.pi / 2 + phi_0 - target_object.phi + best_leg * np.pi / 2
                    if phi_force <= env.phi_limit[
                            1] or phi_force >= env.phi_limit[0]:
                        push_pull = 1
                    else:
                        push_pull = 0
                    PAM_goal = state()

                    PAM_goal.x, PAM_goal.y, PAM_goal.phi = [
                        env.r_grasp * np.cos(phi_0) *
                        np.sign(push_pull * 2 - 1) + legs[best_leg][0],
                        env.r_grasp * np.sin(phi_0) *
                        np.sign(push_pull * 2 - 1) + legs[best_leg][1],
                        np.pi * push_pull + phi_0
                    ]
                    command = [[0, 0, 1]]
                    desired = [command[0][0], command[0][1]]
                else:
                    sys_mode = 3
                    print "ind"
                    print ind
                    if ind >= env.n - 1:
                        sys_mode = 6
                        desired = [0, 0, [0, 0]]
                    else:

                        u_old = [
                            target_object.xdot, target_object.ydot,
                            target_object.phidot
                        ]
                        u = lqr_control(target_object, des, phi[current_leg],
                                        ind)
                        a = [(u[i] - u_old[i]) / env.dt for i in range(3)]
                        F = [a[i] * object_params[0] for i in range(2)]
                        Rho = object_params[0:4]
                        Omega = object_params[4:7]
                        R = [[
                            np.cos(target_object.phi),
                            -np.sin(target_object.phi)
                        ],
                             [
                                 np.sin(target_object.phi),
                                 np.cos(target_object.phi)
                             ]]
                        R2 = [[np.cos(Omega[2]), -np.sin(Omega[2])],
                              [np.sin(Omega[2]),
                               np.cos(Omega[2])]]
                        V_wheel = np.linalg.multi_dot([
                            inv(R2),
                            inv(R), [target_object.xdot, target_object.ydot] +
                            np.linalg.multi_dot([[[0, -target_object.phidot],
                                                  [target_object.phidot, 0]],
                                                 R, [Rho[2], Rho[3]]])
                        ])
                        F_wheel = -np.dot(
                            R,
                            np.dot(
                                R2,
                                np.dot([[Omega[0], 0], [0, Omega[1]]],
                                       V_wheel)))
                        F_des = [F[i] - F_wheel[i] for i in range(2)]

                        ind += 1

                        vel = [
                            u[0] + u[2] * r_max[current_leg] *
                            np.cos(phi[current_leg] + target_object.phi),
                            u[1] + u[2] * r_max[current_leg] *
                            np.sin(phi[current_leg] + target_object.phi)
                        ]

                        if vel == [0, 0]:
                            command = [[0, 0, 1]]
                        else:
                            phi_vel = np.arctan2(vel[1],
                                                 vel[0]) - system.PAM_state.phi
                            if phi_vel < -np.pi:
                                phi_vel += 2 * np.pi
                            if phi_vel >= np.pi:
                                phi_vel -= 2 * np.pi
                            if phi_vel >= -np.pi / 2 and phi_vel < np.pi / 2:
                                vel_sign = 1
                                if phi_vel >= 0:
                                    omega_sign = 1
                                else:
                                    omega_sign = -1
                            else:
                                vel_sign = -1
                                if phi_vel >= np.pi / 2:
                                    omega_sign = -1
                                else:
                                    omega_sign = 1
                            phi_ICC = np.abs(np.pi / 2 - np.abs(phi_vel))
                            middle_point = [
                                gripper_pose[0] + vel[0] * env.dt / 2,
                                gripper_pose[1] + vel[1] * env.dt / 2
                            ]
                            m1 = -1 / (vel[1] / vel[0])
                            b1 = middle_point[1] - m1 * middle_point[0]
                            m2 = -1 / np.tan(system.PAM_state.phi)
                            b2 = system.PAM_state.y - m2 * system.PAM_state.x
                            ICC = [(b2 - b1) / (m1 - m2),
                                   (m1 * b2 - m2 * b1) / (m1 - m2)]
                            r_R = np.sqrt((system.PAM_state.x - ICC[0])**2 +
                                          (system.PAM_state.y - ICC[1])**2)
                            theta_1 = np.arctan2(gripper_pose[1] - ICC[1],
                                                 gripper_pose[0] - ICC[0])
                            theta_2 = np.arctan2(
                                gripper_pose[1] + vel[1] * env.dt - ICC[1],
                                gripper_pose[0] + vel[0] * env.dt - ICC[0])
                            omega_R = np.abs(theta_2 - theta_1) / env.dt
                            vel_R = omega_R * r_R
                            command = [[
                                vel_sign * vel_R, omega_sign * omega_R, 1
                            ]]
                        desired = [command[0][0], command[0][1], F_des]
                        object_path_planned = Path()
                        object_path_planned.header.frame_id = 'frame_0'

            elif (system.PAM_state.x - grasping_goal[0])**2 + (
                    system.PAM_state.y -
                    grasping_goal[1])**2 <= env.r_grasp**2 + env.epsilon and (
                        system.PAM_state.phi -
                        grasping_goal[2])**2 <= env.epsilon_phi:
                sys_mode = 2
                command = [[0.04, 0, 1]]
                desired = [command[0][0], command[0][1]]

            elif (system.PAM_state.x - PAM_goal.x)**2 + (
                    system.PAM_state.y - PAM_goal.y)**2 <= env.epsilon and (
                        system.PAM_state.phi -
                        PAM_goal.phi)**2 >= env.epsilon_phi:
                PAM_goal.phi = np.arctan2(
                    legs[best_leg][1] - system.PAM_state.y,
                    legs[best_leg][0] - system.PAM_state.x)
                grasping_goal[2] = np.arctan2(
                    legs[best_leg][1] - system.PAM_state.y,
                    legs[best_leg][0] - system.PAM_state.x)
                boolean = boolean + 1
                sys_mode = 1
                rospy.loginfo(sys_mode + 0.5)
                if boolean < 3:
                    if PAM_goal.phi - system.PAM_state.phi > 0:
                        desired = [0.00001, 1]
                    else:
                        desired = [0.00001, -1]
                else:
                    desired = [0, 0]
                    boolean = 1
            else:
                target_object.phidot = 0
                target_object.xdot = 0
                target_object.ydot = 0
                sys_mode = 1
                cost_PAM, path, command, goal_orientation = OptPath(
                    [
                        system.PAM_state.x, system.PAM_state.y,
                        system.PAM_state.phi
                    ], [PAM_goal.x, PAM_goal.y, PAM_goal.phi], env.walls,
                    obstacles_PAM, env.n_p, env.dt)
                PAM_path_planned = Path()
                PAM_path_planned.header.frame_id = 'frame_0'
                for i in range(len(path)):
                    pose = PoseStamped()
                    pose.pose.position.x, pose.pose.position.y, pose.pose.orientation.z = path[
                        i]
                    PAM_path_planned.poses.append(pose)

                if len(command) == 0:
                    desired = [0, 0]
                else:
                    dz_final = np.sqrt(
                        (PAM_path_planned.poses[-1].pose.position.x -
                         PAM_goal.x)**2 +
                        (PAM_path_planned.poses[-1].pose.position.y -
                         PAM_goal.y)**2)
                    if dz_final < 0.01 and (
                            PAM_path_planned.poses[-1].pose.orientation.z -
                            PAM_goal.phi)**2 < 0.01 and env.n_p > 3:
                        env.n_p -= 1  #recude the horizon when it is close enough to the goal. this removes the lower velocity effect of being close to the goal with high n_p
                    elif env.n_p <= 20:
                        env.n_p += 1  # increase n_p if it moves far from the goal and due to low n_p it cannot plan well
                    desired = [command[0][0],
                               command[0][1]]  #v and w velocity of gripper
    rospy.loginfo("sys_mode")
    rospy.loginfo(sys_mode)
    old_object_state = copy.copy(target_object)
    fb = system.feedback(target_object, object_params_sim, desired, sys_mode,
                         legs, current_leg, env.object_goal,
                         env.manipulation_object)
    total_time += system.dt
    rospy.loginfo("time:")
    rospy.loginfo(total_time)
    rospy.loginfo("time_run:")
    rospy.loginfo(rospy.get_time() - time_run_start)
    rospy.loginfo(
        "***************************************************************************"
    )
    pub.publish(fb)
    PAM_path_predicted = path_from_command(command,
                                           copy.copy(system.PAM_state))

    input_d.desired_input, input_d.mode = vel, sys_mode
    desiredInput.publish(input_d)
    vis.plot(desired_force, PAM_goal, env.object_goal, PAM_path_predicted,
             object_path_actual, object_path_planned, PAM_path_actual,
             PAM_path_planned)
    r.sleep()
예제 #4
0
    desiredInput = rospy.Publisher('controlInput', controlInput, queue_size=10)
    rospy.Subscriber('feedback',
                     feedback,
                     callback,
                     queue_size=1,
                     buff_size=2**24)

    vis = Visual()
    desired_force = force()
    object_path_actual = Path()
    object_path_actual.header.frame_id = 'frame_0'
    PAM_path_actual = Path()
    PAM_path_actual.header.frame_id = 'frame_0'
    object_path_planned = Path()
    PAM_path_planned = Path()
    old_walker_state = state()
    old_bed_state = state()
    old_blue_chair_state = state()

    env = environment()
    env.example_2()
    # object parameters : Rho(4), Omega(3), width(1), length(1)   #Rho[0,1,2,3]=[mass, inertia, x_c, y_c]
    walker_params = [
        1.71238550e+01, 1.42854052e+02, -9.47588940e-01, -9.75868278e-02,
        10.36983266, 92.26898084, 1.19346089, 0.54, 0.35
    ]
    std = [
        8.47688602, 23.76900771, 0.12580533, 0.06097811, 2.70890194,
        6.40448586, 0.08338584, 0, 0
    ]
    walker_r = np.sqrt(walker_params[7]**2 + walker_params[8]**2) / 2
    # joint_traj_walker.header.frame_id = 'frame_0'
    # joint_traj_walker.joint_names = ["walker_x", "walker_y", "walker_phi"]
    # joint_traj_point_walker = JointTrajectoryPoint()
    # display_traj_walker = DisplayTrajectory()
    # display_traj_walker.model_id = 'walker'
    # robot_traj_walker = RobotTrajectory()

    vis = Visual()
    desired_force = force()
    object_path_actual = Path()
    object_path_actual.header.frame_id = 'frame_0'
    PAM_path_actual = Path()
    PAM_path_actual.header.frame_id = 'frame_0'
    object_path_planned = Path()
    PAM_path_planned = Path()
    old_object_state = state()

    # object parameters : Rho(4), Omega(3), width(1), length(1)   #Rho[0,1,2,3]=[mass, inertia, x_c, y_c]
    # sample from distribution
    walker_mu = [
        1.18129219e+02, 7.23564360e+01, 9.11049246e-02, 7.23271543e-02,
        7.71361628e+01, 7.67173557e+01, 2.12292195e-02, 0.56, 0.36
    ]
    walker_std = [
        5.20518267e+01, 1.41632722e+01, 3.91568512e-02, 4.78442496e-02,
        16.07199159, 16.17982517, 1.85912597, 0.0001, 0.0001
    ]
    walker_r = np.sqrt(walker_mu[7]**2 + walker_mu[8]**2) / 2
    blue_chair_mu = [
        143.91837244, 120.24236598, -0.052589144, 0.050633468, 77.48830679,
        76.84306259, 0.097583331, 0.52, 0.48