def workspace_motion_plan( base_manip, manipulator, vector, steps=10): # TODO - use IK to check if even possibly valid distance, direction = length(vector), normalize(vector) step_length = distance / steps with base_manip.robot: base_manip.robot.SetActiveManipulator(manipulator) base_manip.robot.SetActiveDOFs(manipulator.GetArmIndices()) with collision_saver(base_manip.robot.GetEnv(), openravepy_int.CollisionOptions.ActiveDOFs): try: # TODO - Bug in specifying minsteps. Need to specify at least 2 times the desired otherwise it stops early traj = base_manip.MoveHandStraight(direction, minsteps=10 * steps, maxsteps=steps, steplength=step_length, ignorefirstcollision=None, starteematrix=None, greedysearch=True, execute=False, outputtraj=None, maxdeviationangle=None, planner=None, outputtrajobj=True) return list(sample_manipulator_trajectory(manipulator, traj)) except planning_error: return None
def __init__(self, radius): swap_xz = trans_from_quat( quat_from_angle_vector(math.pi / 2, [0, 1, 0])) translate = trans_from_point(0, 0, radius) self.origin_grasp = translate.dot(swap_xz) pregrasp_vector = PREGRASP_DISTANCE * normalize(np.array([0.5, 0, -1])) self.gripper_from_pregrasp = trans_from_point(*pregrasp_vector)
def sample_edge_point(polygon, radius): from misc.numerical import sample_categorical edges = zip(polygon, polygon[-1:] + polygon[:-1]) edge_weights = { i: max(length(v2 - v1) - 2 * radius, 0) for i, (v1, v2) in enumerate(edges) } # TODO: fail if no options while True: index = sample_categorical(edge_weights) v1, v2 = edges[index] t = random.uniform(radius, length(v2 - v1) - 2 * radius) yield t * normalize(v2 - v1) + v1
def get_closest_edge_point(polygon, point): edges = zip(polygon, polygon[-1:] + polygon[:-1]) best = None for v1, v2 in edges: proj = (v2 - v1)[:2].dot((point - v1)[:2]) if proj <= 0: closest = v1 elif length((v2 - v1)[:2]) <= proj: closest = v2 else: closest = proj * normalize((v2 - v1)) if (best is None) or (length((point - closest)[:2]) < length( (point - best)[:2])): best = closest return best
def workspace_motion_plan(base_manip, manipulator, vector, steps=10): distance, direction = length(vector), normalize(vector) step_length = distance / steps with base_manip.robot: base_manip.robot.SetActiveManipulator(manipulator) base_manip.robot.SetActiveDOFs(manipulator.GetArmIndices()) with collision_saver(base_manip.robot.GetEnv(), openravepy_int.CollisionOptions.ActiveDOFs): try: traj = base_manip.MoveHandStraight(direction, minsteps=10 * steps, maxsteps=steps, steplength=step_length, ignorefirstcollision=None, starteematrix=None, greedysearch=True, execute=False, outputtraj=None, maxdeviationangle=None, planner=None, outputtrajobj=True) return list(sample_manipulator_trajectory(manipulator, traj)) except planning_error: return None
def __init__(self, height): bottom = trans_from_point(0, 0, -height / 2) reflect = trans_from_quat(quat_from_axis_angle(0, -math.pi, 0)) self.origin_grasp = reflect.dot(bottom) pregrasp_vector = PREGRASP_DISTANCE * normalize(np.array([0, 0, -1])) self.gripper_from_pregrasp = trans_from_point(*pregrasp_vector)
def __init__(self, origin_grasp): self.origin_grasp = origin_grasp pregrasp_vector = PREGRASP_DISTANCE * normalize(np.array([0, 0, -1])) self.gripper_from_pregrasp = trans_from_point(*pregrasp_vector)
def sample_grasp_traj(obj, pose, pose2): enable_all(bodies, False) body = bodies[obj] #body.Enable(True) # TODO: fix this set_pose(body, pose.value) # TODO: check if on the same surface point = point_from_pose(pose.value) point2 = point_from_pose(pose2.value) distance = length(point2 - point) if (distance <= 0.0) or (0.6 <= distance): return direction = normalize(point2 - point) orientation = quat_from_angle_vector(angle_from_vector(direction), [0, 0, 1]) #rotate_direction = trans_from_quat_point(orientation, unit_point()) steps = int(math.ceil(distance / PUSH_MAX_DISTANCE) + 1) distances = np.linspace(0., distance, steps) points = [point + d * direction for d in distances] poses = [pose_from_quat_point(orientation, point) for point in points] pose_objects = [pose] + map(Pose, poses[1:-1]) + [pose2] print distance, steps, distances, len(poses) radius, height = get_mesh_radius(body), get_mesh_height(body) contact = cylinder_contact(radius, height) pushes = [] # TODO: I could do all trajectories after all pushes planned for i in xrange(len(poses) - 1): p1, p2 = poses[i:i + 2] # TODO: choose midpoint for base ir_world_from_gripper = manip_from_pose_grasp(p1, contact) set_manipulator_conf(arm, carry_arm_conf) for world_from_base in islice( random_inverse_reachability(ir_model, ir_world_from_gripper), max_failures): set_trans(robot, world_from_base) set_manipulator_conf(arm, carry_arm_conf) if check_collision(robot): continue q = Conf(robot.GetConfigurationValues()) push_arm_confs = [] approach_paths = [] for p in (p1, p2): # TODO: make sure I have the +x push conf world_from_gripper = manip_from_pose_grasp(p, contact) set_manipulator_conf(arm, carry_arm_conf) grasp_arm_conf = solve_inverse_kinematics( arm, world_from_gripper, collisions=False) if grasp_arm_conf is None: break push_arm_confs.append(grasp_arm_conf) set_manipulator_conf(arm, grasp_arm_conf) pregrasp_arm_conf = solve_inverse_kinematics( arm, world_from_gripper.dot(gripper_from_pregrasp), collisions=False) if pregrasp_arm_conf is None: break #if DISABLE_MOTIONS: if True: approach_paths.append([ carry_arm_conf, pregrasp_arm_conf, grasp_arm_conf ]) continue """ set_manipulator_conf(arm, pregrasp_arm_conf) grasp_path = plan_straight_path(robot, grasp_arm_conf, pregrasp_arm_conf) # robot.Release(body) if grasp_path is None: continue pregrasp_path = plan_path(base_manip, pregrasp_arm_conf, carry_arm_conf) if pregrasp_path is None: continue t = Prehensile(full_from_active(robot, grasp_path + pregrasp_path[1:]), obj, pose, grasp) yield [(q, t)] reset_env() return """ else: # Start and end may have different orientations pq1, pq2 = push_arm_confs push_path = plan_straight_path(robot, pq1, pq2) # TODO: make sure the straight path is actually straight if push_path is None: continue po1, po2 = pose_objects[i:i + 2] ap1, ap2 = approach_paths m = Push( full_from_active(robot, ap1), full_from_active(robot, push_arm_confs), #full_from_active(robot, push_path), full_from_active(robot, ap2[::-1]), obj, po1, po2, contact) pushes.append(PushMotion(obj, po1, po2, q, m)) break else: print 'Failure', len(pushes) return print 'Success', len(pushes) yield pushes