def test_generators(task, diagram, diagram_context):
mbp = task.mbp
context = diagram.GetMutableSubsystemContext(mbp, diagram_context)
# Test grasps
print(get_base_body(mbp, task.gripper).name())
print(
get_body_pose(context, mbp.GetBodyByName('left_finger',
task.gripper)).matrix() -
get_body_pose(context, get_base_body(mbp, task.gripper)).matrix())
user_input('Start')
model = task.movable[0]
grasp_gen_fn = get_grasp_gen_fn(task)
for grasp, in grasp_gen_fn(get_model_name(mbp, model)):
grasp.assign(context)
diagram.Publish(diagram_context)
user_input('Continue')
# Test placements
user_input('Start')
pose_gen_fn = get_pose_gen(task, context)
model = task.movable[0]
for pose, in pose_gen_fn(get_model_name(mbp, model), task.surfaces[0]):
pose.assign(context)
diagram.Publish(diagram_context)
user_input('Continue')
def get_pull_fn(task, context, collisions=True, max_attempts=25, step_size=np.pi / 16):
box_from_geom = get_box_from_geom(task.scene_graph)
gripper_frame = get_base_body(task.mbp, task.gripper).body_frame()
fixed = task.fixed_bodies() if collisions else []
pitch = np.pi/2 # TODO: can use a different pitch
grasp_length = 0.02
#approach_vector = 0.01*np.array([0, -1, 0])
# TODO: could also push the door either perpendicular or parallel
# TODO: could solve for kinematic solution of robot and doors
# DoDifferentialInverseKinematics
# TODO: allow small rotation error perpendicular to handle
def fn(robot_name, door_name, dq1, dq2):
robot = task.mbp.GetModelInstanceByName(robot_name)
robot_joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(product(bodies_from_models(task.mbp, [robot, task.gripper]), fixed))
collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph,
robot_joints, collision_pairs=collision_pairs)
door_body = task.mbp.GetBodyByName(door_name)
door_joints = dq1.joints
extend_fn = get_extend_fn(door_joints, resolutions=step_size*np.ones(len(door_joints)))
door_joint_path = [dq1.positions] + list(extend_fn(dq1.positions, dq2.positions)) # TODO: check for collisions
door_cartesian_path = []
for robot_conf in door_joint_path:
set_joint_positions(door_joints, context, robot_conf)
door_cartesian_path.append(get_body_pose(context, door_body))
shape, index = 'cylinder', 1 # Second grasp is np.pi/2, corresponding to +y
#shape, index = 'box', 0 # left_door TODO: right_door
for i in range(2):
handle_aabb, handle_from_box, handle_shape = box_from_geom[int(door_body.model_instance()), door_name, i]
if handle_shape == shape:
break
else:
raise RuntimeError(shape)
[gripper_from_box] = list(get_box_grasps(handle_aabb, orientations=[index], pitch_range=(pitch, pitch), grasp_length=grasp_length))
gripper_from_obj = gripper_from_box.multiply(handle_from_box.inverse())
pull_cartesian_path = [body_pose.multiply(gripper_from_obj.inverse()) for body_pose in door_cartesian_path]
#start_path = list(interpolate_translation(pull_cartesian_path[0], approach_vector))
#end_path = list(interpolate_translation(pull_cartesian_path[-1], approach_vector))
for _ in range(max_attempts):
pull_joint_waypoints = plan_workspace_motion(task.mbp, robot_joints, gripper_frame, reversed(pull_cartesian_path),
collision_fn=collision_fn)
if pull_joint_waypoints is None:
continue
pull_joint_waypoints = pull_joint_waypoints[::-1]
rq1 = Conf(robot_joints, pull_joint_waypoints[0])
rq2 = Conf(robot_joints, pull_joint_waypoints[-1])
combined_joints = robot_joints + door_joints
combined_waypoints = [list(rq) + list(dq) for rq, dq in zip(pull_joint_waypoints, door_joint_path)]
pull_joint_path = plan_waypoints_joint_motion(combined_joints, combined_waypoints, collision_fn=lambda q: False)
if pull_joint_path is None:
continue
traj = Trajectory(Conf(combined_joints, combined_conf) for combined_conf in pull_joint_path)
return rq1, rq2, traj
return fn
def get_grasp_gen_fn(task):
mbp = task.mbp
gripper_frame = get_base_body(mbp, task.gripper).body_frame()
box_from_geom = get_box_from_geom(task.scene_graph)
def gen(obj_name):
obj = mbp.GetModelInstanceByName(obj_name)
obj_aabb, obj_from_box, obj_shape = box_from_geom[
int(obj), get_base_body(mbp, obj).name(), 0]
#finger_aabb, finger_from_box, _ = box_from_geom[int(task.gripper), 'left_finger', 0]
# TODO: union of bounding boxes
if obj_shape == 'cylinder':
grasp_gen = get_top_cylinder_grasps(obj_aabb)
elif obj_shape == 'box':
pitch = 2 * np.pi / 5 # 0 | np.pi/3 | 2 * np.pi / 5
grasp_gen = get_box_grasps(obj_aabb, pitch_range=(pitch, pitch))
else:
raise NotImplementedError(obj_shape)
for gripper_from_box in grasp_gen:
gripper_from_obj = gripper_from_box.multiply(
obj_from_box.inverse())
grasp = Pose(mbp, gripper_frame, obj, gripper_from_obj)
yield (grasp, )
return gen
def get_ik_gen_fn(task,
context,
collisions=True,
max_failures=10,
approach_distance=0.25,
step_size=0.035):
approach_vector = approach_distance * np.array([0, -1, 0])
gripper_frame = get_base_body(task.mbp, task.gripper).body_frame()
fixed = task.fixed_bodies() if collisions else []
initial_guess = None
#initial_guess = get_joint_positions(get_movable_joints(task.mbp, task.robot), context)
def fn(robot_name, obj_name, obj_pose, obj_grasp):
# TODO: if gripper/block in collision, return
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(
product(bodies_from_models(task.mbp, [robot, task.gripper]),
fixed))
collision_fn = get_collision_fn(
task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
joints,
collision_pairs=collision_pairs) # TODO: while holding
gripper_pose = obj_pose.transform.multiply(
obj_grasp.transform.inverse())
gripper_path = list(
interpolate_translation(gripper_pose,
approach_vector,
step_size=step_size))
attempts = 0
last_success = 0
while (attempts - last_success) < max_failures:
attempts += 1
obj_pose.assign(context)
path = plan_frame_motion(task.plant,
joints,
gripper_frame,
gripper_path,
initial_guess=initial_guess,
collision_fn=collision_fn)
if path is None:
continue
print('IK attempts: {}'.format(attempts))
traj = Trajectory([Conf(joints, q) for q in path],
attachments=[obj_grasp])
conf = traj.path[-1]
yield (conf, traj)
last_success = attempts
return fn
def gen(obj_name):
obj = mbp.GetModelInstanceByName(obj_name)
#obj_aabb, obj_from_box = AABBs[obj_name], Isometry3.Identity()
obj_aabb, obj_from_box, _ = box_from_geom[int(obj), get_base_body(mbp, obj).name(), 0]
#finger_aabb, finger_from_box = box_from_geom[int(task.gripper), 'left_finger', 0]
# TODO: union of bounding boxes
#for gripper_from_box in get_top_cylinder_grasps(obj_aabb):
for gripper_from_box in get_box_grasps(obj_aabb, pitch_range=pitch_range):
gripper_from_obj = gripper_from_box.multiply(obj_from_box.inverse())
grasp = Pose(mbp, gripper_frame, obj, gripper_from_obj)
yield grasp,
def gen(obj_name, surface):
obj = mbp.GetModelInstanceByName(obj_name)
surface_body = mbp.GetBodyByName(surface.body_name, surface.model_index)
surface_pose = get_body_pose(context, surface_body)
collision_pairs = set(product(get_model_bodies(mbp, obj), fixed)) # + [surface]
#object_aabb, object_local = AABBs[obj_name], Isometry3.Identity()
#surface_aabb, surface_local = AABBs[surface_name], Isometry3.Identity()
object_aabb, object_local, _ = box_from_geom[int(obj), get_base_body(mbp, obj).name(), 0]
surface_aabb, surface_local, _ = box_from_geom[int(surface.model_index), surface.body_name, surface.visual_index]
for surface_from_object in sample_aabb_placement(object_aabb, surface_aabb):
world_pose = surface_pose.multiply(surface_local).multiply(
surface_from_object).multiply(object_local.inverse())
pose = Pose(mbp, world, obj, world_pose)
pose.assign(context)
if not exists_colliding_pair(task.diagram, task.diagram_context, task.mbp, task.scene_graph, collision_pairs):
yield pose,
def get_grasp_gen(task):
mbp = task.mbp
gripper_frame = get_base_body(mbp, task.gripper).body_frame()
box_from_geom = get_box_from_geom(task.scene_graph)
#pitch_range = (0, 0) # Top grasps
#pitch_range = (np.pi/3, np.pi/3)
pitch_range = (2*np.pi/5, 2*np.pi/5)
#pitch_range = (-np.pi/2, np.pi/2)
def gen(obj_name):
obj = mbp.GetModelInstanceByName(obj_name)
#obj_aabb, obj_from_box = AABBs[obj_name], Isometry3.Identity()
obj_aabb, obj_from_box, _ = box_from_geom[int(obj), get_base_body(mbp, obj).name(), 0]
#finger_aabb, finger_from_box = box_from_geom[int(task.gripper), 'left_finger', 0]
# TODO: union of bounding boxes
#for gripper_from_box in get_top_cylinder_grasps(obj_aabb):
for gripper_from_box in get_box_grasps(obj_aabb, pitch_range=pitch_range):
gripper_from_obj = gripper_from_box.multiply(obj_from_box.inverse())
grasp = Pose(mbp, gripper_frame, obj, gripper_from_obj)
yield grasp,
return gen
def gen(obj_name, surface):
obj = mbp.GetModelInstanceByName(obj_name)
obj_aabb, obj_from_box, _ = box_from_geom[
int(obj), get_base_body(mbp, obj).name(), 0]
collision_pairs = set(product(get_model_bodies(mbp, obj), fixed))
#object_radius = min(object_aabb[:2])
surface_body = mbp.GetBodyByName(surface.body_name,
surface.model_index)
surface_pose = get_body_pose(context, surface_body)
surface_aabb, surface_from_box, _ = box_from_geom[
int(surface.model_index), surface.body_name, surface.visual_index]
for surface_box_from_obj_box in sample_aabb_placement(obj_aabb,
surface_aabb,
shrink=shrink):
world_pose = surface_pose.multiply(surface_from_box).multiply(
surface_box_from_obj_box).multiply(obj_from_box.inverse())
pose = Pose(mbp, world, obj, world_pose)
pose.assign(context)
if not exists_colliding_pair(task.diagram, task.diagram_context,
task.mbp, task.scene_graph,
collision_pairs):
yield (pose, )
def get_ik_fn(task, context, collisions=True, max_failures=5, distance=0.15, step_size=0.04):
#distance = 0.0
approach_vector = distance*np.array([0, -1, 0])
gripper_frame = get_base_body(task.mbp, task.gripper).body_frame()
fixed = task.fixed_bodies() if collisions else []
initial_guess = None
#initial_guess = get_joint_positions(joints, context) # TODO: start with initial
def fn(robot_name, obj_name, pose, grasp):
# TODO: if gripper/block in collision, return
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(product(bodies_from_models(task.mbp, [robot, task.gripper]), fixed))
collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph,
joints, collision_pairs=collision_pairs) # TODO: while holding
grasp_pose = pose.transform.multiply(grasp.transform.inverse())
gripper_path = list(interpolate_translation(grasp_pose, approach_vector))
attempts = 0
last_success = 0
while (attempts - last_success) < max_failures:
attempts += 1
waypoints = plan_workspace_motion(task.mbp, joints, gripper_frame, gripper_path,
initial_guess=initial_guess, collision_fn=collision_fn)
if waypoints is None:
continue
path = plan_waypoints_joint_motion(joints, waypoints, collision_fn=collision_fn)
if path is None:
continue
#path = refine_joint_path(joints, path)
traj = Trajectory(Conf(joints, q) for q in path)
#print(attempts - last_success)
last_success = attempts
return traj.path[-1], traj
return fn
def get_pull_fn(task,
context,
collisions=True,
max_attempts=25,
step_size=np.pi / 16,
approach_distance=0.05):
# TODO: could also push the door either perpendicular or parallel
# TODO: allow small rotation error perpendicular to handle
# DoDifferentialInverseKinematics
box_from_geom = get_box_from_geom(task.scene_graph)
gripper_frame = get_base_body(task.mbp, task.gripper).body_frame()
fixed = task.fixed_bodies() if collisions else []
#approach_vector = approach_distance*np.array([0, -1, 0])
def fn(robot_name, door_name, door_conf1, door_conf2):
robot = task.mbp.GetModelInstanceByName(robot_name)
robot_joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(
product(bodies_from_models(task.mbp, [robot, task.gripper]),
fixed))
collision_fn = get_collision_fn(task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
robot_joints,
collision_pairs=collision_pairs)
door_body = task.mbp.GetBodyByName(door_name)
door_joints = door_conf1.joints
gripper_from_door = get_door_grasp(door_body, box_from_geom)
extend_fn = get_extend_fn(door_joints,
resolutions=step_size *
np.ones(len(door_joints)))
door_joint_path = [door_conf1.positions] + list(
extend_fn(door_conf1.positions,
door_conf2.positions)) # TODO: check for collisions
door_body_path = get_body_path(door_body, context, door_joints,
door_joint_path)
pull_cartesian_path = [
door_pose.multiply(gripper_from_door.inverse())
for door_pose in door_body_path
]
#start_path = list(interpolate_translation(pull_cartesian_path[0], approach_vector))
#end_path = list(interpolate_translation(pull_cartesian_path[-1], approach_vector))
for _ in range(max_attempts):
# TODO: could solve for kinematic solution of robot and door together
pull_joint_waypoints = plan_workspace_motion(
task.mbp,
robot_joints,
gripper_frame,
reversed(pull_cartesian_path),
collision_fn=collision_fn)
if pull_joint_waypoints is None:
continue
pull_joint_waypoints = pull_joint_waypoints[::-1]
combined_joints = robot_joints + door_joints
combined_waypoints = [
list(rq) + list(dq)
for rq, dq in zip(pull_joint_waypoints, door_joint_path)
]
pull_joint_path = plan_waypoints_joint_motion(
combined_joints,
combined_waypoints,
collision_fn=lambda q: False)
if pull_joint_path is None:
continue
# TODO: approach & retreat path?
robot_conf1 = Conf(robot_joints, pull_joint_waypoints[0])
robot_conf2 = Conf(robot_joints, pull_joint_waypoints[-1])
traj = Trajectory(
Conf(combined_joints, combined_conf)
for combined_conf in pull_joint_path)
return (robot_conf1, robot_conf2, traj)
return fn
