def checkCollisionForWaypts(pg_waypt,b_loc_waypt,p_waypt,target_obj,start_config,b_loc,pg_con,robot,env): # pregrasp waypt check manipulator = robot.GetActiveManipulator() robot.SetActiveDOFs(manipulator.GetArmIndices()) robot.SetActiveDOFValues(pg_waypt) if checkCollision(env,target_obj): return True # baseloc waypt check pick_success = pick_target_obj(robot,manipulator,pg_con,target_obj,start_config) if not pick_success: return True robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1]) robot.SetActiveDOFValues(b_loc_waypt) if checkCollision(env,target_obj): return True # move the robot to the base location robot.SetActiveDOFValues(b_loc) # place waypt check robot.SetActiveDOFs(manipulator.GetArmIndices()) robot.SetActiveDOFValues(p_waypt) if checkCollision(env,target_obj): return True return False
def checkCollisionForPregraspAndPlace(pg_con,b_loc,p_con,target_obj,start_config,robot,env): #pregrasp config check manipulator = robot.GetActiveManipulator() robot.SetActiveDOFs(manipulator.GetArmIndices()) robot.SetActiveDOFValues(pg_con) if checkCollision(env,target_obj): return True # baseloc check pick_success = pick_target_obj(robot,manipulator,pg_con,target_obj,start_config) if not pick_success: return True robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1]) robot.SetActiveDOFValues(b_loc) if checkCollision(env,target_obj): return True # place config check robot.SetActiveDOFs(manipulator.GetArmIndices()) robot.SetActiveDOFValues(p_con) if checkCollision(env,target_obj): return True return False
def main():
if len(sys.argv) == 1:
print "NEED ENV NUMBER"
return 0
if len(sys.argv) == 2:
print "NEED LENGTH"
return 0
env_idx = int(sys.argv[1])
length = float(sys.argv[2])
DATA_DIR = '/media/beomjoon/New Volume/place_and_partial_path/length_'+ str(length)+'/'
THETA_DIR = DATA_DIR+'thetas/'
REWARD_DIR = DATA_DIR + 'rewards/'
if not os.path.isdir(REWARD_DIR):
os.mkdir(REWARD_DIR)
env_file_list = os.listdir(DATA_DIR+'/env_files/')
reward_matrix = []
train_env_f_name = get_file_name_with_given_idx(env_file_list,env_idx)
print "full theta generation, length= "+str(length) +" SPAWNED: " + train_env_f_name
traj_file = THETA_DIR+ 'traj_lists_'+str(env_idx)+'.mat'
rdy_indicator_file = REWARD_DIR+ 'env_'+str(env_idx)+'_ready.mat'
rwd_file = REWARD_DIR+'reward_matrix'+str(env_idx)+'.mat'
theta_f_name = THETA_DIR + 'env_'+str(env_idx)+'.mat'
if not os.path.isfile(theta_f_name):
print theta_f_name + ' theta file does exist'
return 0
# restore the environment
env = Environment()
env.Load(DATA_DIR+'env_files/'+train_env_f_name)
env.SetViewer('qtcoin')
# load the pregrasp, base loc, and place config
pregrasp_configs = sio.loadmat( theta_f_name )['pregrasp_config_list']
base_locs = sio.loadmat( theta_f_name )['base_loc_list']
place_configs = sio.loadmat( theta_f_name )['place_config_list']
grasp_idxs = sio.loadmat( theta_f_name )['g_idx_list'][0]
n_thetas = len(pregrasp_configs)
traj_f_name = THETA_DIR + 'traj_lists_' + str(env_idx) + '.mat'
pregrasp_traj_list = sio.loadmat(traj_f_name)['pregrasp_traj_list'][0]
base_loc_traj_list = sio.loadmat(traj_f_name)['base_loc_traj_list'][0] #36,7
if len(base_loc_traj_list) == 1:
base_loc_traj_list = sio.loadmat(traj_f_name)['base_loc_traj_list']
if sum(base_loc_traj_list) == -4:
print 'All of base loc trajs bad'
return
place_traj_list = sio.loadmat(traj_f_name)['place_traj_list'][0]
floor = env.GetKinBody("floorwalls")
floor.Enable(False)
robot = env.GetRobots()[0]
manipulator = robot.SetActiveManipulator('leftarm_torso')
robot.SetActiveDOFs(manipulator.GetArmIndices())
start_config = robot.GetActiveDOFValues() # save the original config to move back to
target_obj= env.GetKinBody("ObjToG")
orig_obj_loc = copy.deepcopy(target_obj.GetTransform())
manipulator = robot.SetActiveManipulator('leftarm_torso')
base_pos = np.array([-0.1,-0.9,-PI/2.0])
theta_list = []
reward_matrix = []
n_thetas = min( len(place_traj_list),len(base_loc_traj_list),len(pregrasp_traj_list) )
for i in range(n_thetas-1):
pregrasp_config = pregrasp_configs[i]
base_loc = base_locs[i]
place_config = place_configs[i]
# get trajectories
pregrasp_traj = pregrasp_traj_list[i+1] # +1 since traj lists have -1 as dummy place holder for the first element
base_traj = base_loc_traj_list[i+1]
place_traj = place_traj_list[i+1]
if len(pregrasp_traj) == 1 or len(base_traj) == 1 or len(place_traj) == 1:
continue
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
"""
if checkCollisionForPregraspAndPlace(pregrasp_config,base_loc,place_config,target_obj,start_config,robot,env):
continue
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
# plan to pregrasp pose
robot.SetActiveDOFs(manipulator.GetArmIndices())
# loop thru all the waypoint, and save the reward
target_obj.Enable(False)
pregrasp_traj_sugg_rwds = get_rwds_for_suggested_waypoints(pregrasp_traj,start_config,pregrasp_config,robot,env)
target_obj.Enable(True)
# choose the one with the minimum planning time
pregrasp_waypt_idx = np.argmax(pregrasp_traj_sugg_rwds)
pregrasp_rwd = pregrasp_traj_sugg_rwds[pregrasp_waypt_idx]
pregrasp_waypt = pregrasp_traj[pregrasp_waypt_idx]
# pick the object up, and set the robot into the pickup config
"""
pick_target_obj(robot,manipulator,pregrasp_config,target_obj,start_config)
# plan to the base_loc
target_obj.Enable(True)
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
d_fn = distance_fn(robot)
s_fn = sample_fn(robot)
e_fn = extend_fn(robot)
c_fn = collision_fn(env, robot)
idx = 0;t=time.time();path = rrt(base_pos,base_traj[idx],d_fn,s_fn,e_fn,c_fn);print time.time()-t
import pdb;pdb.set_trace()
base_traj_sugg_rwds = get_rwds_for_suggested_waypoints(base_traj,base_pos,base_loc,robot,env)
base_waypt_idx = np.argmax( base_traj_sugg_rwds )
base_rwd = base_traj_sugg_rwds[base_waypt_idx]
base_waypt = base_traj[base_waypt_idx]
robot.SetActiveDOFValues(base_loc)
# plan to the place_config
robot.SetActiveDOFs(manipulator.GetArmIndices())
place_traj_sugg_rwds = get_rwds_for_suggested_waypoints(place_traj,start_config,place_config,robot,env)
place_waypt_idx = np.argmax( place_traj_sugg_rwds )
place_rwd = place_traj_sugg_rwds[place_waypt_idx]
place_waypt = place_traj[place_waypt_idx]
robot.SetActiveDOFs(manipulator.GetArmIndices())
theta_list.append( [grasp_idxs[i], base_loc, place_config, pregrasp_waypt_idx, base_waypt_idx, place_waypt_idx] )
reward_matrix.append( [pregrasp_rwd, base_rwd, place_rwd] )
sio.savemat(THETA_DIR+'theta_'+str(i)+'_for_env'+str(env_idx),{'grasp_idx':grasp_idxs[i],\
'pregrasp_config':pregrasp_config,\
'base_loc':base_loc,\
'place_config':place_config,\
'pregrasp_waypt_idx':pregrasp_waypt_idx,\
'base_waypt_idx':base_waypt_idx,\
'place_waypt_idx':place_waypt_idx,\
'pregrasp_traj_sugg_rwds':pregrasp_traj_sugg_rwds,\
'base_traj_sugg_rwds':base_traj_sugg_rwds,\
'place_traj_sugg_rwds':place_traj_sugg_rwds
})
sio.savemat(rdy_indicator_file,{'rdy':[1]})
# load the environment file
env.Destroy()
def main():
if len(sys.argv) < 3:
print "NEED ENV NUMBER and LENGTH"
return 0
if len(sys.argv) == 2:
print "NEED LENGTH"
return 0
env_idx = int(sys.argv[1])
length = float(sys.argv[2])
DATA_DIR = '/media/beomjoon/New Volume/place_and_partial_path/length_'+ str(length)+'_new/'
THETA_DIR = DATA_DIR+'thetas/'
REWARD_DIR = DATA_DIR + 'rewards/'
if not os.path.isdir(REWARD_DIR):
os.mkdir(REWARD_DIR)
env_file_list = os.listdir(DATA_DIR+'/env_files/')
reward_matrix = []
train_env_f_name = get_file_name_with_given_idx(env_file_list,env_idx)
print "SPAWNED: " + DATA_DIR + train_env_f_name
#reward_file = REWARD_DIR+ 'reward_matrix' +str(env_idx)+'.mat'
traj_file = THETA_DIR+ 'traj_lists_'+str(env_idx)+'.mat'
if os.path.isfile(traj_file):
print train_env_f_name +' already done'
return 0
# load the pregrasp, base loc, and place config
theta_f_name = THETA_DIR + 'env_'+str(env_idx)+'.mat'
pregrasp_configs = sio.loadmat( theta_f_name )['pregrasp_config_list']
base_locs = sio.loadmat( theta_f_name )['base_loc_list']
place_configs = sio.loadmat( theta_f_name )['place_config_list']
n_thetas = len(pregrasp_configs)
# for each of the thetas, get paths
# restore the environment
env = Environment()
env.Load(DATA_DIR+'env_files/'+train_env_f_name)
robot = env.GetRobots()[0]
floor = env.GetKinBody("floorwalls")
floor.Enable(False)
#env.SetViewer('qtcoin')
# restore the original environment
manipulator = robot.SetActiveManipulator('leftarm_torso')
robot.SetActiveDOFs(manipulator.GetArmIndices())
start_config = robot.GetActiveDOFValues() # save the original config to move back to
target_obj= env.GetKinBody("ObjToG")
orig_obj_loc = copy.deepcopy(target_obj.GetTransform())
manipulator = robot.SetActiveManipulator('leftarm_torso')
base_pos = np.array([-0.1,-0.9,-PI/2.0])
pregrasp_traj_list = [];pregrasp_traj_list.append([-1])
base_loc_traj_list = [];base_loc_traj_list.append([-1])
place_traj_list = [];place_traj_list.append([-1])
pregrasp_plan_time = []
base_loc_plan_time = []
place_plan_time = []
for i in range(n_thetas):
pregrasp_config = pregrasp_configs[i]
base_loc = base_locs[i]
place_config = place_configs[i]
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
if checkCollisionForPregraspAndPlace(pregrasp_config,base_loc,place_config,target_obj,start_config,robot,env):
continue
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
# plan to pregrasp pose
robot.SetActiveDOFs(manipulator.GetArmIndices())
target_obj.Enable(False)
traj,planning_time,prep_time,plan_status = getMotionPlan(robot,pregrasp_config,env)
pregrasp_plan_time.append(planning_time)
target_obj.Enable(True)
if plan_status == 'Failed':
pregrasp_traj_list.append([-1])
place_traj_list.append([-1])
base_loc_traj_list.append([-1])
print 'pregrasp path failed'
continue
else:
pregrasp_traj = show_trajectory(robot,traj,env)
pregrasp_traj_list.append(pregrasp_traj)
# pick the object up
pick_target_obj(robot,manipulator,pregrasp_config,target_obj,start_config)
# plan to the place_config
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
robot.SetActiveDOFValues(base_loc)
robot.SetActiveDOFs(manipulator.GetArmIndices())
traj,planning_time,prep_time,plan_status = getMotionPlan(robot,place_config,env,100)
place_plan_time.append(planning_time)
if plan_status == 'Failed':
print i,env_idx,length
place_traj_list.append([-1])
base_loc_traj_list.append([-1])
print 'place path failed'
continue
else:
place_traj = show_trajectory(robot,traj,env)
place_traj_list.append(place_traj)
# plan to the base_loc
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
robot.SetActiveDOFValues(base_pos)
traj,planning_time,prep_time,plan_status = getMotionPlan(robot,base_loc,env,100)
base_loc_plan_time.append(planning_time)
if plan_status == 'Failed':
print 'base plan failed'
base_loc_traj_list.append([-1])
continue
else:
base_loc_traj = show_trajectory(robot,traj,env)
base_loc_traj_list.append(base_loc_traj)
sio.savemat(THETA_DIR+ 'traj_lists_'+str(env_idx)+'.mat',{'pregrasp_traj_list':pregrasp_traj_list,\
'base_loc_traj_list':base_loc_traj_list,\
'place_traj_list':place_traj_list,\
'pregrasp_plan_time':pregrasp_plan_time,\
'base_loc_plan_time':base_loc_plan_time,\
'place_plan_time':place_plan_time
} )
# load the environment file
env.Destroy()
def main():
if len(sys.argv) == 1:
print "NEED ENV NUMBER"
return 0
if len(sys.argv) == 2:
print "NEED LENGTH"
return 0
env_idx = int(sys.argv[1])
length = float(sys.argv[2])
DATA_DIR = "/media/beomjoon/New Volume/place_and_partial_path/length_" + str(length) + "/"
THETA_DIR = DATA_DIR + "thetas/"
REWARD_DIR = DATA_DIR + "rewards/"
if not os.path.isdir(REWARD_DIR):
os.mkdir(REWARD_DIR)
env_file_list = os.listdir(DATA_DIR + "/env_files/")
reward_matrix = []
train_env_f_name = get_file_name_with_given_idx(env_file_list, env_idx)
print "full theta generation, length= " + str(length) + " SPAWNED: " + train_env_f_name
traj_file = THETA_DIR + "traj_lists_" + str(env_idx) + ".mat"
rdy_indicator_file = REWARD_DIR + "env_" + str(env_idx) + "_ready.mat"
rwd_file = REWARD_DIR + "reward_matrix" + str(env_idx) + ".mat"
theta_f_name = THETA_DIR + "env_" + str(env_idx) + ".mat"
if os.path.isfile(rdy_indicator_file) or os.path.isfile(rwd_file):
print train_env_f_name + " already done"
return 0
if not os.path.isfile(theta_f_name):
print theta_f_name + " theta file does exist"
return 0
# restore the environment
env = Environment()
env.Load(DATA_DIR + "env_files/" + train_env_f_name)
# env.SetViewer('qtcoin')
# load the pregrasp, base loc, and place config
pregrasp_configs = sio.loadmat(theta_f_name)["pregrasp_config_list"]
base_locs = sio.loadmat(theta_f_name)["base_loc_list"]
place_configs = sio.loadmat(theta_f_name)["place_config_list"]
grasp_idxs = sio.loadmat(theta_f_name)["g_idx_list"][0]
n_thetas = len(pregrasp_configs)
traj_f_name = THETA_DIR + "traj_lists_" + str(env_idx) + ".mat"
pregrasp_traj_list = sio.loadmat(traj_f_name)["pregrasp_traj_list"][0]
base_loc_traj_list = sio.loadmat(traj_f_name)["base_loc_traj_list"][0] # 36,7
if len(base_loc_traj_list) == 1:
base_loc_traj_list = sio.loadmat(traj_f_name)["base_loc_traj_list"]
if sum(base_loc_traj_list) == -4:
print "All of base loc trajs bad"
return
place_traj_list = sio.loadmat(traj_f_name)["place_traj_list"][0]
floor = env.GetKinBody("floorwalls")
floor.Enable(False)
robot = env.GetRobots()[0]
manipulator = robot.SetActiveManipulator("leftarm_torso")
robot.SetActiveDOFs(manipulator.GetArmIndices())
start_config = robot.GetActiveDOFValues() # save the original config to move back to
target_obj = env.GetKinBody("ObjToG")
orig_obj_loc = copy.deepcopy(target_obj.GetTransform())
manipulator = robot.SetActiveManipulator("leftarm_torso")
base_pos = np.array([-0.1, -0.9, -PI / 2.0])
theta_list = []
reward_matrix = []
n_thetas = min(len(place_traj_list), len(base_loc_traj_list), len(pregrasp_traj_list))
for i in range(n_thetas - 1):
pregrasp_config = pregrasp_configs[i]
base_loc = base_locs[i]
place_config = place_configs[i]
# get trajectories
pregrasp_traj = pregrasp_traj_list[
i + 1
] # +1 since traj lists have -1 as dummy place holder for the first element
base_traj = base_loc_traj_list[i + 1]
place_traj = place_traj_list[i + 1]
if len(pregrasp_traj) == 1 or len(base_traj) == 1 or len(place_traj) == 1:
continue
restore_original_scene(manipulator, start_config, base_pos, robot, target_obj, orig_obj_loc)
if checkCollisionForPregraspAndPlace(
pregrasp_config, base_loc, place_config, target_obj, start_config, robot, env
):
print "print failed"
continue
restore_original_scene(manipulator, start_config, base_pos, robot, target_obj, orig_obj_loc)
# plan to pregrasp pose
robot.SetActiveDOFs(manipulator.GetArmIndices())
# loop thru all the waypoint, and save the reward
target_obj.Enable(False)
pregrasp_traj_sugg_rwds = get_rwds_for_suggested_waypoints(
pregrasp_traj, start_config, pregrasp_config, robot, env
)
target_obj.Enable(True)
# choose the one with the minimum planning time
pregrasp_waypt_idx = np.argmax(pregrasp_traj_sugg_rwds)
pregrasp_rwd = pregrasp_traj_sugg_rwds[pregrasp_waypt_idx]
pregrasp_waypt = pregrasp_traj[pregrasp_waypt_idx]
# pick the object up, and set the robot into the pickup config
pick_target_obj(robot, manipulator, pregrasp_config, target_obj, start_config)
# plan to the base_loc
target_obj.Enable(True)
robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
base_traj_sugg_rwds = get_rwds_for_suggested_waypoints(base_traj, base_pos, base_loc, robot, env)
base_waypt_idx = np.argmax(base_traj_sugg_rwds)
base_rwd = base_traj_sugg_rwds[base_waypt_idx]
base_waypt = base_traj[base_waypt_idx]
robot.SetActiveDOFValues(base_loc)
# plan to the place_config
robot.SetActiveDOFs(manipulator.GetArmIndices())
place_traj_sugg_rwds = get_rwds_for_suggested_waypoints(place_traj, start_config, place_config, robot, env)
place_waypt_idx = np.argmax(place_traj_sugg_rwds)
place_rwd = place_traj_sugg_rwds[place_waypt_idx]
place_waypt = place_traj[place_waypt_idx]
robot.SetActiveDOFs(manipulator.GetArmIndices())
theta_list.append([grasp_idxs[i], base_loc, place_config, pregrasp_waypt_idx, base_waypt_idx, place_waypt_idx])
reward_matrix.append([pregrasp_rwd, base_rwd, place_rwd])
sio.savemat(
THETA_DIR + "theta_" + str(i) + "_for_env" + str(env_idx),
{
"grasp_idx": grasp_idxs[i],
"pregrasp_config": pregrasp_config,
"base_loc": base_loc,
"place_config": place_config,
"pregrasp_waypt_idx": pregrasp_waypt_idx,
"base_waypt_idx": base_waypt_idx,
"place_waypt_idx": place_waypt_idx,
"pregrasp_traj_sugg_rwds": pregrasp_traj_sugg_rwds,
"base_traj_sugg_rwds": base_traj_sugg_rwds,
"place_traj_sugg_rwds": place_traj_sugg_rwds,
},
)
sio.savemat(rdy_indicator_file, {"rdy": [1]})
# load the environment file
env.Destroy()
def main():
if len(sys.argv) == 1:
print "NEED ENV NUMBER"
return 0
env_idx = int(sys.argv[1])
DATA_DIR = '/media/beomjoon/New Volume/place_and_partial_path/for_running_experiments/'
THETA_DIR = DATA_DIR+ '/thetas/'
REWARD_DIR = DATA_DIR + 'reward_matrix/'
if not os.path.isdir(REWARD_DIR):
os.mkdir(REWARD_DIR)
env_file_list = os.listdir(DATA_DIR+'/env_files/')
reward_matrix = []
train_env_f_name = get_file_name_with_given_idx(env_file_list,env_idx)
print "SPAWNED: " + train_env_f_name
traj_file = THETA_DIR+ 'traj_lists_'+str(env_idx)+'.mat'
theta_f_name = THETA_DIR + 'thetas.mat'
if not os.path.isfile(theta_f_name):
pg_con_list,b_loc_list,p_con_list,pg_waypt_list,b_waypt_list,p_waypt_list = get_all_thetas( THETA_DIR )
sio.savemat(theta_f_name,{'pg_con_list':pg_con_list,'b_loc_list':b_loc_list,'p_con_list':p_con_list,\
'pg_waypt_list':pg_waypt_list,'b_waypt_list':b_waypt_list,'p_waypt_list':p_waypt_list})
else:
pg_con_list = sio.loadmat(theta_f_name)['pg_con_list']
b_loc_list =sio.loadmat(theta_f_name)['b_loc_list']
p_con_list =sio.loadmat(theta_f_name)['p_con_list']
pg_waypt_list =sio.loadmat(theta_f_name)['pg_waypt_list']
b_waypt_list =sio.loadmat(theta_f_name)['b_waypt_list']
p_waypt_list = sio.loadmat(theta_f_name)['p_waypt_list']
n_thetas = np.shape(pg_con_list)[0]
rwd_file = REWARD_DIR+ 'rwd_matrix'+str(env_idx)+'.mat'
if os.path.isfile(rwd_file):
rwd_mat = sio.loadmat(rwd_file)['reward_matrix']
if np.shape(rwd_mat)[0] == n_thetas:
print train_env_f_name +' already done'
return 0
else:
start_idx = np.shape(rwd_mat)[0]-1
else:
start_idx = 0
# restore the environment
env = Environment()
env.Load(DATA_DIR+'env_files/'+train_env_f_name)
floor = env.GetKinBody("floorwalls")
floor.Enable(False)
# env.SetViewer('qtcoin')
robot = env.GetRobots()[0]
manipulator = robot.SetActiveManipulator('leftarm_torso')
robot.SetActiveDOFs(manipulator.GetArmIndices())
start_config = robot.GetActiveDOFValues() # save the original config to move back to
target_obj= env.GetKinBody("ObjToG")
orig_obj_loc = copy.deepcopy(target_obj.GetTransform())
manipulator = robot.SetActiveManipulator('leftarm_torso')
base_pos = np.array([-0.1,-0.9,-PI/2.0])
theta_list = []
reward_matrix = []
for i in range(n_thetas):
pg_con = pg_con_list[i]
b_loc = b_loc_list[i]
p_con = p_con_list[i]
pg_waypt = pg_waypt_list[i]
b_waypt = b_waypt_list[i]
p_waypt = p_waypt_list[i]
# check if pregrasp pose, base location, and place pose is in collision
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
if checkCollisionForPregraspAndPlace(pg_con,b_loc,p_con,target_obj,start_config,robot,env):
reward_matrix.append( [0, 0, 0] )
print 'pap collision'
sio.savemat(rwd_file,{'reward_matrix':reward_matrix})
continue
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
# check if waypts are in collision
if checkCollisionForWaypts(pg_waypt,b_waypt,p_waypt,target_obj,start_config,b_loc,pg_con,robot,env):
reward_matrix.append( [0, 0, 0] )
print 'waypt collision'
sio.savemat(rwd_file,{'reward_matrix':reward_matrix})
continue
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
# plan to pregrasp pose
target_obj.Enable(False)
robot.SetActiveDOFs(manipulator.GetArmIndices())
pg_traj_rwd = get_rwd_for_suggested_waypoint(pg_waypt,start_config,pg_con,robot,env)
if pg_traj_rwd == -9999:
print 'pregrasp path unsuccessful'
reward_matrix.append( [0, 0, 0] )
sio.savemat(rwd_file,{'reward_matrix':reward_matrix})
continue
# pick the object up, and set the robot into the pickup config
target_obj.Enable(True)
pick_success = pick_target_obj(robot,manipulator,pg_con,target_obj,start_config)
if not pick_success:
print 'pick unsuccessful'
reward_matrix.append( [0, 0, 0] )
sio.savemat(rwd_file,{'reward_matrix':reward_matrix})
continue
"""
#TODO: Testing iterations
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
import pdb;pdb.set_trace()
traj,planning_time,prep_time,plan_status = getMotionPlan(robot,b_loc,env,10)
import pdb;pdb.set_trace()
"""
# plan to the base_loc
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
b_traj_rwd = get_rwd_for_suggested_waypoint(b_waypt,base_pos,b_loc,robot,env)
if b_traj_rwd == -9999:
print 'no base path'
reward_matrix.append( [0, 0, 0] )
sio.savemat(rwd_file,{'reward_matrix':reward_matrix})
continue
# place the robot to base loc
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
robot.SetActiveDOFValues(b_loc)
# plan to the place_config
target_obj.Enable(False)
robot.SetActiveDOFs(manipulator.GetArmIndices())
p_traj_rwd = get_rwd_for_suggested_waypoint(p_waypt,start_config,p_con,robot,env)
if p_traj_rwd == -9999:
reward_matrix.append( [0, 0, 0] )
sio.savemat(rwd_file,{'reward_matrix':reward_matrix})
continue
reward_matrix.append( [pg_traj_rwd, b_traj_rwd, p_traj_rwd] )
sio.savemat(rwd_file,{'reward_matrix':reward_matrix})
# load the environment file
print 'finished running'
env.Destroy()
