if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
env = openravepy.Environment()
env.SetViewer('qtcoin')
env.GetViewer().SetName('Homework 2 Viewer')
# First setup the environment and the robot
visualize = args.visualize
if args.robot == 'herb':
robot = HerbRobot(env, args.manip)
planning_env = HerbEnvironment(robot)
visualize = False
elif args.robot == 'simple':
robot = SimpleRobot(env)
planning_env = SimpleEnvironment(robot)
else:
print 'Unknown robot option: %s' % args.robot
exit(0)
# Next setup the planner
if args.planner == 'rrt':
planner = RRTPlanner(planning_env, visualize=visualize)
elif args.planner == 'rrtconnect':
planner = RRTConnectPlanner(planning_env, visualize=visualize)
else:
print 'Unknown planner option: %s' % args.planner
exit(0)
main(robot, planning_env, planner)
'--bias',
type=float,
default=0.05,
help='Goal bias for RRT/RRT*')
args = parser.parse_args()
# First setup the environment and the robot.
dim = 3 if args.planner == 'nonholrrt' else 2
args.start = np.array(args.start).reshape(dim, 1)
args.goal = np.array(args.goal).reshape(dim, 1)
if args.planner == 'nonholrrt':
planning_env = CarEnvironment(args.map, args.start, args.goal)
else:
planning_env = MapEnvironment(args.map, args.start, args.goal)
# Next setup the planner
if args.planner == 'astar':
planner = AStarPlanner(planning_env, args.epsilon)
elif args.planner == 'rrt':
planner = RRTPlanner(planning_env, bias=args.bias, eta=args.eta)
elif args.planner == 'rrtstar':
planner = RRTStarPlanner(planning_env, bias=args.bias, eta=args.eta)
elif args.planner == 'nonholrrt':
planner = RRTPlannerNonholonomic(planning_env, bias=args.bias)
else:
print('Unknown planner option: %s' % args.planner)
exit(0)
main(planning_env, planner, args.start, args.goal, args.planner)
parser.add_argument('-m',
'--map',
type=str,
default='map1.txt',
help='The environment to plan on')
parser.add_argument('-p',
'--planner',
type=str,
default='rrt',
help='The planner to run (star, rrt, rrtstar)')
parser.add_argument('-s', '--start', nargs='+', type=int, required=True)
parser.add_argument('-g', '--goal', nargs='+', type=int, required=True)
args = parser.parse_args()
# First setup the environment and the robot.
planning_env = MapEnvironment(args.map, args.start, args.goal)
# Next setup the planner
if args.planner == 'astar':
planner = AStarPlanner(planning_env)
elif args.planner == 'rrt':
planner = RRTPlanner(planning_env)
elif args.planner == 'rrtconnect':
planner = RRTStarPlanner(planning_env)
else:
print('Unknown planner option: %s' % args.planner)
exit(0)
main(planning_env, planner, args.start, args.goal)
if __name__ == "__main__":
times = []
costs = []
for i in range(10):
start = np.array([40, 100, 4.71]).reshape((3, 1))
goal = np.array([350, 150, 1.57]).reshape((3, 1))
planning_env = CarEnvironment('car_map.txt', start, goal)
# Next setup the planner
bias = 0.05
eta = 0.5
plan = 'nonholrrt'
if plan == 'rrt':
planner = RRTPlanner(planning_env, bias, eta)
elif plan == 'rrtstar':
planner = RRTStarPlanner(planning_env, bias, eta)
elif plan == 'nonholrrt':
planner = RRTPlannerNonholonomic(planning_env, bias)
else:
print('Unknown planner option')
exit(0)
cost, plan_time, goal_bool = main(planning_env, planner, start, goal,
plan)
if (not goal_bool):
i -= 1
continue
costs.append(cost)
times.append(plan_time)
def main(planner, planning_env, visualize, domain, planner_name, trials):
global height
global width
global robot_radius
env_config, start, goal, file_name = parse_domain(domain)#numpy.array(robot.GetCurrentConfiguration())
print ("env_config:")
for config in env_config:
print (str(config))
print ("start_config:",start)
print ("goal_config:",goal)
start_x = start[1]
start_y = start[2]
start_theta = start[3]
goal_x = goal[1]
goal_y = goal[2]
goal_theta = goal[3]
start_config = ((start_x, start_y), start_theta)
goal_config = ((goal_x, goal_y), goal_theta)
if not trials:
if planner_name == 'v':
planner = VisibilityPlanner(planning_env, visualize, width, height, robot_radius)
elif planner_name == 'rrt':
planner = RRTPlanner(planning_env, visualize, width, height, robot_radius)
elif planner_name == 'prm':
planner = PRMPlanner(planning_env, visualize, width, height, robot_radius)
elif planner_name == 'vrrt':
planner = VRRTPlanner(planning_env, visualize, width, height, robot_radius)
elif planner_name == 'vprm':
planner = VPRMPlanner(planning_env, visualize, width, height, robot_radius)
else:
print ('Unknown planner option: %s' % args.planner)
exit(0)
start_time = time.time()
if planner_name == 'rrt' or planner_name == 'vrrt':
Vertices, Edges, path, construct_time, node_nums, len_path, if_fail = planner.Plan(env_config, start_config, goal_config)
total_time = time.time() - start_time
plan_time = total_time - construct_time
elif planner_name == 'prm' or planner_name == 'vprm':
Vertices, Edges, path, prm_times, num_nodes, len_path, if_fail = planner.Plan(env_config, start_config, goal_config)
else:
Vertices, Edges, path, construct_time, num_nodes, len_path, if_fail = planner.Plan(env_config, start_config, goal_config)
total_time = time.time() - start_time
plan_time = total_time - construct_time
planning_env.InitializePlot(Vertices, Edges, path, env_config, start_config, goal_config, file_name, planner_name)
# planning_env.InitializeMiniPlot(env_config, start_config, goal_config, file_name)
# print "total_time:",total_time
# print "construct_time:",construct_time
# print "plan_time:",plan_time
print "planner:",planner_name
print "file name:",file_name
if planner_name == 'prm' or planner_name == 'vprm':
print "total plan time:",prm_times[0]
print "learn time:",prm_times[1]
print "query time:",prm_times[2]
print "construct time:",prm_times[3]
print "expand time:",prm_times[4]
else:
print "plan_time:",plan_time
if planner_name == 'rrt' or planner_name == 'vrrt':
print "total num nodes:",node_nums[0]
print "nodes reached by extension:",node_nums[1]
print "nodes failed to reach completed:",node_nums[2]
else:
print "num_nodes:",num_nodes
print "len_path:",len_path
print "if_fail:",if_fail
else:
newfile = open("trials" + "-" + file_name + ".txt", "w") #w = write access
planner_types = {}
planner_types["V"] = VisibilityPlanner(planning_env, visualize, width, height, robot_radius)
planner_types["RRT"] = RRTPlanner(planning_env, visualize, width, height, robot_radius)
planner_types["PRM"] = PRMPlanner(planning_env, visualize, width, height, robot_radius)
planner_types["VRRT"] = VRRTPlanner(planning_env, visualize, width, height, robot_radius)
planner_types["VPRM"] = VPRMPlanner(planning_env, visualize, width, height, robot_radius)
planner_names = ["V", "RRT", "PRM", "VRRT", "VPRM"]
for planner_name in planner_names: # each planner
planner = planner_types[planner_name]
num_trials = 50
if planner_name == "V":
print "planner:",planner_name
num_trials = 1
for i in range(num_trials):
start_time = time.time()
if planner_name == 'RRT' or planner_name == 'VRRT':
Vertices, Edges, path, construct_time, node_nums, len_path, if_fail = planner.Plan(env_config, start_config, goal_config)
total_time = time.time() - start_time
plan_time = total_time - construct_time
elif planner_name == 'PRM' or planner_name == 'VPRM':
Vertices, Edges, path, prm_times, num_nodes, len_path, if_fail = planner.Plan(env_config, start_config, goal_config)
else:
Vertices, Edges, path, construct_time, num_nodes, len_path, if_fail = planner.Plan(env_config, start_config, goal_config)
total_time = time.time() - start_time
plan_time = total_time - construct_time
planning_env.InitializePlot(Vertices, Edges, path, env_config, start_config, goal_config, file_name, planner_name, i)
print "total_time:",total_time
print "construct_time:",construct_time
print "plan_time:",plan_time
newfile.write(file_name + "\t" + "trial " + str(i) + "\t")
newfile.write(str(planner_name) + "\t")
if planner_name == 'PRM' or planner_name == 'VPRM':
newfile.write(str(prm_times[0]) + "\t") #total plan time
newfile.write(str(prm_times[1]) + "\t") #learn time
newfile.write(str(prm_times[2]) + "\t") #query time
newfile.write(str(prm_times[3]) + "\t") #construct time
newfile.write(str(prm_times[4]) + "\t") #expand time
else:
newfile.write(str(plan_time) + "\t")
newfile.write("\t")
newfile.write("\t")
newfile.write("\t")
newfile.write("\t")
if planner_name == 'RRT' or planner_name == 'VRRT':
newfile.write(str(node_nums[0]) + "\t") # total num nodes
newfile.write(str(node_nums[1]) + "\t") # nodes reached by extension
newfile.write(str(node_nums[2]) + "\t") # nodes failed to reach completed
else:
newfile.write(str(num_nodes) + "\t")
newfile.write("\t")
newfile.write("\t")
newfile.write(str(len_path) + "\t")
newfile.write(str(if_fail) + "\n")
newfile.close()
robot.ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not robot.ikmodel.load():
robot.ikmodel.autogenerate()
# Create environments for planning the arm and base
resolution = [args.hres, args.hres, args.tres]
herb = HerbRobot(env, robot, args.manip)
arm_env = HerbEnvironment(herb)
herb_base = SimpleRobot(env, robot)
base_env = SimpleEnvironment(herb_base, resolution)
base_planner = AStarPlanner(base_env, visualize=False)
arm_planner = None
# TODO: Here initialize your arm planner
arm_planner = RRTPlanner(arm_env, visualize=False)
# add a table and move the robot into place
table = env.ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
env.Add(table)
table_pose = numpy.array([[0, 0, -1, 0.7], [-1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 0, 1]])
table.SetTransform(table_pose)
# set a bottle on the table
bottle = herb.robot.GetEnv().ReadKinBodyXMLFile(
'models/objects/fuze_bottle.kinbody.xml')
herb.robot.GetEnv().Add(bottle)
table_aabb = table.ComputeAABB()
bottle_transform = bottle.GetTransform()
