def simple_push(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
place_body(env,
cylinder_body(env, .07, .05, name='blue_cylinder', color=BLUE),
(1.65, -.6, PI / 4), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
start_constrained = {'blue_cylinder': 'table1'}
goal_constrained = {
'blue_cylinder': ('table2',
Point(
get_point(env.GetKinBody('blue_cylinder')) +
np.array([-1.65, -1., 0.])))
#'blue_cylinder': ('table1', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-.2,.8, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
def decision_namo(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'navigation.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
place_body_on_floor(box_body(env, .1, .3, .8, name='red1', color=RED),
(0, -1.5, 0))
place_body_on_floor(box_body(env, .1, .3, .8, name='red2', color=RED),
(0, 1.5, 0))
place_body_on_floor(box_body(env, .1, .3, .8, name='red3', color=RED),
(-.5, 1.5, 0))
place_body_on_floor(box_body(env, .1, .3, .8, name='red4', color=RED),
(.5, 1.5, 0))
place_body_on_floor(box_body(env, .1, .3, .8, name='red5', color=RED),
(-1, 1.5, 0))
place_body_on_floor(box_body(env, .1, .3, .8, name='red6', color=RED),
(1, 1.5, 0))
floor_object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \
str(body.GetName()) not in obstacle_names]
goal_config = Config(np.array((1.5, 0, PI)))
goal_holding = False # False | None
goal_poses = {
#'blue': Pose(pose_from_base_values([0, 1.5, 0], z=BODY_PLACEMENT_Z_OFFSET))
}
return ManipulationProblem(function_name(inspect.stack()),
floor_object_names=floor_object_names,
goal_config=goal_config,
goal_holding=goal_holding,
goal_poses=goal_poses)
def nonmonotonic(env, N=4, M=3):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'regrasp.xml')
#env.Load(ENVIRONMENTS_DIR + '3tables.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE *
np.array([-1.5, 1.5, 3]),
look_point=(1.5, -1.5, 0))
pb, bb = place_body, box_body
pb(env, bb(env, .1, 1.5, .2, name='obst1', color=GREY), (1.75, 0, 0),
'table1')
pb(env, bb(env, 1.5, .1, .2, name='obst2', color=GREY), (0, -1.75, 0),
'table2')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2'] #, 'table3']
goal_poses = {}
for i in range(N):
displacement = -.75 + (1.5 * (i + .5)) / N
value = .25 + (.75 * (i + 1)) / N
if i < M:
green = 'green%s' % i
pb(env, bb(env, .05, .1, .2, name=green, color=(0, value, 0, 1)),
(1.75 - 0.1, displacement, 0), 'table1')
goal_poses[green] = Pose(
pose_from_quat_point(
quat_from_axis_angle(0, 0, -PI / 2),
np.array([displacement, -1.75 + 0.1, 0.7709])))
blue = 'blue%s' % i
pb(env, bb(env, .05, .1, .2, name=blue, color=(0, 0, value, 1)),
(1.75 - 0.2, displacement, 0), 'table1')
goal_poses[blue] = 'initial'
cyan = 'cyan%s' % i
pb(env, bb(env, .05, .1, .2, name=cyan, color=(0, value, value, 1)),
(displacement, -1.75 + 0.2, PI / 2), 'table2')
goal_poses[cyan] = 'initial'
#pb(env, bb(env, .05, .05, .1, name='red1', color=RED), (.1, -1.8, PI/16), 'table2')
#pb(env, bb(env, .15, .05, .15, name='red2', color=RED), (-.4, -1.95, PI/5), 'table2')
#pb(env, bb(env, .07, .07, .07, name='red3', color=RED), (.5, -1.9, PI/3), 'table2')
#pb(env, bb(env, .1, .1, .25, name='red4', color=RED), (1.9, -0.55, PI/7), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
goal_poses=goal_poses)
def move_regrasp(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'regrasp.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE *
np.array([-1.5, 1.5, 3]),
look_point=(1.5, -1.5, 0))
pb, bb = place_body, box_body
pb(env, bb(env, .3, .05, .3, name='obst1', color=GREY), (1.65, .075, 0),
'table1')
pb(env, bb(env, .3, .05, .3, name='obst2', color=GREY), (1.65, .425, 0),
'table1')
pb(env, bb(env, .05, .4, .3, name='obst3', color=GREY), (1.825, .25, 0),
'table1')
pb(env, bb(env, .3, .05, .3, name='obst4', color=GREY), (1.65, -.125, 0),
'table1')
pb(env, bb(env, .3, .05, .3, name='obst5', color=GREY), (1.65, -.375, 0),
'table1')
pb(env, bb(env, .05, .3, .3, name='obst6', color=GREY), (1.825, -.25, 0),
'table1')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
pb(env, bb(env, .03, .1, .2, name='green', color=GREEN), (1.55, 0.25, 0),
'table1')
pb(env, bb(env, .03, .1, .2, name='blue', color=BLUE), (1.5, 0.25, 0),
'table1')
#pb(env, bb(env, .05, .05, .1, name='red1', color=RED), (.1, -1.8, PI/16), 'table2')
#pb(env, bb(env, .15, .05, .15, name='red2', color=RED), (-.4, -1.95, PI/5), 'table2')
#pb(env, bb(env, .07, .07, .07, name='red3', color=RED), (.5, -1.9, PI/3), 'table2')
#pb(env, bb(env, .1, .1, .25, name='red4', color=RED), (1.9, -0.55, PI/7), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_poses = {
'green':
Pose(
pose_from_quat_point(quat_from_axis_angle(0, 0, PI / 2),
np.array([1.55, -0.25, 0.7709]))),
'blue':
'initial'
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
goal_poses=goal_poses)
def dantam(env): # (Incremental Task and Motion Planning: A Constraint-Based Approach)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
set_default_robot_config(env.GetRobots()[0])
set_point(env.GetRobots()[0], (-1.5, 0, 0))
m, n = 3, 3
#m, n = 5, 5
n_obj = 8
side_dim = .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
set_point(table, (0, 0, 0))
env.Add(table)
pose_indices = list(product(range(m), range(n)))
colors = {}
for r, c in pose_indices:
color = np.zeros(4)
color[2-r] = 1.
colors[(r, c)] = color + float(c)/(n-1)*np.array([1, 0, 0, 0])
poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for r, c in pose_indices:
x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0])
y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1])
poses[(r, c)] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
initial_indices = randomize(pose_indices[:])
initial_poses = {}
goal_poses = {}
for i, indices in enumerate(pose_indices[:n_obj]):
name = 'block%d-%d'%indices
color = colors[indices]
initial_poses[name] = poses[initial_indices[i]]
goal_poses[name] = poses[indices]
obj = box_body(env, *box_dims, name=name, color=color)
set_pose(obj, initial_poses[name].value)
env.Add(obj)
#for obj in randomize(objects):
# randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(function_name(inspect.stack()),
object_names=initial_poses.keys(), table_names=[get_name(table)],
goal_poses=goal_poses,
initial_poses=initial_poses, known_poses=poses.values())
def stacking(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE *
np.array([-1.5, 1.5, 3]),
look_point=(1.5, -1.5, 0))
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
regions = [
create_region(env,
'blue_goal', ((-1, 1), (-1, 0)),
'table1',
color=np.array((0, 0, 1, .5))),
create_region(env,
'green_goal', ((-1, -.5), (-1, 1)),
'table2',
color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='blue_box', color=BLUE),
(1.65, 0.115, PI / 3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='green_box', color=GREEN),
(1.55, -0.215, 7 * PI / 6), 'table1')
pb(env, box_body(env, .07, .07, .2, name='purple_box', color=BLACK),
(.15, -1.8, 7 * PI / 6), 'table2')
pb(env, box_body(env, .07, .07, .2, name='red_box1', color=RED),
(1.55, -0.215, 0), 'green_box')
#pb(env, box_body(env, .07, .07, .2, name='red_box2', color=RED), (1.65, .6, PI/4), 'table1')
#pb(env, box_body(env, .07, .07, .2, name='red_box3', color=RED), (.3, -1.9, PI/12), 'table2')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_stackings = {
# 'purple_box': 'blue_box' # target_cylinder
}
goal_regions = {'blue_box': 'blue_goal', 'green_box': 'green_goal'}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
regions=regions,
goal_stackings=goal_stackings,
goal_regions=goal_regions)
def distract_n(env, n):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE *
np.array([-1.5, 1.5, 3]),
look_point=(1.5, -1.5, 0))
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
regions = [
create_region(env,
'green_goal', ((-1, 1), (-1, 0)),
'table1',
color=np.array((0, 1, 0, .5))),
]
pb = place_body
center = np.array([1.75, 0.25, 0])
pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), center,
'table1')
id = count(1)
for dx, dy in [(-.15, 0), (.15, 0), (0, -.15), (0, .15)]:
pb(env,
box_body(env, .07, .05, .2, name='red_box%d' % next(id), color=RED),
center + np.array([dx, dy, 0]), 'table1')
#for x in irange(-.7, .7, .15):
# for y in irange(-2.0, -1.5, .15):
for x, y in islice(
product(irange(-.65, .75, .15), irange(-1.95, -1.5, .15)), n):
pb(env,
box_body(env, .07, .05, .2, name='red_box%d' % next(id), color=RED),
np.array([x, y, 0]), 'table2')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_regions = {
'green_box': 'green_goal',
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
regions=regions,
goal_regions=goal_regions)
def push_wall(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1, 1, 3]),
look_point=(3, 0, 0))
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
place_body(
env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN),
(1.65, -.5, PI / 4), 'table1')
#place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1')
#set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z())
place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED),
(1.50, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED),
(1.6, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED),
(1.7, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED),
(1.8, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED),
(1.9, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED),
(2.0, 0, 0), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
start_constrained = {'green_cylinder': 'table1'}
goal_constrained = {
'green_cylinder': ('table1',
Point(
get_point(env.GetKinBody('green_cylinder')) +
np.array([-.2, 1.0, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
def replace(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE *
np.array([-1.5, 1.5, 3]),
look_point=(1.5, -1.5, 0))
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
pb = place_body
id = count(1)
center = np.array([1.75, 0.25, 0])
pb(env, box_body(env, .07, .05, .2, name='green', color=GREEN), center,
'table1')
for dx, dy in [(-.15, 0), (.15, 0), (0, -.15), (0, .15)]:
pb(env, box_body(env, .07, .05, .2, name='red%d' % next(id),
color=RED), center + np.array([dx, dy, 0]), 'table1')
center = np.array([.25, -1.75, 0])
pb(env, box_body(env, .07, .05, .2, name='blue', color=BLUE), center,
'table1')
for dx, dy in [(-.15, 0), (.15, 0), (0, -.15), (0, .15)]:
pb(env, box_body(env, .07, .05, .2, name='red%d' % next(id),
color=RED), center + np.array([dx, dy, 0]), 'table2')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_poses = {
'green': 'blue',
'blue': 'green',
}
#for obj in object_names: # This make the problem highly non-monotonic and thus hard (still solved it though)
# if 'red' in obj:
# goal_poses[obj] = 'initial'
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
goal_poses=goal_poses)
def simple(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
#table_names = ['table1']
table_names = ['table1', 'table2']
regions = [
create_region(env,
'blue_goal', ((-1, 1), (-1, 0)),
'table1',
color=np.array((0, 0, 1, .5))),
]
pb = place_body
pb(env, box_body(env, .07, .05, .2, name='blue_box', color=BLUE),
(1.65, 0.115, PI / 3), 'table1')
pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN),
(1.55, -0.215, 7 * PI / 6), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_regions = {
'blue_box': 'blue_goal',
}
#goal_config = 'initial' # None
goal_config = None
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
regions=regions,
goal_config=goal_config,
goal_regions=goal_regions)
def srivastava_table(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae')
camera_trans = camera_look_at((1., -3., 4), look_point=(2, 1, 0))
body_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
dx = .5
for body_name in body_names:
body = env.GetKinBody(body_name)
point = get_point(body)
point[0] += dx
set_point(body, point)
table_names = [
body_name for body_name in body_names if 'table' in body_name
]
object_names = [
body_name for body_name in body_names if body_name not in table_names
]
for object_name in object_names:
body = env.GetKinBody(object_name)
point = get_point(body)
point[2] += BODY_PLACEMENT_Z_OFFSET
set_point(body, point)
goal_holding = 'object1'
goal_config = 'initial' # None
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
goal_config=goal_config,
goal_holding=goal_holding)
def trivial_namo(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'navigation.xml')
goal_config = Config(np.array((1.5, 0, PI)))
return ManipulationProblem(function_name(inspect.stack()),
goal_config=goal_config)
def dantam_distract(env, n_obj): # (Incremental Task and Motion Planning: A Constraint-Based Approach)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
m, n = 3, 3
#m, n = 5, 5
side_dim = .07 # .05 | .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
#separation = (side_dim/2, side_dim/2)
coordinates = list(product(range(m), range(n)))
assert n_obj <= len(coordinates)
obj_coordinates = sample(coordinates, n_obj)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
set_point(table, (0, 0, 0))
env.Add(table)
robot = env.GetRobots()[0]
set_default_robot_config(robot)
set_base_values(robot, (-1.5, 0, 0))
#set_base_values(robot, (0, width/2 + .5, math.pi))
#set_base_values(robot, (.35, width/2 + .35, math.pi))
#set_base_values(robot, (.35, width/2 + .35, 3*math.pi/4))
poses = []
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for r in range(m):
row = []
x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0])
for c in range(n):
y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1])
row.append(Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))))
poses.append(row)
initial_poses = {}
goal_poses = {}
# TODO - randomly assign goal poses here
for i, (r, c) in enumerate(obj_coordinates):
row_color = np.zeros(4)
row_color[2-r] = 1.
if i == 0:
name = 'goal%d-%d'%(r, c)
color = BLUE
goal_poses[name] = poses[m/2][n/2]
else:
name = 'block%d-%d'%(r, c)
color = RED
initial_poses[name] = poses[r][c]
obj = box_body(env, *box_dims, name=name, color=color)
set_pose(obj, poses[r][c].value)
env.Add(obj)
#for obj in randomize(objects):
# randomly_place_body(env, obj, [get_name(table)])
known_poses = list(flatten(poses))
#known_poses = list(set(flatten(poses)) - {poses[r][c] for r, c in obj_coordinates}) # TODO - put the initial poses here
return ManipulationProblem(function_name(inspect.stack()),
object_names=initial_poses.keys(), table_names=[get_name(table)],
goal_poses=goal_poses,
initial_poses=initial_poses, known_poses=known_poses)