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 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 move_regrasp(env):
env.Load(ENVIRONMENTS_DIR + 'regrasp.xml') # the environment for HBf
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 srivastava_table(env):
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 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)
