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 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 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 simple_push(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']
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)