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 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 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 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 create_shelf(env, obst_x, obst_width, obst_height, name_idx, stacked_obj_name, table_name): width = 0.4 length = 0.01 height = obst_height top_wall_width = 0.001 bottom_wall_width = 0.0001 table_pos = aabb_from_body(env.GetKinBody(table_name)).pos() table_x = table_pos[0] table_y = table_pos[1] place_body(env, box_body(env, width, length, height, name='right_wall_' + str(name_idx), color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (table_x + .0, table_y + obst_x - (obst_width - .05) / 2, 0), stacked_obj_name) place_body(env, box_body(env, width, length, height, name='left_wall_' + str(name_idx), color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (table_x + .0, table_y + obst_x + (obst_width - .05) / 2, 0), stacked_obj_name) place_body(env, box_body(env, length, obst_width - 0.05, height, name='back_wall_' + str(name_idx), color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (table_x + .225, table_y + obst_x, 0), stacked_obj_name) place_body(env, box_body(env, width, obst_width - 0.05, top_wall_width, name='top_wall_' + str(name_idx), color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (table_x + 0, table_y + obst_x, 0), 'back_wall_' + str(name_idx)) place_body(env, box_body(env, width, obst_width - 0.05, bottom_wall_width, name='bottom_wall_' + str(name_idx), color=OBST_COLOR, transparency=0.5), (table_x + 0, table_y + obst_x, 0), stacked_obj_name) region = create_region(env, 'place_region_' + str(name_idx), ((-1.0, 1.0), (-0.85, 0.85)), 'bottom_wall_' + str(name_idx), color=np.array((0, 0, 0, .5))) region.draw(env) return region
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 simple2(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))), 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 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 two_tables_through_door(env, obj_shapes=None, obj_poses=None, obst_shapes=None, obst_poses=None): 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() # loading areas init_loading_region = AARegion('init_loading_area',\ ((-2.51,-0.81),(-2.51,0)),\ 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_region4 = AARegion('init_loading_area4',\ ((-2.51,-1.5),(-0.1,2)),\ 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_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))) if obj_shapes == None: OBJECTS, obj_shapes, obj_poses = create_objects(env, conveyor_belt) else: OBJECTS = load_objects(env, obj_shapes, obj_poses, color=(0, 1, 0)) if obst_shapes == None: OBSTACLES, obst_shapes, obst_poses = create_obstacles( env, loading_regions) else: OBSTACLES = load_objects(env, obst_shapes, obst_poses, color=(0, 0, 1)) initial_saver = DynamicEnvironmentStateSaver(env) initial_state = (initial_saver, []) init_base_conf = np.array([0, 1.05, 0]) problem = {'initial_state':initial_state,\ 'obstacles':OBSTACLES,\ 'objects':OBJECTS,\ 'loading_region':loading_region,\ 'env':env,\ 'obst_shapes':obst_shapes,\ 'obst_poses':obst_poses,\ 'obj_shapes':obj_shapes,\ 'obj_poses':obj_poses,\ 'all_region':all_region,\ 'init_base_conf':init_base_conf} return problem # the second is for indicating 0 placed objs
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 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) """