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
