Example #1
0
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)
Example #2
0
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)
Example #3
0
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 get_goal_config_from_theta_list(theta_list):
	env=Environment()
	env.Reset()

	simple_prob=separate(env,0)
	floor = env.GetKinBody("floorwalls")
	floor.Enable(False)
	oracle = ManipulationOracle(simple_prob,env)
	robot = env.GetRobots()[0]




	target=env.GetKinBody('ObjToG')
	target.Enable(False)
	manipulator = robot.SetActiveManipulator('leftarm_torso') 
	robot.SetActiveDOFs(manipulator.GetArmIndices())
	gmodel = databases.grasping.GraspingModel(robot,target)
	ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,\
					iktype=IkParameterization.Type.Transform6D)
	if not ikmodel.load():
		ikmodel.autogenerate()

	

#	env.SetViewer('qtcoin')
	goal_config_list = []
	for idx in range(np.shape(theta_list)[0]):
		print idx
		base_pose = theta_list[idx,0:3]
		grasp     = theta_list[idx,3:]
		robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
		robot.SetActiveDOFValues(base_pose)
		
		set_default_robot_config(robot)  # set arms to the pregrasp pose
	
		Tgrasp = gmodel.getGlobalGraspTransform(grasp,collisionfree=False)
		goal_config = inverse_kinematics(oracle,Tgrasp)
		if goal_config is not None:
			import pdb; pdb.set_trace()
		goal_config_list.append(goal_config)
	env.Destroy()

	return goal_config_list
Example #5
0
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)
Example #6
0
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)
Example #7
0
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)
Example #8
0
def srivastava_table(env, n=INF):
  env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae')
  set_default_robot_config(env.GetRobots()[0])
  body_names = [get_name(body) for body in env.GetBodies() if not body.IsRobot()]
  table_names = [body_name for body_name in body_names if 'table' in body_name]

  dx = .5
  for body_name in body_names:
    body = env.GetKinBody(body_name)
    set_point(body, get_point(body) + np.array([dx, 0, 0]))

  objects = [env.GetKinBody(body_name) for body_name in body_names if body_name not in table_names]
  for obj in objects: env.Remove(obj)
  object_names = []
  for obj in take(objects, n):
    randomly_place_body(env, obj, table_names)
    object_names.append(get_name(obj))

  goal_holding = 'object1'
  goal_config = 'initial' # None

  return ManipulationProblem(None,
      object_names=object_names, table_names=table_names,
      goal_config=goal_config, goal_holding=goal_holding)
Example #9
0
def srivastava_table(env, n=INF):
  env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae')
  set_default_robot_config(env.GetRobots()[0])
  body_names = [get_name(body) for body in env.GetBodies() if not body.IsRobot()]
  table_names = [body_name for body_name in body_names if 'table' in body_name]

  dx = .5
  for body_name in body_names:
    body = env.GetKinBody(body_name)
    set_point(body, get_point(body) + np.array([dx, 0, 0]))

  objects = [env.GetKinBody(body_name) for body_name in body_names if body_name not in table_names]
  for obj in objects: env.Remove(obj)
  object_names = []
  for obj in take(objects, n):
    randomly_place_body(env, obj, table_names)
    object_names.append(get_name(obj))

  goal_holding = 'object1'
  goal_config = 'initial' # None

  return ManipulationProblem(None,
      object_names=object_names, table_names=table_names,
      goal_config=goal_config, goal_holding=goal_holding)
