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)
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)
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
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)
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)
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)
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)
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)
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)
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"
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)
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)
"""
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]