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)