Example #10
0
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)
Example #11
0
def create_conveyor_belt_problem(env, obj_setup=None, problem_idx=0):
    if obj_setup is not None:
        obj_shapes = obj_setup['object_shapes']
        obj_poses = obj_setup['object_poses']
        obst_shapes = obj_setup['obst_shapes']
        obst_poses = obj_setup['obst_poses']

    fdir = os.path.dirname(os.path.abspath(__file__))

    if problem_idx == 0 or problem_idx == 1:
        env.Load(fdir + '/convbelt_env_diffcult_shapes.xml')
    else:
        env.Load(fdir + '/convbelt_env_diffcult_shapes_two_rooms.xml')
    """
    if problem_idx == 0:
        env.Load(fdir + '/convbelt_env_diffcult_shapes.xml')
    else:
        env.Load(fdir + '/convbelt_env.xml')
    """

    robot = env.GetRobots()[0]
    set_default_robot_config(robot)

    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')
    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')
    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
    """
    self.home_region_xy = [x_extents / 2.0, 0]
    self.home_region_xy_extents = [x_extents, y_extents]
    self.home_region = AARegion('entire_region',
                                ((-x_extents + self.home_region_xy[0], x_extents + self.home_region_xy[0]),
                                 (-y_extents, y_extents)), z=0.135, color=np.array((1, 1, 0, 0.25)))
    """
    init_base_conf = np.array([0, 1.05, 0])
    set_robot_config(np.array([0, 1.05, 0]), robot)

    # converyor belt region
    conv_x = 3
    conv_y = 1
    conveyor_belt = AARegion('conveyor_belt',
                             ((-1 + conv_x, 20 * max_width + conv_x),
                              (-0.4 + conv_y, 0.5 + conv_y)),
                             z=0.01,
                             color=np.array((1, 0, 0, 0.25)))

    y_extents = 5.0
    x_extents = 3.01
    entire_region = AARegion('entire_region',
                             ((-7.4, 20 * max_width + conv_x),
                              (-y_extents - 2.5, y_extents - 2)),
                             z=0.01,
                             color=np.array((1, 1, 0, 0.25)))
    loading_region = AARegion('loading_area', ((-7.4, -0.5), (-7.5, 3.0)),
                              z=0.01,
                              color=np.array((1, 1, 0, 0.25)))

    big_region_1 = AARegion('big_region_1', ((-5, -0.5), (-7.5, -0.4)),
                            z=0.01,
                            color=np.array((1, 1, 0, 0.25)))
    big_region_2 = AARegion('big_region_2', ((-7.4, -4.0), (-7.5, 3.0)),
                            z=0.01,
                            color=np.array((1, 1, 0, 0.25)))

    if problem_idx == 0 or problem_idx == 1:
        objects = []
        i = 1
        for tobj in env.GetBodies():
            if tobj.GetName().find('tobj') == -1: continue
            randomly_place_in_region(env, tobj, conveyor_belt)
            set_obj_xytheta([2 + i, 1.05, 0], tobj)
            objects.append(tobj)
            i += 1.1

        square_objects, obj_shapes, obj_poses = create_objects(env,
                                                               conveyor_belt,
                                                               num_objects=10)
        objects += square_objects
    else:
        objects = []
        i = 1
        for tobj in env.GetBodies():
            if tobj.GetName().find('tobj') == -1: continue
            randomly_place_in_region(env, tobj, conveyor_belt)
            set_obj_xytheta([2 + i, 1.05, get_body_xytheta(tobj)[0, -1]], tobj)
            #objects.append(tobj)
            i += 1.1

        square_objects, obj_shapes, obj_poses = create_objects(env,
                                                               conveyor_belt,
                                                               num_objects=20)
        for obj in square_objects:
            set_obj_xytheta([2 + i, 1.05, get_body_xytheta(obj)[0, -1]], obj)
            objects.append(obj)
            i += 1.1

        #objects += square_objects
    """
    if problem_idx == 0:
        objects = []
        i = 1
        for tobj in env.GetBodies():
            if tobj.GetName().find('tobj') == -1: continue
            randomly_place_in_region(env, tobj, conveyor_belt)
            set_obj_xytheta([2+i, 1.05, 0], tobj)
            objects.append(tobj)
            i += 1.1

        square_objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt, num_objects=10)
        objects += square_objects
    else:
        objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt, num_objects=20)
    """

    initial_saver = DynamicEnvironmentStateSaver(env)
    initial_state = (initial_saver, [])
    problem = {
        'initial_state': initial_state,
        'objects': objects,
        'conveyor_belt_region': conveyor_belt,
        'loading_region': loading_region,
        'big_region_1': big_region_1,
        'big_region_2': big_region_2,
        'env': env,
        'entire_region': entire_region,
        'init_base_conf': init_base_conf
    }
    return problem  # the second is for indicating 0 placed objs
