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
def load_station(time_step=0.0):
# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc
object_file_path = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
#object_file_path = FOAM_BRICK_PATH
station = ManipulationStation(time_step)
station.AddCupboard()
mbp = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=mbp,
scene_graph=scene_graph)
station.Finalize()
robot = mbp.GetModelInstanceByName('iiwa')
gripper = mbp.GetModelInstanceByName('gripper')
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = dict(zip(get_movable_joints(mbp, robot), initial_conf))
initial_poses = {
object: create_transform(translation=[.6, 0, 0]),
}
task = Task(mbp, scene_graph, robot, gripper, movable=[object], surfaces=[],
initial_positions=initial_positions, initial_poses=initial_poses,
goal_on=[])
return mbp, scene_graph, task
def get_open_trajectory(plant, gripper):
gripper_joints = get_movable_joints(plant, gripper)
gripper_extend_fn = get_extend_fn(gripper_joints)
gripper_closed_conf = get_close_wsg50_positions(plant, gripper)
gripper_path = list(
gripper_extend_fn(gripper_closed_conf,
get_open_wsg50_positions(plant, gripper)))
gripper_path.insert(0, gripper_closed_conf)
return Trajectory(Conf(gripper_joints, q) for q in gripper_path)
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 fn(robot_name, conf1, conf2, fluents=[]):
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
moving = bodies_from_models(task.mbp, [robot, gripper])
obstacles = set(task.fixed_bodies())
attachments = []
for fact in fluents:
predicate = fact[0]
if predicate == 'atconf':
name, conf = fact[1:]
conf.assign(context)
obstacles.update(conf.bodies)
elif predicate == 'atpose':
name, pose = fact[1:]
pose.assign(context)
obstacles.update(pose.bodies)
elif predicate == 'atgrasp':
robot, name, grasp = fact[1:]
attachments.append(grasp)
moving.update(grasp.bodies)
else:
raise ValueError(predicate)
obstacles -= moving
#print(sorted(body.name() for body in moving))
#print(sorted(body.name() for body in obstacles))
#print(attachments)
# Can make separate predicate for the frame something is in at a particular time
collision_pairs = set(product(moving, obstacles)) if collisions else set()
collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph,
joints, collision_pairs=collision_pairs, attachments=attachments)
open_wsg50_gripper(task.mbp, context, gripper)
set_joint_positions(joints, context, conf1.positions)
path = plan_joint_motion(joints, conf1.positions, conf2.positions, collision_fn=collision_fn,
restarts=7, iterations=75, smooth=100)
if path is None:
return None
#path = refine_joint_path(joints, path)
traj = Trajectory(Conf(joints, q) for q in path)
return traj,
def fn(robot_name, conf1, conf2, fluents=[]):
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
if teleport:
traj = Trajectory([conf1, conf2])
return (traj, )
moving = bodies_from_models(task.mbp, [robot, gripper])
obstacles = set(task.fixed_bodies())
attachments = parse_fluents(fluents, context, obstacles)
for grasp in attachments:
moving.update(grasp.bodies)
obstacles -= moving
collision_pairs = set(product(moving,
obstacles)) if collisions else set()
collision_fn = get_collision_fn(task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
joints,
collision_pairs=collision_pairs,
attachments=attachments)
#weights = np.ones(len(joints))
#weights = np.array([sum(weights[i:]) for i in range(len(weights))])
distance_fn = None # TODO: adjust the resolutions
open_wsg50_gripper(task.mbp, context, gripper)
path = plan_joint_motion(joints,
conf1.positions,
conf2.positions,
distance_fn=distance_fn,
collision_fn=collision_fn,
restarts=10,
iterations=75,
smooth=50)
if path is None:
return None
traj = Trajectory([Conf(joints, q) for q in path],
attachments=attachments)
return (traj, )
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
def get_pddlstream_problem(task, context, collisions=True):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
plant = task.mbp
robot = task.robot
robot_name = get_model_name(plant, robot)
world = plant.world_frame() # mbp.world_body()
robot_joints = get_movable_joints(plant, robot)
robot_conf = Conf(robot_joints, get_configuration(plant, context, robot))
init = [
('Robot', robot_name),
('CanMove', robot_name),
('Conf', robot_name, robot_conf),
('AtConf', robot_name, robot_conf),
('HandEmpty', robot_name),
]
goal_literals = []
if task.reset_robot:
goal_literals.append(('AtConf', robot_name, robot_conf), )
for obj in task.movable:
obj_name = get_model_name(plant, obj)
#obj_frame = get_base_body(mbp, obj).body_frame()
obj_pose = Pose(plant, world, obj,
get_world_pose(plant, context,
obj)) # get_relative_transform
init += [
('Graspable', obj_name),
('Pose', obj_name, obj_pose),
#('InitPose', obj_name, obj_pose),
('AtPose', obj_name, obj_pose)
]
for surface in task.surfaces:
init += [('Stackable', obj_name, surface)]
# TODO: detect already stacked
for surface in task.surfaces:
surface_name = get_model_name(plant, surface.model_index)
if 'sink' in surface_name:
init += [('Sink', surface)]
if 'stove' in surface_name:
init += [('Stove', surface)]
for door in task.doors:
door_body = plant.tree().get_body(door)
door_name = door_body.name()
door_joints = get_parent_joints(plant, door_body)
door_conf = Conf(door_joints,
get_joint_positions(door_joints, context))
init += [
('Door', door_name),
('Conf', door_name, door_conf),
('AtConf', door_name, door_conf),
]
for positions in [get_door_positions(door_body, DOOR_OPEN)]:
conf = Conf(door_joints, positions)
init += [('Conf', door_name, conf)]
if task.reset_doors:
goal_literals += [('AtConf', door_name, door_conf)]
for obj, transform in task.goal_poses.items():
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, transform)
init += [('Pose', obj_name, obj_pose)]
goal_literals.append(('AtPose', obj_name, obj_pose))
for obj in task.goal_holding:
goal_literals.append(
('Holding', robot_name, get_model_name(plant, obj)))
for obj, surface in task.goal_on:
goal_literals.append(('On', get_model_name(plant, obj), surface))
for obj in task.goal_cooked:
goal_literals.append(('Cooked', get_model_name(plant, obj)))
goal = And(*goal_literals)
print('Initial:', init)
print('Goal:', goal)
stream_map = {
#'sample-pose': from_gen_fn(get_stable_gen(task, context, collisions=collisions)),
'sample-reachable-pose':
from_gen_fn(
get_reachable_pose_gen_fn(task, context, collisions=collisions)),
'sample-grasp':
from_gen_fn(get_grasp_gen_fn(task)),
'plan-ik':
from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)),
'plan-motion':
from_fn(get_motion_fn(task, context, collisions=collisions)),
'plan-pull':
from_fn(get_pull_fn(task, context, collisions=collisions)),
'TrajPoseCollision':
get_collision_test(task, context, collisions=collisions),
'TrajConfCollision':
get_collision_test(task, context, collisions=collisions),
}
#stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def load_manipulation(time_step=0.0):
source = True
if source:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/amazon_table_simplified.sdf")
CUPBOARD_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/cupboard.sdf")
#IIWA7_PATH = FindResourceOrThrow(
# "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
goal_shelf = 'shelf_lower'
else:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf")
CUPBOARD_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf")
IIWA7_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf")
#IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
goal_shelf = 'bottom'
mbp = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
dx_table_center_to_robot_base = 0.3257
dz_table_top_robot_base = 0.0127
dx_cupboard_to_table_center = 0.43 + 0.15
dz_cupboard_to_table_center = 0.02
cupboard_height = 0.815
cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center
cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base
robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa',
scene_graph=scene_graph, plant=mbp)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper',
scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error
amazon_table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='amazon_table',
scene_graph=scene_graph, plant=mbp)
cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard',
scene_graph=scene_graph, plant=mbp)
brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick',
scene_graph=scene_graph, plant=mbp)
# left_door, left_door_hinge, cylinder
weld_gripper(mbp, robot, gripper)
weld_to_world(mbp, robot, create_transform())
weld_to_world(mbp, amazon_table, create_transform(
translation=[dx_table_center_to_robot_base, 0, -dz_table_top_robot_base]))
weld_to_world(mbp, cupboard, create_transform(
translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi]))
mbp.Finalize(scene_graph)
shelves = [
'bottom',
'shelf_lower',
'shelf_upper'
'top',
]
goal_surface = VisualElement(cupboard, 'top_and_bottom', shelves.index(goal_shelf))
surfaces = [
#VisualElement(amazon_table, 'amazon_table', 0),
goal_surface,
]
door_names = [
'left_door',
'right_door',
]
doors = [mbp.GetBodyByName(name).index() for name in door_names]
door_position = DOOR_CLOSED # np.pi/2
#door_position = DOOR_OPEN
#door_position = np.pi
#door_position = np.pi/8
initial_positions = {
mbp.GetJointByName('left_door_hinge'): -door_position,
#mbp.GetJointByName('right_door_hinge'): door_position,
mbp.GetJointByName('right_door_hinge'): np.pi,
}
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions.update(zip(get_movable_joints(mbp, robot), initial_conf))
initial_poses = {
#brick: create_transform(translation=[0.6, 0, 0]),
brick: create_transform(translation=[0.4, 0, 0]),
}
goal_on = [
(brick, goal_surface),
]
task = Task(mbp, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors,
initial_positions=initial_positions, initial_poses=initial_poses, goal_on=goal_on)
return mbp, scene_graph, task
def load_manipulation(time_step=0.0,
use_meshcat=True,
use_controllers=True,
use_external=False):
if not use_external:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/amazon_table_simplified.sdf"
)
CUPBOARD_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/cupboard.sdf")
IIWA7_PATH = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf"
)
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
goal_shelf = 'shelf_lower'
#goal_shelf = 'shelf_upper'
else:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf"
)
CUPBOARD_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf"
)
IIWA7_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf"
)
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf"
)
goal_shelf = 'bottom'
#IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
plant = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
dx_table_center_to_robot_base = 0.3257
dz_table_top_robot_base = 0.0127
dx_cupboard_to_table_center = 0.43 + 0.15
dz_cupboard_to_table_center = 0.02
cupboard_height = 0.815
cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center
cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base
robot = AddModelFromSdfFile(file_name=IIWA7_PATH,
model_name='iiwa',
scene_graph=scene_graph,
plant=plant)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH,
model_name='gripper',
scene_graph=scene_graph,
plant=plant) # TODO: sdf frame/link error
table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH,
model_name='table',
scene_graph=scene_graph,
plant=plant)
cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH,
model_name='cupboard',
scene_graph=scene_graph,
plant=plant)
brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH,
model_name='brick',
scene_graph=scene_graph,
plant=plant)
weld_gripper(plant, robot, gripper)
weld_to_world(plant, robot, create_transform())
weld_to_world(
plant, table,
create_transform(translation=[
dx_table_center_to_robot_base, 0, -dz_table_top_robot_base
]))
weld_to_world(
plant, cupboard,
create_transform(translation=[cupboard_x, 0, cupboard_z],
rotation=[0, 0, np.pi]))
plant.Finalize(scene_graph)
shelves = [
'bottom',
'shelf_lower',
'shelf_upper',
'top',
]
goal_surface = Surface(plant, cupboard, 'top_and_bottom',
shelves.index(goal_shelf))
surfaces = [
#Surface(plant, table, 'amazon_table', 0),
#goal_surface,
]
door_names = ['left_door', 'right_door']
#door_names = []
doors = [plant.GetBodyByName(name).index() for name in door_names]
door_position = DOOR_CLOSED # ~np.pi/2
#door_position = DOOR_OPEN
#door_position = np.pi
#door_position = np.pi/2
#door_position = np.pi/8
initial_positions = {
plant.GetJointByName('left_door_hinge'):
-door_position,
#plant.GetJointByName('right_door_hinge'): door_position,
plant.GetJointByName('right_door_hinge'):
np.pi / 2,
}
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
#brick: create_transform(translation=[0.6, 0, 0]),
brick:
create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]),
#brick: create_transform(translation=[0.2, -0.3, 0], rotation=[0, 0, np.pi/8]),
}
goal_poses = {
brick:
create_transform(translation=[0.8, 0.2, 0.2927],
rotation=[0, 0, np.pi / 8]),
}
goal_holding = [
#brick,
]
goal_on = [
#(brick, goal_surface),
]
diagram, state_machine = build_diagram(plant,
scene_graph,
robot,
gripper,
meshcat=use_meshcat,
controllers=use_controllers)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=[brick],
surfaces=surfaces,
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
goal_poses=goal_poses,
goal_holding=goal_holding,
goal_on=goal_on,
reset_robot=True,
reset_doors=False)
return task, diagram, state_machine
def load_station(time_step=0.0, **kwargs):
# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc
#object_file_path = FindResourceOrThrow(
# "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
# RuntimeError: Error control wants to select step smaller than minimum allowed (1e-14)
station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision)
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
robot = plant.GetModelInstanceByName('iiwa')
gripper = plant.GetModelInstanceByName('gripper')
station.AddCupboard()
brick = AddModelFromSdfFile(file_name=object_file_path,
model_name="brick",
plant=plant,
scene_graph=scene_graph)
station.Finalize()
door_names = ['left_door', 'right_door']
doors = [plant.GetBodyByName(name).index() for name in door_names]
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = {
plant.GetJointByName('left_door_hinge'):
-DOOR_CLOSED,
#plant.GetJointByName('left_door_hinge'): -np.pi / 2,
plant.GetJointByName('right_door_hinge'):
np.pi / 2,
}
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
brick:
create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]),
}
goal_poses = {
brick:
create_transform(translation=[0.8, 0.2, 0.2927],
rotation=[0, 0, 5 * np.pi / 4]),
}
diagram, state_machine = build_manipulation_station(station)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=[brick],
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
goal_poses=goal_poses,
reset_robot=True,
reset_doors=False)
task.station = station
return task, diagram, state_machine
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 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)
