def two_tables_through_door(env, obj_length, n=7): # Previously 4, 8
env.Load(ENVIRONMENTS_DIR + 'two_tables'+str(obj_length)+'.xml')
#set_default_robot_config(env.GetRobots()[0])
table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()])
objects = []
# objects.append(env.GetKinBody("ObjToG"))
goal_regions = {}
for i in range(2*n):
objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED))
for i in range(n):
name = 'green'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=GREEN))
goal_regions[name] = 'table1'
for i in range(n):
name = 'blue'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE))
goal_regions[name] = 'table3'
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, ['table2', 'table4'])
return ManipulationProblem(None,
object_names=object_names, table_names=table_names,
goal_regions=goal_regions)
def simple_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))
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 distractions(env):
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 product((-.15, 0, .15), repeat=2):
if not (dx == 0 and dy == 0):
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 in irange(-.65, .75, .15):
for y in irange(-1.95, -1.5, .15):
pb(env, box_body(env, .05, .05, .15, 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 dinner(env):
env.Load(ENVIRONMENTS_DIR + 'kitchen.xml')
obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
table_names = ['table1']
sink_names = ['washer']
stove_names = ['heater']
regions = [
create_region(env, 'blue_goal', ((-1, 1), (-1, -.5)), 'table1', color=np.array((0, 0, 1, .5))),
create_region(env, 'green_goal', ((-1, 1), (.5, 1)), 'table1', color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='blue', color=BLUE), (1.65, 0.115, PI/3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='green', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red1', color=RED), (1.6, 0.615, 0), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red2', color=RED), (1.7, -0.415, PI/8), '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': 'blue_goal',
'green': 'green_goal'
}
goal_cleaned = ['blue']
goal_cooked = ['green']
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names, table_names=table_names, sink_names=sink_names, stove_names=stove_names, regions=regions,
goal_regions=goal_regions, goal_cleaned=goal_cleaned, goal_cooked=goal_cooked)
def two_tables_through_door(env, obj_length, n=7): # Previously 4, 8
env.Load(ENVIRONMENTS_DIR + 'two_tables'+str(obj_length)+'.xml')
#env.Load(ENVIRONMENTS_DIR + 'two_tables'+'.xml')
#set_default_robot_config(env.GetRobots()[0])
table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()])
objects = []
# objects.append(env.GetKinBody("ObjToG"))
goal_regions = {}
for i in range(2*n):
objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED))
for i in range(n):
name = 'green'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=GREEN))
goal_regions[name] = 'table1'
for i in range(n):
name = 'blue'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE))
goal_regions[name] = 'table3'
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, ['table2', 'table4'])
# randomly_place_body(env, obj, ['table2'])
return ManipulationProblem(None,
object_names=object_names, table_names=table_names,
goal_regions=goal_regions)
def separate(env, n=7): # Previously 4, 8
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'tables.xml')
set_default_robot_config(env.GetRobots()[0])
table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()])
objects = []
goal_regions = {}
for i in range(2*n):
objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED))
for i in range(n):
name = 'green'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=GREEN))
goal_regions[name] = 'table1'
for i in range(n):
name = 'blue'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE))
goal_regions[name] = 'table3'
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, ['table2', 'table4'])
return ManipulationProblem(None,
object_names=object_names, table_names=table_names,
goal_regions=goal_regions)
def two_tables(env, n=2):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + '2tables.xml')
set_default_robot_config(env.GetRobots()[0])
table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()])
#m = 4*n
objects = []
goal_regions = {}
#for i in range(4*m):
for i in range(10*n):
objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED))
#for i in range(n):
for i in range(1):
name = 'blue'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE))
goal_regions[name] = 'table2'
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, ['table1'])
return ManipulationProblem(None,
object_names=object_names, table_names=table_names,
goal_regions=goal_regions)
def pap_ir_oracle(env): # TODO - vary object and table geometry
from manipulation.problems.fixed import ENVIRONMENTS_DIR # NOTE - prevents circular imports
from manipulation.oracle import ManipulationOracle
from manipulation.problems import ManipulationProblem
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
table_name = 'table'
env.Add(box_body(env, 1., 1., .75, name=table_name))
body_name = 'body'
env.Add(box_body(env, .07, .07, .2, name=body_name))
return ManipulationOracle(ManipulationProblem(None, object_names=[body_name],
table_names=[table_name]), env, preload_databases=False, debug=False)
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 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 kitchen(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'kitchen.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1']
sink_names = ['washer']
#stove_names = ['heater']
stove_names = ['microwave']
regions = [
create_region(env,
'blue_goal', ((-1, 1), (-1, -.5)),
'table1',
color=np.array((0, 0, 1, .5))),
create_region(env,
'green_goal', ((-1, 1), (.5, 1)),
'table1',
color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='blue', color=BLUE),
(1.65, 0.115, PI / 3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='green', color=GREEN),
(1.55, -0.215, 7 * PI / 6), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red1', color=RED),
(1.6, 0.615, 0), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red2', color=RED),
(1.7, -0.415, PI / 8), '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': 'blue_goal', 'green': 'green_goal'}
goal_cleaned = ['blue']
goal_cooked = ['green']
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
sink_names=sink_names,
stove_names=stove_names,
regions=regions,
goal_regions=goal_regions,
goal_cleaned=goal_cleaned,
goal_cooked=goal_cooked)
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 create_obstacles(env, loading_regions):
NUM_OBSTACLES = 4
OBSTACLES = []
obstacle_poses = {}
obstacle_shapes = {}
i = 0
while len(OBSTACLES) < NUM_OBSTACLES:
width = np.random.rand(1) * (max_width - min_width) + min_width
length = np.random.rand(1) * (max_length - min_length) + min_length
height = np.random.rand(1) * (max_height - min_height) + min_height
new_body = box_body(env,width,length,height,\
name='obst%s'%len(OBSTACLES),\
color=(0, (i+.5)/NUM_OBSTACLES, 1))
trans = np.eye(4)
trans[2, -1] = 0.075
env.Add(new_body)
new_body.SetTransform(trans)
xytheta = randomly_place_in_region(env,new_body,\
loading_regions[np.random.randint(len(loading_regions))])
if not (xytheta is None):
obstacle_shapes['obst%s' %
len(OBSTACLES)] = [width[0], length[0], height[0]]
obstacle_poses['obst%s' % len(OBSTACLES)] = xytheta
OBSTACLES.append(new_body)
else:
env.Remove(new_body)
return OBSTACLES, obstacle_shapes, obstacle_poses
def pap_ir_oracle(env): # TODO - vary object and table geometry
from manipulation.problems.fixed import ENVIRONMENTS_DIR # NOTE - prevents circular imports
from manipulation.oracle import ManipulationOracle
from manipulation.problems import ManipulationProblem
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
table_name = 'table'
#env.Add(box_body(env, 1., 1., .75, name=table_name)) # NOTE - .75 is too tall for top grasps
env.Add(box_body(env, 1., 1., .7, name=table_name))
body_name = 'body'
env.Add(box_body(env, .07, .07, .2, name=body_name))
return ManipulationOracle(ManipulationProblem(None,
object_names=[body_name],
table_names=[table_name]),
env,
preload_databases=False,
debug=False)
def create_objects(env, conveyor_belt):
num_objects = 5
objects = []
obj_shapes = {}
obj_poses = {}
for i in range(num_objects):
if i > 10 and i < 15:
min_width = 0.7
max_width = 0.7
min_length = 0.6
else:
min_width = 0.2
max_width = 0.6
min_length = 0.2
width = np.random.rand(1) * (max_width - min_width) + min_width
length = np.random.rand(1) * (max_width - min_length) + min_length
height = np.random.rand(1) * (max_height - min_height) + min_height
new_body = box_body(env, width, length, height, \
name='obj%s' % i, \
color=(0, (i + .5) / num_objects, 0))
trans = np.eye(4)
trans[2, -1] = 0.075
env.Add(new_body)
new_body.SetTransform(trans)
xytheta = randomly_place_in_region(env, new_body, conveyor_belt)
objects.append(new_body)
obj_shapes['obj%s' % i] = [width[0], length[0], height[0]]
obj_poses['obj%s' % i] = xytheta
return objects, obj_shapes, obj_poses
def decision_wall_namo(
env, rows, cols
): # TODO - could make variant that requires replacing things as well
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'decision_2tables.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']
for x in range(cols):
for y in range(rows):
#if y >= rows/2 and x >= cols/2: continue
if y >= rows / 2 and x >= cols - 1:
continue # NOTE - makes one side harder than the other
place_body_on_floor(
box_body(env, .1, .1, .8, name='red%s-%s' % (x, y), color=RED),
(-.25 + x * .75 / cols, -2.5 + (y + .5) * 5 / rows, 0))
place_body(env, box_body(env, .07, .07, .2, name='blue0', color=BLUE),
(1.75, +0.2, 0), 'table1')
place_body(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE),
(1.75, -0.2, 0), 'table1')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \
str(body.GetName()) not in obstacle_names and 'blue' in str(body.GetName())]
floor_object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \
str(body.GetName()) not in obstacle_names and 'red' in str(body.GetName())]
goal_config = 'initial'
goal_holding = None # False | None
goal_regions = {
'blue0': 'table2',
'blue1': 'table2',
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
floor_object_names=floor_object_names,
object_names=object_names,
table_names=table_names,
goal_config=goal_config,
goal_holding=goal_holding,
goal_regions=goal_regions)
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 move_several(env, n):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
box_dims = (.07, .07, .2)
#separation = (.08, .08)
separation = (.10, .10)
length = math.sqrt(n+1)*(box_dims[0] + separation[0])
width = math.sqrt(n+1)*(box_dims[1] + separation[1])
height = .7
table1 = box_body(env, length, width, height, name='table1', color=get_color('tan1'))
set_point(table1, (0, 0, 0))
env.Add(table1)
table2 = box_body(env, length, width, height, name='table2', color=get_color('tan1'))
set_point(table2, (1.5, 0, 0))
env.Add(table2)
robot = env.GetRobots()[0]
set_default_robot_config(robot)
set_base_values(robot, (-1.5, 0, 0))
# TODO - place walls and/or a roof to make more similar to pebble graph people
objects = []
goal_regions = {}
obj = box_body(env, .07, .07, .2, name='blue', color=BLUE)
set_point(obj, (0, 0, height + BODY_PLACEMENT_Z_OFFSET))
objects.append(obj)
goal_regions[get_name(obj)] = get_name(table2)
env.Add(obj)
for i in range(n):
objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED))
for obj in randomize(objects[1:]):
randomly_place_body(env, obj, [get_name(table1)])
return ManipulationProblem(None,
object_names=[get_name(body) for body in objects], table_names=[get_name(table) for table in [table1, table2]],
goal_regions=goal_regions)
def grid_arrangement(env, m, n): # (Dealing with Difficult Instances of Object Rearrangment)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
box_dims = (.12, .04, .08)
#separation = (.08, .08)
separation = (.12, .12)
#separation = (.16, .16)
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, (1.75, 0, 0))
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))
objects = []
goal_poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for i in range(m):
x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0])
row_color = np.zeros(4)
row_color[2-i] = 1.
for j in range(n):
y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1])
name = 'block%d-%d'%(i, j)
color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0])
goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
objects.append(box_body(env, *box_dims, name=name, color=color))
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(None,
object_names=object_names, table_names=[get_name(table)],
goal_poses=goal_poses)
def separate(env, n=7): # Previously 4, 8
env.Load(ENVIRONMENTS_DIR + 'tables.xml')
set_default_robot_config(env.GetRobots()[0])
table_names = filter(lambda name: 'table' in name, [get_name(body) for body in env.GetBodies() if not body.IsRobot()])
objects = []
goal_regions = {}
for i in range(2*n):
objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED))
for i in range(n):
name = 'green'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=GREEN))
goal_regions[name] = 'table1'
for i in range(n):
name = 'blue'+str(i+1)
objects.append(box_body(env, .07, .07, .2, name=name, color=BLUE))
goal_regions[name] = 'table3'
objects.append(box_body(env, .07, .07, .2, name='black', color=BLACK))
object_names = [get_name(body) for body in objects]
robot = env.GetRobots()[0]
robot.SetActiveManipulator('leftarm')
print robot.GetActiveManipulator().GetLocalToolTransform()
grasps = {}
#for obj_name in object_names:
obj_name = 'black'
env.Add(objects[-1])
obj = env.GetKinBody(obj_name)
with obj:
obj.SetTransform(np.eye(4))
obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.GRASP)
#obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH) # TOP and SIDE are swapped
grasps[get_name(obj)] = obj_grasps
for obj in randomize(objects):
randomly_place_body(env, obj, ['table2', 'table4'])
return ManipulationProblem(None,
object_names=object_names, table_names=table_names,
goal_regions=goal_regions,grasps=grasps)
def push_wall(env):
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 stacking(env):
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 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 shelf_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment)
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
#m, n = 2, 10
m, n = 2, 4
box_dims = (.07, .07, .2)
#separation = (.08, .08)
separation = (.15, .15)
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, (1.75, 0, 0))
env.Add(table)
# TODO - place walls and/or a roof to make more similar to pebble graph people
objects = []
goal_poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for i in range(m):
x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0])
row_color = np.zeros(4)
row_color[2-i] = 1.
for j in range(n):
y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1])
name = 'block%d-%d'%(i, j)
color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0])
goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
objects.append(box_body(env, *box_dims, name=name, color=color))
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(None,
object_names=object_names, table_names=[get_name(table)],
goal_poses=goal_poses)
def shelf_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment)
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
#m, n = 2, 10
m, n = 2, 4
box_dims = (.07, .07, .2)
#separation = (.08, .08)
separation = (.15, .15)
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, (1.75, 0, 0))
env.Add(table)
# TODO - place walls and/or a roof to make more similar to pebble graph people
objects = []
goal_poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for i in range(m):
x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0])
row_color = np.zeros(4)
row_color[2-i] = 1.
for j in range(n):
y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1])
name = 'block%d-%d'%(i, j)
color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0])
goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
objects.append(box_body(env, *box_dims, name=name, color=color))
object_names = [get_name(body) for body in objects]
print object_names
for obj in randomize(objects):
randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(None,
object_names=object_names, table_names=[get_name(table)],
goal_poses=goal_poses)
def easy_dinner(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'dinner.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'shelves', 'plate1', 'kitchen_table']
sink_names = ['washer']
stove_names = ['microwave']
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='cyan2', color=CYAN),
(1.7, 0.0, PI / 4), 'table1')
pb(env, box_body(env, .1, .1, .1, name='green1', color=GREEN),
(-1.7, 1.8, 0), 'shelves')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_regions = {
'green1': 'plate1',
}
goal_cleaned = ['cyan2']
goal_cooked = ['green1']
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
sink_names=sink_names,
stove_names=stove_names,
goal_regions=goal_regions,
goal_cleaned=goal_cleaned,
goal_cooked=goal_cooked)
def simple(env):
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
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, .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',
'green_box': 'green_goal'
}
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names, table_names=table_names, regions=regions,
goal_regions=goal_regions)
def create_obstacles(env, loading_regions):
num_obstacles = 3
obstacles = []
obstacle_poses = {}
obstacle_shapes = {}
i = 0
min_width = 0.2
max_width = 0.4
min_length = 0.2
max_length = 3
max_height = 0.5
while len(obstacles) < num_obstacles:
if len(obstacles) == 0:
min_length = 1.8
max_length = 1.8
else:
min_length = 0.2
max_length = 0.8
width = np.random.rand(1) * (max_width - min_width) + min_width
length = np.random.rand(1) * (max_length - min_length) + min_length
height = np.random.rand(1) * (max_height - min_height) + min_height
new_body = box_body(env,
width,
length,
height,
name='obst%s' % len(obstacles),
color=(0, (i + .5) / num_obstacles, 1))
trans = np.eye(4)
trans[2, -1] = 0.075
env.Add(new_body)
new_body.SetTransform(trans)
xytheta = randomly_place_in_region(env, new_body, loading_regions)
#loading_regions[np.random.randint(len(loading_regions))])
if not (xytheta is None):
obstacle_shapes['obst%s' %
len(obstacles)] = [width[0], length[0], height[0]]
obstacle_poses['obst%s' % len(obstacles)] = xytheta
obstacles.append(new_body)
else:
raise ValueError, 'Not enough spot for obstacles'
#env.Remove(new_body)
return obstacles, obstacle_shapes, obstacle_poses
def load_objects(env, obj_shapes, obj_poses, color):
objects = []
i = 0
nobj = len(obj_shapes.keys())
for obj_name in obj_shapes.keys():
xytheta = obj_poses[obj_name].squeeze()
width, length, height = obj_shapes[obj_name]
quat = quat_from_z_rot(xytheta[-1])
new_body = box_body(env, width, length, height, \
name=obj_name, \
color=np.array(color) / float(nobj - i))
i += 1
env.Add(new_body)
set_point(new_body, [xytheta[0], xytheta[1], 0.075])
set_quat(new_body, quat)
objects.append(new_body)
return objects
def draw_q_value_rod_for_action(action_idx, base_pose, q_val, penv, maxQ):
normalized_q = q_val / float(maxQ)
width = 0.2
length = 0.2
height = normalized_q
if height == 0.5:
color = (0, 0, 1)
else:
color = (0.8, 0.2, 0.5)
new_body = box_body(penv.env,
width,
length,
height,
name='q_value_obj%s' % action_idx,
color=color)
penv.env.Add(new_body)
if base_pose is not None:
set_obj_xytheta(base_pose, new_body)
def restore_env(curr_env, past_env):
assert len(curr_env.GetBodies()) == 7 # make sure the curr env is "empty" with only ObjToG in it
curr_env_objtog = curr_env.GetKinBody("ObjToG")
curr_env_objtog.SetTransform(past_env.GetKinBody("ObjToG").GetTransform())
new_obj_idx = 1
for past_env_body in past_env.GetBodies():
if (
past_env_body.GetName() == "pr2"
or past_env_body.GetName() == "floorwalls"
or past_env_body.GetName() == "table2"
or past_env_body.GetName() == "table1"
or past_env_body.GetName() == "table4"
or past_env_body.GetName() == "table3"
or past_env_body.GetName() == "ObjToG"
):
continue
new_obj = box_body(curr_env, 0.07, 0.07, 0.2, name="red" + str(new_obj_idx), color=RED)
new_obj.SetTransform(past_env_body.GetTransform())
new_obj_idx += 1
curr_env.AddKinBody(new_obj)
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 create_objects(env, conveyor_belt):
NUM_OBJECTS = 5
OBJECTS = []
obj_shapes = {}
obj_poses = {}
for i in range(NUM_OBJECTS):
width = np.random.rand(1) * (max_width - min_width) + min_width
length = np.random.rand(1) * (max_width - min_length) + min_length
height = np.random.rand(1) * (max_height - min_height) + min_height
new_body = box_body(env,width,length,height,\
name='obj%s'%i,\
color=(0, (i+.5)/NUM_OBJECTS, 0))
trans = np.eye(4)
trans[2, -1] = 0.075
env.Add(new_body)
new_body.SetTransform(trans)
xytheta = randomly_place_in_region(env, new_body, conveyor_belt,
np.array([0]))
OBJECTS.append(new_body)
obj_shapes['obj%s' % i] = [width[0], length[0], height[0]]
obj_poses['obj%s' % i] = xytheta
return OBJECTS, obj_shapes, obj_poses
def create_objects(env, conveyor_belt, num_objects):
objects = []
obj_shapes = {}
obj_poses = {}
for i in range(num_objects):
#if 0 <= i < 0:
if i == 0 or i == 1:
min_width = 0.6
max_width = 0.7
min_length = 0.7
name = 'big_obj'
else:
min_width = 0.4
max_width = 0.5
min_length = 0.6
name = 'small_obj'
width = np.random.rand(1) * (max_width - min_width) + min_width
length = np.random.rand(1) * (max_width - min_length) + min_length
height = np.random.rand(1) * (max_height - min_height) + min_height
new_body = box_body(env,
width,
length,
height,
name=name + '%s' % i,
color=(0, 0.5, 0))
trans = np.eye(4)
trans[2, -1] = 0.075
env.Add(new_body)
new_body.SetTransform(trans)
xytheta = randomly_place_in_region(env, new_body, conveyor_belt)
objects.append(new_body)
obj_shapes['obj%s' % i] = [width[0], length[0], height[0]]
obj_poses['obj%s' % i] = xytheta
return objects, obj_shapes, obj_poses
def dinner(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'dinner.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'shelves', 'plate1', 'plate2', 'kitchen_table']
sink_names = ['washer']
stove_names = ['microwave']
regions = [
#create_region(env, 'blue_goal', ((-1, 1), (-1, -.5)), 'table1', color=np.array((0, 0, 1, .5))),
#create_shelf_region(env, 'shelf', ((-1, 1), (-1, 1)), 'shelves', .99),
]
pb = place_body
#pb(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE), (1.5, -0.4, 0), 'table1')
#pb(env, box_body(env, .07, .07, .2, name='blue2', color=BLUE), (1.5, 0.4, 0), 'table1')
pb(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE),
(1.6, -0.1, PI / 3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='blue2', color=BLUE),
(1.5, 0.15, PI / 6), 'table1')
#pb(env, box_body(env, .07, .07, .2, name='cyan1', color=CYAN), (1.55, -0.5, PI/5), 'table1') # NOTE - just need to be cleaned
pb(env, box_body(env, .07, .07, .2, name='cyan2', color=CYAN),
(1.7, 0.0, PI / 4), 'table1')
pb(env, box_body(env, .1, .1, .1, name='green1', color=GREEN),
(-1.7, 1.8, 0), 'shelves')
pb(env, box_body(env, .1, .1, .1, name='green2', color=GREEN),
(-1.3, 1.8, 0), 'shelves')
#set_body(env, box_body(env, .1, .1, .15, name='green1', color=GREEN), (-1.7, 1.85, .991), 0)
#set_body(env, box_body(env, .1, .1, .15, name='green2', color=GREEN), (-1.3, 1.85, .991), 0)
pb(env, box_body(env, .07, .07, .07, name='pink1', color=PINK),
(-1.7, 1.7, 0), 'shelves')
pb(env, box_body(env, .07, .07, .07, name='pink2', color=PINK),
(-1.3, 1.7, 0), 'shelves')
pb(env, box_body(env, .07, .07, .07, name='pink3', color=PINK),
(-1.8, 1.8, 0), 'shelves')
pb(env, box_body(env, .07, .07, .07, name='pink4', color=PINK),
(-1.2, 1.8, 0), 'shelves')
#pb(env, box_body(env, .1, .1, .1, name='red1', color=RED), (-1.5, 1.8, 0), 'shelves') # NOTE - next to green
#pb(env, box_body(env, .07, .07, .2, name='red1', color=RED), (1.6, 0.615, 0), 'table1')
#pb(env, box_body(env, .07, .07, .2, name='red2', color=RED), (1.7, -0.215, PI/8), 'table1')
#set_body(env, box_body(env, .07, .07, .2, name='red3', color=RED), (-2, 0.1, 1.07), 0)
#set_body(env, box_body(env, .07, .07, .2, name='red4', color=RED), (-2, -0.1, .79), 0)
#set_body(env, box_body(env, .07, .07, .2, name='red3', color=RED), (-2, 0.1, .99), 0)
#set_body(env, box_body(env, .07, .07, .2, name='red4', color=RED), (-2, -0.1, .99), 0)
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_poses = {
'blue1':
Pose(
pose_from_quat_point(quat_from_axis_angle(0, 0, 0),
np.array([1.5, -0.4, .701]))),
'blue2':
Pose(
pose_from_quat_point(quat_from_axis_angle(0, 0, 0),
np.array([1.5, 0.4, .701]))),
#'blue1': 'initial',
#'blue2': 'initial',
'pink1':
'initial',
'pink2':
'initial',
'pink3':
'initial',
'pink4':
'initial',
}
# TODO - food table
# TODO - Pantry table
goal_regions = {
#'blue1': 'blue_goal',
#'green1': 'green_goal',
'green1': 'plate1',
'green2': 'plate2',
}
goal_cleaned = ['blue1', 'blue2'] + ['cyan2']
#goal_cleaned = ['blue1', 'blue2', 'cyan1', 'cyan2']
goal_cooked = ['green1', 'green2']
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
sink_names=sink_names,
stove_names=stove_names,
regions=regions,
goal_poses=goal_poses,
goal_regions=goal_regions,
goal_cleaned=goal_cleaned,
goal_cooked=goal_cooked)
def main():
env=Environment()
env.Load('env.xml')
robot = env.GetRobots()[0]
#set_default_robot_config(robot)
robot_initial_pose = get_pose(robot)
leftarm_manip = robot.GetManipulator('leftarm')
robot.SetActiveManipulator('leftarm')
ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel1.load():
ikmodel1.autogenerate()
rightarm_manip = robot.GetManipulator('rightarm')
rightarm_torso_manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveManipulator('rightarm_torso')
manip = robot.GetActiveManipulator()
ee = manip.GetEndEffector()
ikmodel2 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel2.load():
ikmodel2.autogenerate()
set_config(robot,FOLDED_LEFT_ARM,robot.GetManipulator('leftarm').GetArmIndices())
set_config(robot,mirror_arm_config(FOLDED_LEFT_ARM),\
robot.GetManipulator('rightarm').GetArmIndices())
robot_initial_config = np.array([-1,1,0])
set_robot_config(robot_initial_config,robot)
env.SetViewer('qtcoin')
target = env.GetKinBody('target')
width = 0.1 #np.random.rand(1)*(max_width-min_width)+min_width
length = 0.8 #np.random.rand(1)*(max_length-min_length) + min_length
height = 1.0 #np.random.rand(1)*(max_height-min_height)+min_height
new_body = box_body(env,width,length,height,\
name='obst',\
color=(0, 5, 1))
trans = np.eye(4);
trans[2,-1] = 0.075
#env.Add(new_body);
#new_body.SetTransform(trans)
all_region = region = AARegion('all_region',((-2.51,2.51),(-2.51,2.51)),\
z=0.0001,color=np.array((1,1,0,0.25)))
activate_arms_torso_base(robot)
# Try picking different objects
pick_obj = lambda obj_name: pick(obj_name,env,robot,all_region)
import pdb;pdb.set_trace()
pbase,grasp,g_config = sample_pick(env.GetRobot('valkyrie'),robot,env,all_region)
pick_obj(env.GetRobot('valkyrie'),robot,g_config,leftarm_manip,rightarm_manip)
import pdb;pdb.set_trace()
path,tpath,status = get_motion_plan(robot,robot_xytheta,env,maxiter=10,n_node_lim=5000)
import pdb;pdb.set_trace()
sample_pick(target,robot,env,all_region)
import pdb;pdb.set_trace()
def rearrangement_problem(env,obj_poses=None):
# import random
# seed = 50
# random.seed(seed)
# np.random.seed(seed)
MIN_DELTA = .01 # .01 | .02
ENV_FILENAME = ENVIRONMENTS_DIR+'/single_table.xml'
env.Load(ENV_FILENAME)
robot = env.GetRobots()[0]
TABLE_NAME = 'table1'
NUM_OBJECTS = 8
WIDTH = .05 # .07 | .1
TARGET_WIDTH = 0.07
OBJ_HEIGHT = 0.2
objects = [box_body(env, WIDTH, WIDTH, .2, name='obj%s'%i, \
color=(0, (i+.5)/NUM_OBJECTS, 0)) for i in range(NUM_OBJECTS)]
target_obj = box_body(env, TARGET_WIDTH, TARGET_WIDTH, .2, name='target_obj', \
color=(1, 1.5, 0))
#TODO: Place obstacle that prevent you to reach from top
OBST_X = 0
OBST_WIDTH = 1.
OBST_COLOR = (1, 0, 0)
OBST_HEIGHT = 0.4
OBST_TRANSPARENCY = .25
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst1', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X-(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst2', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X+(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .05, OBST_WIDTH, OBST_HEIGHT, name='obst3', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.225, OBST_X, 0), TABLE_NAME)
# roof
OBST_Z = OBST_HEIGHT
place_xyz_body(env, box_body(env, .4, OBST_WIDTH, .01, name='obst4', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(0, OBST_X, OBST_Z,0), TABLE_NAME)
# I think this can be done once and for all
REST_TORSO = [.15]
robot.SetDOFValues(REST_TORSO, [robot.GetJointIndex('torso_lift_joint')])
l_model = databases.linkstatistics.LinkStatisticsModel(robot)
if not l_model.load(): l_model.autogenerate()
l_model.setRobotWeights()
min_delta = 0.01
l_model.setRobotResolutions(xyzdelta=min_delta) # xyzdelta is the minimum Cartesian distance of an object
extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T
robot.SetAffineTranslationLimits(*extrema)
X_PERCENT = 0.0
# define intial region for movable objects
init_region = create_region(env, 'init_region', ((-0.8, 0.8), (-0.7, 0.6)), TABLE_NAME, color=np.array((1, 0, 1, .5)))
#init_region.draw(env)
# define target object region
target_obj_region = create_region(env, 'target_obj_region', ((-0.2, 0.6), (-0.5,0.5)), \
TABLE_NAME, color=np.array((0, 0, 0, 0.9)))
target_obj_region.draw(env)
# place object
if obj_poses is not None:
for obj in objects:
set_pose(obj,obj_poses[get_name(obj)])
set_quat(obj, quat_from_z_rot(0))
if env.GetKinBody(get_name(obj)) is None: env.Add(obj)
set_pose( target_obj, obj_poses[get_name(target_obj)] )
set_quat( target_obj, quat_from_z_rot(0) )
if env.GetKinBody(get_name(target_obj)) is None: env.Add(target_obj)
else:
for obj in objects:
randomly_place_region(env, obj, init_region)
set_quat(obj, quat_from_z_rot(0))
randomly_place_region(env, target_obj, target_obj_region)
set_quat(target_obj, quat_from_z_rot(0))
object_names = [get_name(body) for body in objects] # (tothefront,totheback), (to_the_right,to_the_left)
set_xy(robot, -.75, 0)
# LEFT_ARM_CONFIG = HOLDING_LEFT_ARM
robot.SetDOFValues(REST_LEFT_ARM, robot.GetManipulator('leftarm').GetArmIndices())
robot.SetDOFValues(mirror_arm_config(REST_LEFT_ARM), robot.GetManipulator('rightarm').GetArmIndices())
grasps = {}
for obj_name in object_names:
obj = env.GetKinBody(obj_name)
obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) \
+ get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)
grasps[get_name(obj)] = obj_grasps
target_obj = env.GetKinBody('target_obj')
target_obj_grasps = get_grasps(env, robot, target_obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)+\
get_grasps(env, robot, target_obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH)
grasps['target_obj'] = target_obj_grasps
initial_poses={}
for obj in objects:
initial_poses[get_name(obj)] = get_pose(obj)
initial_poses[get_name(target_obj)] = get_pose(target_obj)
return ManipulationProblem(None,
object_names=object_names, table_names='table1',
goal_regions=init_region,grasps=grasps,
initial_poses = initial_poses)
def grid_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment)
# env.Load(ENVIRONMENTS_DIR + 'empty.xml')
env.Load(ENVIRONMENTS_DIR + 'regrasp_one_table.xml') # the environment for HBf
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')
"""
set_default_robot_config(env.GetRobots()[0])
#m, n = 2, 10
m, n = 2, 10
box_dims = (.12, .04, .08)
#separation = (.08, .08)
separation = (.12, .12)
#separation = (.16, .16)
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, (1.75, 0, 0))
# env.Add(table)
table = env.GetKinBody('table1')
objects = []
goal_poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for i in range(m):
x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0])
row_color = np.zeros(4)
row_color[2-i] = 1.
for j in range(n):
y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1])
name = 'block%d-%d'%(i, j)
# if i==0 and j==0:
# color = np.array([1,0,0,0])
# box_dims = (.12, .06, .08)
# else:
color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0])
box_dims = (.12, .04, .1)
goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
objects.append(box_body(env, *box_dims, name=name, color=color))
object_names = [get_name(body) for body in objects]
object_names.append('ObjToG')
objects.append(env.GetKinBody('ObjToG'))
for obj in randomize(objects):
#randomly_place_body(env, obj, [get_name(table)])
randomly_place_body(env, obj, ['table1'])
return ManipulationProblem(None,
object_names=object_names, table_names=[get_name(table)],
goal_poses=goal_poses)
def two_tables_through_door(env): # Previously 4, 8
env.Load('env.xml')
robot = env.GetRobots()[0]
set_default_robot_config(robot)
region = create_region(env, 'goal', ((-1, 1), (-.3, .3)), \
'floorwalls', color=np.array((0, 0, 1, .25)))
set_config(robot, FOLDED_LEFT_ARM,
robot.GetManipulator('leftarm').GetArmIndices())
set_config(robot,mirror_arm_config(FOLDED_LEFT_ARM),\
robot.GetManipulator('rightarm').GetArmIndices())
# left arm IK
robot.SetActiveManipulator('leftarm')
manip = robot.GetActiveManipulator()
ee = manip.GetEndEffector()
ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel1.load():
ikmodel1.autogenerate()
# right arm torso IK
robot.SetActiveManipulator('rightarm_torso')
manip = robot.GetActiveManipulator()
ee = manip.GetEndEffector()
ikmodel2 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel2.load():
ikmodel2.autogenerate()
# obj definitions
min_height = 0.4
max_height = 1
min_width = 0.2
max_width = 0.6
min_length = 0.2
max_length = 0.6
# loading areas
#rightmost one
init_loading_region = AARegion('init_loading_area',
((-2.51, -0.81), (-2.51, -1)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region.draw(env)
init_loading_region2 = AARegion('init_loading_area2',
((-2.51, -0.81), (1.7, 2.6)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region2.draw(env)
init_loading_region3 = AARegion('init_loading_area3',
((-1.3, -0.81), (-1, 0)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region3.draw(env)
init_loading_region4 = AARegion('init_loading_area4',
((-2.51, -2), (-1, 0)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region4.draw(env)
loading_regions =[init_loading_region,init_loading_region2,\
init_loading_region3,init_loading_region4]
loading_region = AARegion('loading_area', ((-2.51, -0.81), (-2.51, 2.51)),
z=0.0001,
color=np.array((1, 1, 0, 0.25)))
loading_region.draw(env)
# converyor belt region
conv_x = 2
conv_y = 1
conveyor_belt = AARegion('conveyor_belt',
((-1 + conv_x, 10 * max_width + conv_x),
(-0.4 + conv_y, 0.5 + conv_y)),
z=0.0001,
color=np.array((1, 0, 0, 0.25)))
conveyor_belt.draw(env)
all_region = AARegion('all_region',
((-2.51, 10 * max_width + conv_x), (-3.51, 3.51)),
z=0.0001,
color=np.array((1, 1, 0, 0.25)))
"""
obj1 = box_body(env,0.5,0.5,0.5,\
name='obst1',\
color=(0, 1, 1))
env.Add(obj1)
obj2 = box_body(env,0.5,0.5,0.5,\
name='obst2',\
color=(0, 1, 1))
env.Add(obj2)
set_point(obj1,[-1,-1,0.75])
set_point(obj1,[-1.9,-0.5,0.01])
set_point(obj2,[-1.,-0.5,0.01])
set_point(obj2,[-1,0.7,0.01])
"""
NUM_OBSTACLES = 4
OBSTACLES = []
obstacle_poses = {}
obstacle_shapes = {}
i = 0
for i in range(NUM_OBSTACLES):
width = np.random.rand(1) * (max_width - min_width) + min_width
length = np.random.rand(1) * (max_length - min_length) + min_length
height = np.random.rand(1) * (max_height - min_height) + min_height
trans = np.eye(4)
trans[2, -1] = 0.075
new_body = box_body(env,width,length,height,\
name='obj%s'%i,\
color=(0, (i+.5)/NUM_OBSTACLES, 0))
env.Add(new_body)
new_body.SetTransform(trans)
xytheta = randomly_place_in_region(
env, new_body, loading_regions[np.random.randint(4)])
if not (xytheta is None):
obstacle_shapes['obst%s' %
len(OBSTACLES)] = [width[0], length[0], height[0]]
obstacle_poses['obst%s' % len(OBSTACLES)] = xytheta
OBSTACLES.append(new_body)
else:
env.Remove(new_body)
goal_base_pose = np.array([-2, -2, 5 * PI / 4])
robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
import pdb
pdb.set_trace()
n_node_lim_list = [3000, 4000, 5000, 6000, 7000] #,8000,9000,1000]
stime = time.time()
n_node_lim = np.inf
for n_node_lim in n_node_lim_list:
path, tpath2, status2 = get_motion_plan(robot,
goal_base_pose,
env,
maxiter=20,
n_node_lim=n_node_lim)
if status2 is "HasSolution":
print n_node_lim
break
print time.time() - stime
import pdb
pdb.set_trace()
set_robot_config(goal_base_pose, robot)
"""
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)