def main():
	theta_values=[]
	theta_augmented_with_grasp = []

	env_file_list = os.listdir(DATA_DIR+'/env_files/')
	reward_matrix = []
	theta_file_template = 'env_'
	thetas = get_all_thetas(theta_file_template)
	sio.savemat( SAVE_DIR + 'thetas.mat',{'thetas':thetas})
#	goal_config_list = get_goal_config_from_theta_list(thetas)
#	sio.savemat( SAVE_DIR + 'goal_config_list.mat',{'goal_config_list':goal_config_list})

	for env_idx in range(len(env_file_list)):
		train_env_f_name = get_file_name_with_given_idx(env_file_list,env_idx)

		# load the environment file
		print train_env_f_name


		env=Environment()
		env.Reset()

		simple_prob=separate(env,0)
		floor = env.GetKinBody("floorwalls")
		floor.Enable(False)
		oracle = ManipulationOracle(simple_prob,env)

		#env.SetViewer('qtcoin')
		## Recovering the environment
		restore_env(env,train_env_f_name)

		# set the robot to default configuration
		robot = env.GetRobots()[0]
		manipulator = robot.SetActiveManipulator('leftarm_torso') 
		set_default_robot_config(robot)  # set arms to the pregrasp pose
		robot.SetActiveDOFs(manipulator.GetArmIndices())
			
		evaluator = ThetaEvaluator(env,oracle)
		env_theta_vals = []
		env_plan_time = []
		env_ik_time= []
		tst = time.time()
		for theta_idx in range(np.shape(thetas)[0]):
			theta = thetas[theta_idx,:]
			if len(theta) is not 0:
				base_pose = theta[0:3]
				grasp     = theta[3:]
				each = time.time()
				theta_val,ik_time,plan_time = evaluator.get_true_vals_from_base_pose_and_grasp(grasp,base_pose)
			else:
				theta_val = 0
			env_theta_vals.append(theta_val)
			env_plan_time.append(plan_time)
			env_ik_time.append(ik_time)
		print time.time() - tst
		theta_values.append(env_theta_vals)
		ik_times.append(env_ik_time)
		plan_times.append(env_plan_time)
		sio.savemat(SAVE_DIR + 'reward_matrix.mat',{'reward_matrix':theta_values,'ik_times':ik_times,'plan_times':plan_times})

		env.Destroy()
def main():
	theta_values=[]
	ik_times = []
	plan_times = []
	theta_augmented_with_grasp = []

	env_file_list = os.listdir(DATA_DIR+'/env_files/')
	reward_matrix = []
	theta_file_template = 'env_'
	if not os.path.isfile(SAVE_DIR+'thetas.mat'):
		thetas = get_all_thetas(theta_file_template)
		sio.savemat( SAVE_DIR + 'thetas.mat',{'thetas':thetas})
	else:
		thetas = sio.loadmat( SAVE_DIR + 'thetas.mat')['thetas']
#	goal_config_list = get_goal_config_from_theta_list(thetas)
#	sio.savemat( SAVE_DIR + 'goal_config_list.mat',{'goal_config_list':goal_config_list})
	
	if len(sys.argv) == 1:
		print "NEED ENV NUMBER"
		return 0 
	env_idx = int(sys.argv[1])
	train_env_f_name = get_file_name_with_given_idx(env_file_list,env_idx)

	reward_file = SAVE_DIR + 'reward_matrix' +str(env_idx)+'.mat'
	if os.path.isfile(reward_file):
		print train_env_f_name +' already done'
		return 0
	print "SPAWNED: " + train_env_f_name

	# load the environment file
