def fn(q):
if any(not within_limits(joint, position) for joint, position in zip(joints, q)):
return True
if not collision_pairs:
return False
set_joint_positions(joints, plant_context, q)
for attachment in attachments:
attachment.assign(plant_context)
colliding = exists_colliding_pair(diagram, diagram_context, plant, scene_graph, collision_pairs)
return colliding
def fn(robot_name, obj_name, obj_pose, obj_grasp):
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())
end_position = gripper_pose.multiply(approach_vector) + world_vector
translation = end_position - gripper_pose.translation()
gripper_path = list(
interpolate_translation(gripper_pose,
translation,
step_size=step_size))
attempts = 0
last_success = 0
while (attempts - last_success) < max_failures:
attempts += 1
obj_pose.assign(context)
if door_bodies:
for door_body in door_bodies:
positions = get_door_positions(door_body, DOOR_OPEN)
for door_joint in get_movable_joints(
task.plant, door_body.model_instance()):
if door_joint.child_body() == door_body:
set_joint_positions([door_joint], context,
positions)
path = plan_frame_motion(task.plant,
joints,
gripper_frame,
gripper_path,
initial_guess=initial_guess,
collision_fn=collision_fn)
if path is None:
continue
traj = Trajectory([Conf(joints, q) for q in path],
attachments=[obj_grasp])
conf = traj.path[-1]
yield (conf, traj)
last_success = attempts
def open_wsg50_gripper(mbp, context, model_index):
set_joint_positions(get_movable_joints(mbp, model_index), context,
get_open_wsg50_positions(mbp, model_index))
def close_wsg50_gripper(mbp, context, model_index): # 0.05
set_joint_positions(get_movable_joints(mbp, model_index), context,
get_close_wsg50_positions(mbp, model_index))
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