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 fn():
while True:
q = sample_fn()
set_joint_positions(joints, context, q)
world_from_body = get_body_pose(context, body)
point_world = world_from_body.translation()
if aabb_contains_point(point_world, aabb):
return q
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 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
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_value(q):
key = id(q)
if key not in cache:
set_joint_positions(joints, context, q)
cache[key] = get_body_pose(context, body)
return cache[key]
def get_body_path(body, context, joints, joint_path):
body_path = []
for conf in joint_path:
set_joint_positions(joints, context, conf)
body_path.append(get_body_pose(context, body))
return body_path