#	print train_env_f_name

	env=Environment()
	env.Reset()

	simple_prob=separate(env,0)
	floor = env.GetKinBody("floorwalls")
	floor.Enable(False)
	oracle = ManipulationOracle(simple_prob,env)

	#env.SetViewer('qtcoin')
	## Recovering the environment
	restore_env(env,train_env_f_name,DATA_DIR)

	# set the robot to default configuration
	robot = env.GetRobots()[0]
	manipulator = robot.SetActiveManipulator('leftarm_torso') 
	set_default_robot_config(robot)  # set arms to the pregrasp pose
	robot.SetActiveDOFs(manipulator.GetArmIndices())
		
	evaluator = ThetaEvaluator(env,oracle)
	env_theta_vals = []
	env_plan_time = []
	env_ik_time= []

	tst = time.time()
	for theta_idx in range(np.shape(thetas)[0]):
		print "Running ENV " + str(theta_idx) 
		theta = thetas[theta_idx,:]
		if len(theta) is not 0:
			base_pose = theta[0:3]
			grasp     = theta[3:]
			each = time.time()
			theta_val,ik_time,plan_time = evaluator.get_true_vals_from_base_pose_and_grasp(grasp,base_pose)
		else:
			theta_val = 0
		env_theta_vals.append(theta_val)
		env_plan_time.append(plan_time)
		env_ik_time.append(ik_time)
	theta_values.append(env_theta_vals)
	ik_times.append(env_ik_time)
	plan_times.append(env_plan_time)
	
	sio.savemat(SAVE_DIR + 'reward_matrix' +str(env_idx)+'.mat',\
		{'reward_matrix':theta_values,'ik_times':ik_times,'plan_times':plan_times})

	env.Destroy()
	print "ENV " + str(env_idx) + "FINISHED RUNNING"
