def move_to_pre_trench_configuration(move_arm, x_start, y_start): """ :type move_arm: class 'moveit_commander.move_group.MoveGroupCommander' :type x_start: float :type y_start: float """ # Compute shoulder yaw angle to trench alpha = math.atan2(y_start - constants.Y_SHOU, x_start - constants.X_SHOU) h = math.sqrt( pow(y_start - constants.Y_SHOU, 2) + pow(x_start - constants.X_SHOU, 2)) l = constants.Y_SHOU - constants.HAND_Y_OFFSET beta = math.asin(l / h) # Move to pre trench position, align shoulder yaw joint_goal = move_arm.get_current_joint_values() joint_goal[constants.J_DIST_PITCH] = 0.0 joint_goal[constants.J_HAND_YAW] = math.pi / 2.2 joint_goal[constants.J_PROX_PITCH] = -math.pi / 2 joint_goal[constants.J_SHOU_PITCH] = math.pi / 2 joint_goal[constants.J_SHOU_YAW] = alpha + beta # If out of joint range, abort if (is_shou_yaw_goal_in_range(joint_goal) == False): return False joint_goal[constants.J_SCOOP_YAW] = 0 move_arm.go(joint_goal, wait=True) move_arm.stop() return True
def pre_guarded_move(move_arm,move_limbs,args): targ_x = args[2] targ_y = args[3] norm_x = args[4] norm_y = args[5] norm_z = args[6] offset = args[7] overdrive = args[8] # STUB: GROUND HEIGHT TO BE EXTRACTED FROM DEM targ_elevation = -0.2 # Compute shoulder yaw angle to target alpha = math.atan2( (targ_y+norm_y*offset)-constants.Y_SHOU, (targ_x+norm_x*offset)-constants.X_SHOU) h = math.sqrt(pow( (targ_y+norm_y*offset)-constants.Y_SHOU,2) + pow( (targ_x+norm_x*offset)-constants.X_SHOU,2) ) l = constants.Y_SHOU - constants.HAND_Y_OFFSET beta = math.asin (l/h) # Move to pre move position, align shoulder yaw joint_goal = move_arm.get_current_joint_values() joint_goal[constants.J_DIST_PITCH] = 0 joint_goal[constants.J_HAND_YAW] = 0 joint_goal[constants.J_PROX_PITCH] = -math.pi/2 joint_goal [constants.J_SHOU_PITCH] = math.pi/2 joint_goal[constants.J_SHOU_YAW] = alpha + beta # If out of joint range, abort if (is_shou_yaw_goal_in_range(joint_goal) == False): return False joint_goal[constants.J_SCOOP_YAW] = 0 move_arm.go(joint_goal, wait=True) move_arm.stop() # Once aligned to move goal and offset, place scoop tip at surface target offset goal_pose = move_limbs.get_current_pose().pose goal_pose.position.x = targ_x+norm_x*offset goal_pose.position.y = targ_y+norm_y*offset # Change to limbs, not arm, and figure quat out # oal_pose.orientation.x = norm_x # goal_pose.orientation.y = norm_y # goal_pose.orientation.x = norm_z # goal_pose.orientation.x = norm_x # TODO: trigo on scoop offset goal_pose.position.z = targ_elevation + constants.SCOOP_OFFSET + norm_z*offset move_limbs.set_pose_target(goal_pose) plan = move_limbs.plan() if len(plan.joint_trajectory.points) == 0: # If no plan found, abort return False plan = move_limbs.go(wait=True) move_limbs.stop() move_limbs.clear_pose_targets() print "Done planning approach of guarded_move" return True
def pre_guarded_move(move_arm, args): targ_x = args[2] targ_y = args[3] targ_z = args[4] direction_x = args[5] direction_y = args[6] direction_z = args[7] search_distance = args[8] # STUB: GROUND HEIGHT TO BE EXTRACTED FROM DEM targ_elevation = -0.2 if (targ_z + targ_elevation) == 0: offset = search_distance else: offset = (targ_z * search_distance) / (targ_z + targ_elevation) # Compute shoulder yaw angle to target alpha = math.atan2((targ_y + direction_y * offset) - constants.Y_SHOU, (targ_x + direction_x * offset) - constants.X_SHOU) h = math.sqrt( pow((targ_y + direction_y * offset) - constants.Y_SHOU, 2) + pow((targ_x + direction_x * offset) - constants.X_SHOU, 2)) l = constants.Y_SHOU - constants.HAND_Y_OFFSET beta = math.asin(l / h) # Move to pre move position, align shoulder yaw joint_goal = move_arm.get_current_joint_values() joint_goal[constants.J_DIST_PITCH] = 0 joint_goal[constants.J_HAND_YAW] = 0 joint_goal[constants.J_PROX_PITCH] = -math.pi / 2 joint_goal[constants.J_SHOU_PITCH] = math.pi / 2 joint_goal[constants.J_SHOU_YAW] = alpha + beta # If out of joint range, abort if (is_shou_yaw_goal_in_range(joint_goal) == False): return False joint_goal[constants.J_SCOOP_YAW] = 0 move_arm.go(joint_goal, wait=True) move_arm.stop() # Once aligned to move goal and offset, place scoop tip at surface target offset goal_pose = move_arm.get_current_pose().pose goal_pose.position.x = targ_x goal_pose.position.y = targ_y goal_pose.position.z = targ_z move_arm.set_pose_target(goal_pose) plan = move_arm.plan() if len(plan.joint_trajectory.points) == 0: # If no plan found, abort return False plan = move_arm.go(wait=True) move_arm.stop() move_arm.clear_pose_targets() print "Done planning approach of guarded_move" return True
def move_to_pre_trench_configuration_dig_circ(move_arm, robot, x_start, y_start): """ :type move_arm: class 'moveit_commander.move_group.MoveGroupCommander' :type x_start: float :type y_start: float """ # Initilize to current position joint_goal = move_arm.get_current_pose().pose robot_state = robot.get_current_state() move_arm.set_start_state(robot_state) # Compute shoulder yaw angle to trench alpha = math.atan2(y_start - constants.Y_SHOU, x_start - constants.X_SHOU) h = math.sqrt( pow(y_start - constants.Y_SHOU, 2) + pow(x_start - constants.X_SHOU, 2)) l = constants.Y_SHOU - constants.HAND_Y_OFFSET beta = math.asin(l / h) # Move to pre trench position, align shoulder yaw joint_goal = move_arm.get_current_joint_values() joint_goal[constants.J_DIST_PITCH] = 0.0 joint_goal[constants.J_HAND_YAW] = 0.0 joint_goal[constants.J_PROX_PITCH] = -math.pi / 2 joint_goal[constants.J_SHOU_PITCH] = math.pi / 2 joint_goal[constants.J_SHOU_YAW] = alpha + beta # If out of joint range, abort if (is_shou_yaw_goal_in_range(joint_goal) == False): return False joint_goal[constants.J_SCOOP_YAW] = 0 move_arm.set_joint_value_target(joint_goal) _, plan, _, _ = move_arm.plan() return plan
def guarded_move_plan(move_arm, robot, moveit_fk, args): robot_state = robot.get_current_state() move_arm.set_start_state(robot_state) ### pre-guarded move starts here ### targ_x = args.start.x targ_y = args.start.y targ_z = args.start.z direction_x = args.normal.x direction_y = args.normal.y direction_z = args.normal.z search_distance = args.search_distance # STUB: GROUND HEIGHT TO BE EXTRACTED FROM DEM targ_elevation = -0.2 if (targ_z + targ_elevation) == 0: offset = search_distance else: offset = (targ_z * search_distance) / (targ_z + targ_elevation) # Compute shoulder yaw angle to target alpha = math.atan2((targ_y + direction_y * offset) - constants.Y_SHOU, (targ_x + direction_x * offset) - constants.X_SHOU) h = math.sqrt( pow((targ_y + direction_y * offset) - constants.Y_SHOU, 2) + pow((targ_x + direction_x * offset) - constants.X_SHOU, 2)) l = constants.Y_SHOU - constants.HAND_Y_OFFSET beta = math.asin(l / h) # Move to pre move position, align shoulder yaw joint_goal = move_arm.get_current_joint_values() joint_goal[constants.J_DIST_PITCH] = 0 joint_goal[constants.J_HAND_YAW] = 0 joint_goal[constants.J_PROX_PITCH] = -math.pi / 2 joint_goal[constants.J_SHOU_PITCH] = math.pi / 2 joint_goal[constants.J_SHOU_YAW] = alpha + beta # If out of joint range, abort if (is_shou_yaw_goal_in_range(joint_goal) == False): return False joint_goal[constants.J_SCOOP_YAW] = 0 move_arm.set_joint_value_target(joint_goal) _, plan_a, _, _ = move_arm.plan() if len(plan_a.joint_trajectory.points) == 0: # If no plan found, abort return False # Once aligned to move goal and offset, place scoop tip at surface target offset cs, start_state, goal_pose = calculate_joint_state_end_pose_from_plan_arm( robot, plan_a, move_arm, moveit_fk) move_arm.set_start_state(cs) goal_pose.position.x = targ_x goal_pose.position.y = targ_y goal_pose.position.z = targ_z move_arm.set_pose_target(goal_pose) _, plan_b, _, _ = move_arm.plan() if len(plan_b.joint_trajectory.points) == 0: # If no plan found, abort return False pre_guarded_move_traj = cascade_plans(plan_a, plan_b) ### pre-guarded move ends here ### # Drive scoop tip along norm vector, distance is search_distance cs, start_state, goal_pose = calculate_joint_state_end_pose_from_plan_arm( robot, pre_guarded_move_traj, move_arm, moveit_fk) move_arm.set_start_state(cs) goal_pose.position.x = targ_x goal_pose.position.y = targ_y goal_pose.position.z = targ_z goal_pose.position.x -= direction_x * search_distance goal_pose.position.y -= direction_y * search_distance goal_pose.position.z -= direction_z * search_distance move_arm.set_pose_target(goal_pose) _, plan_c, _, _ = move_arm.plan() guarded_move_traj = cascade_plans(pre_guarded_move_traj, plan_c) pre_guarded_move_end_time = pre_guarded_move_traj.joint_trajectory.points[ len(pre_guarded_move_traj.joint_trajectory.points) - 1].time_from_start guarded_move_end_time = guarded_move_traj.joint_trajectory.points[ len(guarded_move_traj.joint_trajectory.points) - 1].time_from_start estimated_time_ratio = pre_guarded_move_end_time / guarded_move_end_time return guarded_move_traj, estimated_time_ratio
def grind(move_arm, move_limbs, move_grinder, args): x_start = args[1] y_start = args[2] depth = args[3] length = args[4] parallel = args[5] ground_position = args[6] # Compute shoulder yaw angle to trench alpha = math.atan2(y_start - constants.Y_SHOU, x_start - constants.X_SHOU) h = math.sqrt( pow(y_start - constants.Y_SHOU, 2) + pow(x_start - constants.X_SHOU, 2)) l = constants.Y_SHOU - constants.HAND_Y_OFFSET beta = math.asin(l / h) joint_goal = move_arm.get_current_joint_values() joint_goal[constants.J_DIST_PITCH] = 0 joint_goal[constants.J_HAND_YAW] = 4 * math.pi / 3 joint_goal[constants.J_PROX_PITCH] = -math.pi / 2 joint_goal[constants.J_SHOU_PITCH] = math.pi / 2 joint_goal[constants.J_SHOU_YAW] = alpha + beta # If out of joint range, abort if (is_shou_yaw_goal_in_range(joint_goal) == False): return False move_arm.go(joint_goal, wait=True) move_arm.stop() alpha = alpha + beta if parallel: R = math.sqrt(x_start * x_start + y_start * y_start) # adjust trench to fit scoop circular motion dx = 0.04 * R * math.sin(alpha) # Center dig_circular in grind trench dy = 0.04 * R * math.cos(alpha) x_start = 0.9 * ( x_start + dx ) # Move starting point back to avoid scoop-terrain collision y_start = 0.9 * (y_start - dy) else: dx = 5 * length / 8 * math.sin(alpha) dy = 5 * length / 8 * math.cos(alpha) x_start = 0.97 * ( x_start - dx ) # Move starting point back to avoid scoop-terrain collision y_start = 0.97 * (y_start + dy) # Place the grinder placed above the desired starting point, at # an altitude of 0.25 meters in the base_link frame. goal_pose = move_grinder.get_current_pose().pose goal_pose.position.x = x_start # Position goal_pose.position.y = y_start goal_pose.position.z = 0.25 move_grinder.set_pose_target(goal_pose) plan = move_grinder.plan() if len(plan.joint_trajectory.points) == 0: # If no plan found, abort return False plan = move_grinder.go(wait=True) move_grinder.stop() move_grinder.clear_pose_targets() # entering terrain z_start = ground_position + constants.GRINDER_OFFSET - depth go_to_Z_coordinate(move_grinder, x_start, y_start, z_start) # grinding ice forward cartesian_plan, fraction = plan_cartesian_path(move_grinder, length, alpha, parallel) move_grinder.execute(cartesian_plan, wait=True) move_grinder.stop() joint_goal = move_grinder.get_current_joint_values() if parallel: change_joint_value(move_grinder, constants.J_SHOU_YAW, joint_goal[0] + 0.08) else: x_now = move_grinder.get_current_pose().pose.position.x y_now = move_grinder.get_current_pose().pose.position.y z_now = move_grinder.get_current_pose().pose.position.z x_goal = x_now + 0.08 * math.cos(alpha) y_goal = y_now + 0.08 * math.sin(alpha) go_to_Z_coordinate(move_grinder, x_goal, y_goal, z_now) # grinding ice backwards cartesian_plan, fraction = plan_cartesian_path(move_grinder, -length, alpha, parallel) move_grinder.execute(cartesian_plan, wait=True) move_grinder.stop() # exiting terrain x_now = move_grinder.get_current_pose().pose.position.x y_now = move_grinder.get_current_pose().pose.position.y go_to_Z_coordinate(move_grinder, x_start, y_start, 0.22) return True