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
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()
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