def get_box_grasps(aabb,
max_width=DEFAULT_MAX_WIDTH,
grasp_length=DEFAULT_LENGTH,
orientations=list(range(4)),
pitch_range=(-np.pi / 2,
np.pi / 2)): # y is out of gripper initially
# TODO: different positions
center, extent = aabb
dx, dy, dz = extent
reflect_z = create_transform(rotation=[np.pi / 2, 0, 0])
aabb_from_body = create_transform(translation=center).inverse()
min_pitch, max_pitch = pitch_range
if min_pitch == max_pitch:
generator = ((min_pitch, ori) for ori in orientations)
else:
generator = ((random.uniform(min_pitch,
max_pitch), random.choice(orientations))
for _ in count())
for pitch, orientation in generator:
d1, d2 = (dx, dy) if (orientation % 2) == 1 else (dy, dx)
if 2 * d2 <= max_width:
rotate_z = create_transform(
rotation=[0, 0, orientation * np.pi / 2])
rotate_x = create_transform(rotation=[pitch, 0, 0])
threshold = math.atan2(d1, dz)
distance = dz / math.cos(pitch) if abs(
pitch) < threshold else d1 / abs(math.sin(pitch))
translate_z = create_transform(
translation=[0, 0, -TOOL_Z - distance + grasp_length])
yield reflect_z.multiply(translate_z).multiply(rotate_x).multiply(
rotate_z).multiply(aabb_from_body)
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 interpolate_translation(transform, translation, step_size=0.01):
distance = np.linalg.norm(translation)
if distance == 0:
yield transform
return
direction = np.array(translation) / distance
for t in list(np.arange(0, distance, step_size)) + [distance]:
yield transform.multiply(create_transform(translation=t * direction))
def get_top_cylinder_grasps(
aabb,
max_width=DEFAULT_MAX_WIDTH,
grasp_length=DEFAULT_LENGTH): # y is out of gripper initially
center, extent = aabb
w, l, h = 2 * extent
reflect_z = create_transform(rotation=[np.pi / 2, 0, 0])
translate_z = create_transform(
translation=[0, 0, -TOOL_Z - (h / 2) + grasp_length])
aabb_from_body = create_transform(translation=center).inverse()
diameter = (w + l) / 2 # TODO: check that these are close
if max_width < diameter:
return
while True:
theta = random.uniform(0, 2 * np.pi)
rotate_z = create_transform(rotation=[0, 0, theta])
yield reflect_z.multiply(translate_z).multiply(rotate_z).multiply(
aabb_from_body)
def weld_gripper(mbp, robot_index, gripper_index):
X_EeGripper = create_transform([0, 0, 0.081], [np.pi / 2, 0, np.pi / 2])
robot_body = get_model_bodies(mbp, robot_index)[-1]
gripper_body = get_model_bodies(mbp, gripper_index)[0]
mbp.AddJoint(
WeldJoint(name="weld_gripper_to_robot_ee",
parent_frame_P=robot_body.body_frame(),
child_frame_C=gripper_body.body_frame(),
X_PC=X_EeGripper))
def load_tables(time_step=0.0):
mbp = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
# TODO: meshes aren't supported during collision checking
robot = AddModelFromSdfFile(file_name=IIWA14_SDF_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
table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table',
scene_graph=scene_graph, plant=mbp)
table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table2',
scene_graph=scene_graph, plant=mbp)
sink = AddModelFromSdfFile(file_name=SINK_PATH, model_name='sink',
scene_graph=scene_graph, plant=mbp)
stove = AddModelFromSdfFile(file_name=STOVE_PATH, model_name='stove',
scene_graph=scene_graph, plant=mbp)
broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH, model_name='broccoli',
scene_graph=scene_graph, plant=mbp)
#wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall',
# scene_graph=scene_graph, plant=mbp)
wall = None
table2_x = 0.75
table_top_z = get_aabb_z_placement(AABBs['sink'], AABBs['table']) # TODO: use geometry
weld_gripper(mbp, robot, gripper)
weld_to_world(mbp, robot, create_transform(translation=[0, 0, table_top_z]))
weld_to_world(mbp, table, create_transform())
weld_to_world(mbp, table2, create_transform(translation=[table2_x, 0, 0]))
weld_to_world(mbp, sink, create_transform(translation=[table2_x, 0.25, table_top_z]))
weld_to_world(mbp, stove, create_transform(translation=[table2_x, -0.25, table_top_z]))
if wall is not None:
weld_to_world(mbp, wall, create_transform(translation=[table2_x / 2, 0, table_top_z]))
mbp.Finalize(scene_graph)
movable = [broccoli]
surfaces = [
VisualElement(sink, 'base_link', 0), # Could also just pass the link index
VisualElement(stove, 'base_link', 0),
]
initial_poses = {
broccoli: create_transform(translation=[table2_x, 0, table_top_z]),
}
task = Task(mbp, scene_graph, robot, gripper,
movable=movable, surfaces=surfaces,
initial_poses=initial_poses,
goal_cooked=[broccoli])
return mbp, scene_graph, task
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_tables(time_step=0.0, use_meshcat=True, use_controllers=True):
plant = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
# TODO: meshes aren't supported during collision checking
robot = AddModelFromSdfFile(file_name=IIWA14_SDF_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=TABLE_SDF_PATH,
model_name='table',
scene_graph=scene_graph,
plant=plant)
table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH,
model_name='table2',
scene_graph=scene_graph,
plant=plant)
sink = AddModelFromSdfFile(file_name=SINK_PATH,
model_name='sink',
scene_graph=scene_graph,
plant=plant)
stove = AddModelFromSdfFile(file_name=STOVE_PATH,
model_name='stove',
scene_graph=scene_graph,
plant=plant)
broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH,
model_name='broccoli',
scene_graph=scene_graph,
plant=plant)
#wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall',
# scene_graph=scene_graph, plant=mbp)
wall = None
table2_x = 0.75
table_top_z = 0.7655 # TODO: use geometry
weld_gripper(plant, robot, gripper)
weld_to_world(plant, robot,
create_transform(translation=[0, 0, table_top_z]))
weld_to_world(plant, table, create_transform())
weld_to_world(plant, table2,
create_transform(translation=[table2_x, 0, 0]))
weld_to_world(plant, sink,
create_transform(translation=[table2_x, 0.25, table_top_z]))
weld_to_world(plant, stove,
create_transform(translation=[table2_x, -0.25, table_top_z]))
if wall is not None:
weld_to_world(
plant, wall,
create_transform(translation=[table2_x / 2, 0, table_top_z]))
plant.Finalize(scene_graph)
movable = [broccoli]
surfaces = [
Surface(plant, sink, 'base_link',
0), # Could also just pass the link index
Surface(plant, stove, 'base_link', 0),
]
initial_poses = {
broccoli: create_transform(translation=[table2_x, 0, table_top_z]),
}
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=movable,
surfaces=surfaces,
initial_poses=initial_poses,
goal_cooked=[broccoli])
return task, diagram, state_machine
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
