def run():
env = Environment()
env.SetViewer('qtcoin')
# robot_pos = [2.6, -1.3, 1.0]
robot_pos = [0, 0, 0]
env.Load(os.getcwd() + '/worlds/exp5.env.xml')
robot = env.GetRobots()[0]
manipulator = robot.GetManipulator('arm')
robot.SetActiveManipulator(manipulator)
tower_scenario = TowerEnv(robot)
scenario_pos_list, scenario_size_list = tower_scenario.create_scenario()
# print('scenario_pos_list',scenario_pos_list)
boxes_body_list, _ = create_boxes(env, scenario_pos_list,
scenario_size_list, robot_pos)
volumecolors = array(
((1, 0, 0, 0.5), (0, 1, 0, 0.5), (0, 0, 1, 0.5), (0, 1, 1, 0.5),
(1, 0, 1, 0.5), (1, 1, 0, 0.5), (0.5, 1, 0, 0.5), (0.5, 0, 1, 0.5),
(0, 0.5, 1, 0.5), (1, 0.5, 0, 0.5), (0, 1, 0.5, 0.5), (1, 0, 0.5,
0.5)))
for body_id, body in enumerate(boxes_body_list):
for ig, g in enumerate(body.GetLinks()[0].GetGeometries()):
g.SetDiffuseColor(volumecolors[body_id])
raw_input()
trials_num = 10
for i in range(trials_num):
scenario_pos_list, scenario_size_list = tower_scenario.create_scenario(
)
_ = Recreate_boxes(env, scenario_pos_list, scenario_size_list,
boxes_body_list, robot_pos)
for body_id, body in enumerate(boxes_body_list):
for ig, g in enumerate(body.GetLinks()[0].GetGeometries()):
g.SetDiffuseColor(volumecolors[body_id])
start_config = tower_scenario.generate_start_config()
goal_config = tower_scenario.generate_goal_config()
if (start_config is None):
print('no start config')
continue
if (goal_config is None):
print('no goal config')
continue
print('Find both start and goal config')
execute_activeDOFValues(solution=start_config, robot=robot, env=env)
raw_input()
time_limit = 20.0
motion_range = 20.0
sample_per_batch = 100.0
orplanner = ORPLANNER(robot=robot,
env=env,
planner_name="OMPL_BITstar",
time_limit=time_limit,
motion_range=motion_range,
samples_per_batch=sample_per_batch)
#generate trajectory
orplanner.set_goal(goal_config)
# start_time = time.time()
traj, interpolated_traj = orplanner.plan_traj()
# end_time = time.time()
if (traj is None):
print("no trajectory solution")
continue
raw_input()
taskmanip = interfaces.TaskManipulation(robot)
taskmanip.CloseFingers()
robot.WaitForController(0)
robot.Grab(boxes_body_list[3])
robot.GetController().SetPath(traj)
robot.WaitForController(0)
time.sleep(0.5)
while (True):
pass
def main(env,options):
comm = MPI.COMM_WORLD
rank = comm.Get_rank()
print("rank:",rank)
"Main example code."
# load a scene from ProjectRoom environment XML file
env.Load(os.getcwd()+'/worlds/exp2.env.xml')
print(env.GetCollisionChecker())
time.sleep(1)
#get robot info
robot = env.GetRobots()[0]
manipulator = robot.GetManipulator('arm')
robot.SetActiveManipulator(manipulator)
#find available solution and execute solution 3.5,-1.2,1.0
outside_pos_center = [3.5,-1.7,1.0]
inside_pos_center = [3.5,-1.2,1.0]
start_config,start_pos = generate_goalIk_exp1(right=True,robot=robot,pos=outside_pos_center)
# start_config = generate_goalIk(right=True,robot=robot)
if(start_config is None):
print("initialize robot arm eith failure")
exit()
execute_activeDOFValues(solution=start_config,robot=robot,env=env)
time.sleep(0.1)
#use palner to plan path
time_limit = 50.0
motion_range = 20.0
sample_per_batch = 100.0
orplanner = ORPLANNER(robot=robot,env=env,planner_name = "OMPL_BITstar",
time_limit = time_limit, motion_range = motion_range,samples_per_batch=sample_per_batch)
goal_num = 500
success_num = 0
inside = True
delta_time = 0.0
for i in range(goal_num):
print("time_limit: %f motion_range:%f sample_per_batch:%f "%(time_limit,motion_range,sample_per_batch))
print("path num:",i)
print("sucess num:",success_num)
print("total plan time:",delta_time)
#move to outside and then generate goal
goal_config = None
goal_config,start_pos = generate_goalIk_exp1(right=True,robot=robot,pos=outside_pos_center)
execute_activeDOFValues(solution=start_config,robot=robot,env=env)
print "moving to outside"
time.sleep(0.5)
goal_config,end_pos = generate_goalIk_exp1(right=True,robot=robot,pos=inside_pos_center)
if(goal_config is None):
print("No goal IK solution")
continue
#generate trajectory
orplanner.set_goal(goal_config)
start_time = time.time()
traj,interpolated_traj = orplanner.plan_traj()
end_time = time.time()
if(traj is None):
print("no trajectory solution")
continue
#record trjectory if available
record_trajectory_withobs(start=start_config,end=goal_config,
traj=interpolated_traj,fileId=rank)
record_trajectory_endEffecor(start=start_pos,end=end_pos,
traj=interpolated_traj,fileId=rank)
# Execute the trajectory.
robot.GetController().SetPath(traj)
robot.WaitForController(0)
time.sleep(0.5)
success_num+=1
delta_time += (end_time-start_time)
def run():
comm = MPI.COMM_WORLD
rank = comm.Get_rank()
print("rank:", rank)
"Main example code."
# load a scene from ProjectRoom environment XML file
env = Environment()
env.SetViewer('qtcoin')
generator = Shape_pos_generator()
archs_shapes, archs_positions, archs_start, archs_goal = generator.generate(
)
robot_pos = [2.6, -1.3, 1.0]
env_id = rank + np.random.randint(200) #rank%215
env.Load(os.getcwd() + '/worlds/exp3.env.xml')
# create_exp2(env)
body_list, aabb_list = create_boxes(env=env,
pos_list=archs_positions[env_id],
size_list=archs_shapes[env_id],
robot_pos=robot_pos)
# test_dynamicEnv(aabb_list)
#get robot info
robot = env.GetRobots()[0]
manipulator = robot.GetManipulator('arm')
robot.SetActiveManipulator(manipulator)
#find available solution and execute solution 3.5,-1.2,1.0
outside_pos_center = transform_pos(original_position=robot_pos,
relative_position=archs_start[env_id])
inside_pos_center = transform_pos(original_position=robot_pos,
relative_position=archs_goal[env_id])
start_config, start_pos = generate_goalIk_exp1(right=True,
robot=robot,
pos=outside_pos_center)
# start_config = generate_goalIk(right=True,robot=robot)
if (start_config is None):
print("initialize robot arm eith failure")
exit()
execute_activeDOFValues(solution=start_config, robot=robot, env=env)
time.sleep(0.1)
#use palner to plan path
time_limit = 50.0
motion_range = 20.0
sample_per_batch = 100.0
orplanner = ORPLANNER(robot=robot,
env=env,
planner_name="OMPL_BITstar",
time_limit=time_limit,
motion_range=motion_range,
samples_per_batch=sample_per_batch)
goal_num = 50000
success_num = 0
inside = True
delta_time = 0.0
for i in range(goal_num):
if (success_num > 30):
break
print("time_limit: %f motion_range:%f sample_per_batch:%f " %
(time_limit, motion_range, sample_per_batch))
print("path num:", i)
print("sucess num:", success_num)
print("total plan time:", delta_time)
#move to outside and then generate goal
goal_config = None
start_config = None
start_config, start_pos = generate_goalIk_exp1(right=True,
robot=robot,
pos=outside_pos_center)
execute_activeDOFValues(solution=start_config, robot=robot, env=env)
print "moving to outside"
time.sleep(0.5)
goal_config, end_pos = generate_goalIk_exp1(right=True,
robot=robot,
pos=inside_pos_center)
if (goal_config is None):
print("No goal IK solution")
continue
#generate trajectory
orplanner.set_goal(goal_config)
start_time = time.time()
traj, interpolated_traj = orplanner.plan_traj()
end_time = time.time()
if (traj is None):
print("no trajectory solution")
continue
# record_trajectory_randomObs(start=start_config,end=goal_config,
# traj=interpolated_traj,fileId=rank,aabb_list = (aabb_list).tolist())
# Execute the trajectory.
robot.GetController().SetPath(traj)
robot.WaitForController(0)
time.sleep(0.5)
success_num += 1
delta_time += (end_time - start_time)
def run():
comm = MPI.COMM_WORLD
rank = comm.Get_rank()
print("rank:",rank)
"Main example code."
# load a scene from ProjectRoom environment XML file
env = Environment()
# env.SetViewer('qtcoin')
shelf_obb = ShelfObb()
frame_shapes,frame_positions =shelf_obb.generate_frames()
board_shapes,board_positions = shelf_obb.generate_borad_obb()
shapes = frame_shapes + board_shapes
positions = frame_positions + board_positions
# archs_shapes,archs_positions,archs_start,archs_goal = generator.generate()
robot_pos = [2.6, -1.3, 1.0]
env.Load(os.getcwd()+'/worlds/exp4.env.xml')
body_list,aabb_list = create_boxes(env=env,pos_list=positions,size_list=shapes,robot_pos=robot_pos)
# print(str(aabb_list.tolist()))
time.sleep(1)
#get robot info
robot = env.GetRobots()[0]
manipulator = robot.GetManipulator('arm')
robot.SetActiveManipulator(manipulator)
#find available solution and execute solution 3.5,-1.2,1.0
start_config,start_pos = generate_goalIk_shelf(robot=robot,center_pos=list(robot_pos))
if(start_config is None):
print("initialize robot arm eith failure")
exit()
execute_activeDOFValues(solution=start_config,robot=robot,env=env)
time.sleep(0.1)
#use palner to plan path
time_limit = 50.0
motion_range = 20.0
sample_per_batch = 100.0
orplanner = ORPLANNER(robot=robot,env=env,planner_name = "OMPL_BITstar",
time_limit = time_limit, motion_range = motion_range,samples_per_batch=sample_per_batch)
goal_num = 50000
success_num = 0
inside = True
delta_time = 0.0
for i in range(goal_num):
if(success_num>20):
break
print("time_limit: %f motion_range:%f sample_per_batch:%f "%(time_limit,motion_range,sample_per_batch))
print("path num:",i)
print("sucess num:",success_num)
print("total plan time:",delta_time)
#move to outside and then generate goal
goal_config = None
start_config = None
start_config,start_pos = generate_goalIk_shelf(robot=robot,center_pos=list(robot_pos))
execute_activeDOFValues(solution=start_config,robot=robot,env=env)
print "moving to outside"
goal_config,end_pos = generate_goalIk_shelf(robot=robot,center_pos=list(robot_pos),
pos_contraint=True,contrained_pos=list(start_pos))
# execute_activeDOFValues(solution=goal_config,robot=robot,env=env)
if(goal_config is None):
print("No goal IK solution")
continue
raw_input("...")
continue
#generate trajectory
orplanner.set_goal(goal_config)
start_time = time.time()
traj,interpolated_traj = orplanner.plan_traj()
end_time = time.time()
if(traj is None):
print("no trajectory solution")
continue
# record_trajectory_randomObs(start=start_config,end=goal_config,
# traj=interpolated_traj,fileId=rank,aabb_list = (aabb_list).tolist())
# Execute the trajectory.
robot.GetController().SetPath(traj)
robot.WaitForController(0)
time.sleep(0.5)
success_num+=1
delta_time += (end_time-start_time)
def run():
env = Environment()
# env.SetViewer('qtcoin')
comm = MPI.COMM_WORLD
rank = comm.Get_rank()
mpi_size = comm.Get_size()
print("rank:",rank,"size: ",mpi_size)
"Main example code."
# load a scene from ProjectRoom environment XML file
shelf_obb = ShelfObb()
frame_shapes,frame_positions =shelf_obb.generate_frames()
board_shapes,board_positions = shelf_obb.generate_borad_obb()
shapes = frame_shapes + board_shapes
positions = frame_positions + board_positions
# archs_shapes,archs_positions,archs_start,archs_goal = generator.generate()
robot_pos = [2.6, -1.3, 1.0]
env.Load(os.getcwd()+'/worlds/exp4.env.xml')
body_list,aabb_list = create_boxes(env=env,pos_list=positions,size_list=shapes,
robot_pos=robot_pos)
#get robot info
robot = env.GetRobots()[0]
manipulator = robot.GetManipulator('arm')
robot.SetActiveManipulator(manipulator)
#get task from dataset
init_states = None
goal_states = None
obs_list,init_array,goal_array = read_task_withObs("dataset/tasks_shelf.txt")
init_states = np.copy(init_array)
goal_states = np.copy(goal_array)
#use palner to plan path
time_limit = args.time_limit#5.0
motion_range = 20.0
sample_per_batch = 100.0
planner_name = "OMPL_RRTstar"
orplanner = ORPLANNER(robot=robot,env=env,planner_name = planner_name,
time_limit = time_limit, motion_range = motion_range,samples_per_batch=sample_per_batch)
goal_num = int(1000/mpi_size)
success_num = 0
inside = True
delta_time = 0.0
res_notebook = resultNotebook()
for i in range(goal_num):
print("time_limit: %f motion_range:%f sample_per_batch:%f "%(time_limit,motion_range,sample_per_batch))
print("path num:",i)
print("sucess num:",success_num)
print("total plan time:",delta_time)
state_id = i + rank*goal_num
#load scene
obs_aabbs = list(obs_list[state_id])
shapes,positions = transform_obb(obs_aabbs)
aabb_list = Recreate_boxes(env=env,pos_list=positions,size_list=shapes,
body_list=body_list,robot_pos=robot_pos)
#move to outside and then generate goal
start_config = None
start_config = np.copy(init_states[state_id,:])
execute_activeDOFValues(solution=start_config,robot=robot,env=env)
time.sleep(0.1)
goal_config = None
goal_config = np.copy(goal_states[state_id,:])
if(goal_config is None):
print("No goal IK solution")
continue
# print("rank id",rank)
# print("start config",start_config)
# print("goal config",goal_config)
#generate trajectory
orplanner.set_goal(goal_config)
start_time = time.time()
traj,interpolated_traj = orplanner.plan_traj()
end_time = time.time()
if(traj is None):
res_notebook.append(success=False,init_state=start_config,goal_state=goal_config)
print("no trajectory solution")
continue
else:
# print interpolated_traj
# path_array = getData_trajec
res_notebook.append(success=True,init_state=start_config,goal_state=goal_config,
path_array=np.array(interpolated_traj))
#record trjectory if available
# record_trajectory_withobs(start=start_config,end=goal_config,
# traj=interpolated_traj,fileId=rank)
# Execute the trajectory.
robot.GetController().SetPath(traj)
robot.WaitForController(0)
time.sleep(0.1)
success_num+=1
delta_time += (end_time-start_time)
# result_string = "path num: %.2f, sucess num: %.2f total success time: %.4f"%(goal_num,success_num,delta_time)
# with open('result_data/result_%s_shelf.txt'%planner_name,'a+') as f:
# f.write(result_string+"\n")
save_result(res_notebook,"result_data_shelf/%s_shelf_%drank_%ds.pkl"%(planner_name,rank,int(time_limit)))
def run():
comm = MPI.COMM_WORLD
rank = comm.Get_rank()
print("rank:", rank)
"Main example code."
# load a scene from ProjectRoom environment XML file
env = Environment()
env.SetViewer('qtcoin')
robot_pos = [2.6, -1.3,
0.8] #the origin of the world but not actual robot pos
env.Load(os.getcwd() + '/worlds/exp5.env.xml')
# body_list,aabb_list = create_boxes(env=env,pos_list=positions,size_list=shapes,robot_pos=robot_pos)
# print(str(aabb_list.tolist()))
time.sleep(1)
#get robot info
robot = env.GetRobots()[0]
manipulator = robot.GetManipulator('arm')
robot.SetActiveManipulator(manipulator)
taskmanip = interfaces.TaskManipulation(robot)
#load scene
tower_env = TowerEnv(robot=robot, env=env)
box_positions, box_shapes = tower_env.create_scenario_checkerboard()
# board_positions,board_shapes = tower_env.create_shelf()
positions, shapes = box_positions, box_shapes
body_list, aabb_list = create_boxes(env=env,
pos_list=positions,
size_list=shapes,
robot_pos=robot_pos)
time.sleep(0.1)
#use palner to plan path
time_limit = 50.0
motion_range = 20.0
sample_per_batch = 100.0
orplanner = ORPLANNER(robot=robot,
env=env,
planner_name="OMPL_BITstar",
time_limit=time_limit,
motion_range=motion_range,
samples_per_batch=sample_per_batch)
goal_num = 5000000
success_num = 0
inside = True
delta_time = 0.0
path_num = 0
while (True):
if (success_num > 300):
break
box_positions, box_shapes = tower_env.create_scenario_checkerboard()
board_positions, board_shapes = tower_env.create_board()
positions, shapes = box_positions + board_positions, box_shapes + board_shapes
for body in body_list:
env.Remove(body)
body_list, aabb_list = create_boxes(env=env,
pos_list=positions,
size_list=shapes,
robot_pos=robot_pos,
draw=True,
no_color=3)
aabb_list = add_table_AABB(aabb_list)
time.sleep(0.1)
#add color
# raw_input("...")
#move to outside and then generate goal
goal_config = None
start_config = None
# start_config,start_pos = generate_goalIk_shelf(robot=robot,center_pos=list(robot_pos))
# print "getting start config"
start_box_index, start_config = tower_env.generate_start_config_checkerboard(
robot_pos=robot_pos)
if (start_config is None):
# print("No start IK solution")
continue
# print "getting goal config"
goal_config = tower_env.generate_goal_config_checkerboard(
robot_pos=robot_pos)
if (goal_config is None):
# print("No goal IK solution")
continue
print("time_limit: %f motion_range:%f sample_per_batch:%f " %
(time_limit, motion_range, sample_per_batch))
print("path num:", path_num)
print("sucess num:", success_num)
print("total plan time:", delta_time)
path_num += 1
execute_activeDOFValues(solution=start_config, robot=robot, env=env)
#generate trajectory
orplanner.set_goal(goal_config)
start_time = time.time()
traj, interpolated_traj = orplanner.plan_traj()
end_time = time.time()
if (traj is None):
print("no trajectory solution")
continue
# raw_input("...")
# record_trajectory_randomObs(start=start_config,end=goal_config,
# traj=interpolated_traj,fileId=rank,aabb_list = (aabb_list).tolist())
#grab object
taskmanip.CloseFingers()
time.sleep(2)
robot.Grab(body_list[start_box_index])
# Execute the trajectory.
robot.GetController().SetPath(traj)
robot.WaitForController(0)
time.sleep(0.5)
#release grab
taskmanip.ReleaseFingers(target=body_list[start_box_index])
time.sleep(1)
success_num += 1
delta_time += (end_time - start_time)
def run():
env = Environment()
# env.SetViewer('qtcoin')
comm = MPI.COMM_WORLD
rank = comm.Get_rank()
mpi_size = comm.Get_size()
print("rank:", rank, "size: ", mpi_size)
"Main example code."
robot_pos = [2.6, -1.3,
0.8] #the origin of the world but not actual robot pos
env.Load(os.getcwd() + '/worlds/exp5.env.xml')
#get robot info
robot = env.GetRobots()[0]
manipulator = robot.GetManipulator('arm')
robot.SetActiveManipulator(manipulator)
taskmanip = interfaces.TaskManipulation(robot)
#load scene
tower_env = TowerEnv(robot=robot, env=env)
box_positions, box_shapes = tower_env.create_scenario_checkerboard()
# board_positions,board_shapes = tower_env.create_shelf()
positions, shapes = box_positions, box_shapes
body_list, aabb_list = create_boxes(env=env,
pos_list=positions,
size_list=shapes,
robot_pos=robot_pos)
time.sleep(0.1)
#get task from dataset
init_states = None
goal_states = None
obs_list, init_array, goal_array = read_task_withObs(
"dataset/tasks_continuous_stacking.txt")
init_states = np.copy(init_array)
goal_states = np.copy(goal_array)
#use palner to plan path
time_limit = args.time_limit #5.0
motion_range = 20.0
sample_per_batch = 100.0
planner_name = "OMPL_BITstar"
orplanner = ORPLANNER(robot=robot,
env=env,
planner_name=planner_name,
time_limit=time_limit,
motion_range=motion_range,
samples_per_batch=sample_per_batch)
goal_num = 400
success_num = 0
inside = True
delta_time = 0.0
# raw_input("...")
res_notebook = resultNotebook()
for i in range(goal_num):
print("time_limit: %f motion_range:%f sample_per_batch:%f " %
(time_limit, motion_range, sample_per_batch))
print("path num:", i)
print("sucess num:", success_num)
print("total plan time:", delta_time)
state_id = i #+ rank*goal_num
#load scene
obs_aabbs = list(obs_list[state_id])
shapes, positions = transform_obb(obs_aabbs[0:-1])
for body in body_list:
env.Remove(body)
body_list, aabb_list = create_boxes(env=env,
pos_list=positions,
size_list=shapes,
robot_pos=robot_pos,
draw=True,
no_color=3,
smaller_box=True)
time.sleep(0.1)
#move to outside and then generate goal
start_config = None
start_config = np.copy(init_states[state_id, :])
execute_activeDOFValues(solution=start_config, robot=robot, env=env)
time.sleep(0.1)
if (env.CheckCollision(robot)):
print("!!!start colliion!!!")
goal_config = None
goal_config = np.copy(goal_states[state_id, :])
if (goal_config is None):
print("No goal IK solution")
continue
#generate trajectory
orplanner.set_goal(goal_config)
start_time = time.time()
traj, interpolated_traj = orplanner.plan_traj()
end_time = time.time()
if (traj is None):
res_notebook.append(success=False,
init_state=start_config,
goal_state=goal_config)
print("no trajectory solution")
continue
else:
print("saving data")
res_notebook.append(success=True,
init_state=start_config,
goal_state=goal_config,
path_array=np.array(interpolated_traj))
#grab object
# taskmanip.CloseFingers()
# time.sleep(2)
# robot.Grab(body_list[0])
# Execute the trajectory.
robot.GetController().SetPath(traj)
robot.WaitForController(0)
time.sleep(0.5)
#release grab
# taskmanip.ReleaseFingers(target=body_list[0])
# time.sleep(1)
success_num += 1
delta_time += (end_time - start_time)
save_result(
res_notebook,
"result_data_stacking_continuous/%s_shelf_%drank_%ds.pkl" %
(planner_name, rank, int(time_limit)))