Example #14
0
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)
Example #15
0
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 main():
	env_file_order = sio.loadmat(DATA_DIR+'env_file_order.mat')['file_orders'][0]
	theta_values=[]
	theta_augmented_with_grasp = []
	for env_idx in range(160,len(env_file_order)):
		# load the environment file
		env_order = env_file_order[env_idx]
		train_env_f_name = 'scene'+str(env_order)+'_env.dae'
		print train_env_f_name

		#theta_file_template = 'good_thetas_'
		theta_file_template = 'good_thetas_leslie_'
		thetas = sio.loadmat(THETA_DIR+theta_file_template+str(env_idx)+'.mat')['thetas']
		if theta_file_template is not 'good_thetas_leslie_':
			#artificial_vals= sio.loadmat(THETA_DIR+'good_thetas_'+\
			#				str(env_idx)+'.mat')['values']
			#thetas = thetas[artificial_vals[0]>0,:]
			thetas = get_all_thetas(theta_file_template)
		else:
			thetas = get_all_thetas(theta_file_template)
		#	sio.savemat('theta_values_leslie.mat',{'thetas':thetas})
		env=Environment()
		env.Reset()

		simple_prob=separate(env,0)
		floor = env.GetKinBody("floorwalls")
		floor.Enable(False)
		oracle = ManipulationOracle(simple_prob,env)

		#env.SetViewer('qtcoin')
		## Recovering the environment
		restore_env(env,train_env_f_name,DATA_DIR)

		# set the robot to default configuration
		robot = env.GetRobots()[0]
		manipulator = robot.SetActiveManipulator('leftarm_torso') 
		set_default_robot_config(robot)  # set arms to the pregrasp pose
		robot.SetActiveDOFs(manipulator.GetArmIndices())
			
		evaluator = ThetaEvaluator(env,oracle)
		env_theta_vals = []
		for theta_idx in range(np.shape(thetas)[0]):
			theta = thetas[theta_idx,:]
			if theta_file_template == 'good_thetas_leslie_':
				if len(theta) is not 0:
					base_pose = theta[0:3]
					grasp     = theta[3:]
					theta_val,ik_time,plan_time = evaluator.get_true_vals_from_base_pose_and_grasp(grasp,base_pose)
				else:
					theta_val = 0
					
			else:
				theta_val,g = evaluator.get_true_vals(theta)
				if g is not None:
					print g
					theta_augmented_with_grasp.append( np.r_[theta,g] )
				else:
					theta_augmented_with_grasp.append( np.r_[theta] )
			env_theta_vals.append(theta_val)
		theta_values.append(env_theta_vals)
		if theta_file_template == 'good_thetas_leslie_':
			sio.savemat('theta_rewards_leslile5.mat',{'reward':theta_values})
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)
    """
Example #18
0
def create_conveyor_belt_problem(env, obj_setup=None):
    if obj_setup is not None:
        obj_shapes = obj_setup['object_shapes']
        obj_poses = obj_setup['object_poses']
        obst_shapes = obj_setup['obst_shapes']
        obst_poses = obj_setup['obst_poses']

    fdir = os.path.dirname(os.path.abspath(__file__))
    env.Load(fdir + '/convbelt_env.xml')
    robot = env.GetRobots()[0]
    set_default_robot_config(robot)

    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()
    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()
    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

    loading_region = AARegion('loading_area', ((-3.51, -0.81), (-2.51, 2.51)),
                              z=0.01,
                              color=np.array((1, 1, 0, 0.25)))

    # converyor belt region
    conv_x = 3
    conv_y = 1
    conveyor_belt = AARegion('conveyor_belt',
                             ((-1 + conv_x, 20 * max_width + conv_x),
                              (-0.4 + conv_y, 0.5 + conv_y)),
                             z=0.01,
                             color=np.array((1, 0, 0, 0.25)))

    all_region = AARegion('all_region',
                          ((-3.51, 20 * max_width + conv_x), (-2.51, 2.51)),
                          z=0.01,
                          color=np.array((1, 1, 0, 0.25)))

    if obj_setup is None:
        objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt)
        obstacles, obst_shapes, obst_poses = create_obstacles(
            env, loading_region)
    else:
        objects = load_objects(env, obj_shapes, obj_poses, color=(0, 1, 0))
        obstacles = load_objects(env, obst_shapes, obst_poses, color=(0, 0, 1))

    initial_saver = DynamicEnvironmentStateSaver(env)
    initial_state = (initial_saver, [])
    #set_obj_xytheta([-1, -1, 1], obstacles[0])
    #set_obj_xytheta([-2, 2.3, 0], obstacles[1])
    init_base_conf = np.array([0, 1.05, 0])
    set_robot_config(np.array([0, 1.05, 0]), robot)
    #obst_poses = [randomly_place_in_region(env, obj, loading_region) for obj in obstacles]
    #obst_poses = [get_body_xytheta(obj) for obj in obstacles]
    """
    tobj = env.GetKinBody('tobj3')
    tobj_xytheta = get_body_xytheta(tobj.GetLinks()[1])
    tobj_xytheta[0, -1] = (160 / 180.0) * np.pi
    set_obj_xytheta(tobj_xytheta, tobj.GetLinks()[1])
    objects = []
    for tobj in env.GetBodies():
        if tobj.GetName().find('tobj') == -1: continue
        randomly_place_in_region(env, tobj, conveyor_belt)
        objects.append(tobj)
    """

    problem = {
        'initial_state': initial_state,
        'obstacles': obstacles,
        'objects': objects,
        'conveyor_belt_region': conveyor_belt,
        'loading_region': loading_region,
        'env': env,
        'obst_shapes': obst_shapes,
        'obst_poses': obst_poses,
        'obj_shapes': obj_shapes,
        'obj_poses': obj_poses,
        'entire_region': all_region,
        'init_base_conf': init_base_conf
    }
    return problem  # the second is for indicating 0 placed objs
        env_f_name = "scene" + str(env_order) + "_env.dae"

        print "generating optimal theta value for n_evals = " + str(n_theta_evals) + " with env file = " + env_f_name
        print str(float(total_n_thetas * n_theta_evals + env_order + 1) / total_evals * 100) + "% Complete"

        ## Load the environment
        env = Environment()
        env.Reset()
        simple_prob = separate(env, 0)
        floor = env.GetKinBody("floorwalls")
        floor.Enable(False)
        # 		env.SetViewer('qtcoin')

        robot = env.GetRobots()[0]
        manipulator = robot.SetActiveManipulator("leftarm_torso")
        set_default_robot_config(robot)  # set arms to the pregrasp pose

        training_env = Environment()
        training_env.Load(data_dir + "env_files/" + env_f_name)
        restore_env(env, training_env)
        training_env.Destroy()

        ikmodel, gmodel, path_plan_time, goals = load_database(env, robot, manipulator)
        grasps, grasp_indices = gmodel.computeValidGrasps(checkcollision=False, checkik=False)

        goal_indices = range(len(goals))
        grasp_indices = range(len(grasps))

        ## restore the following: [goal,grasp]=(1,1),(1,2),...,(1,n_grasps),(2,1),...,(2,n_grasps),...,(n_goals,n_grasps)
        optimal_theta_idx = optimal_theta_idx_list[n_theta_evals, env_idx]