def main(robot, planning_env, planner):
# start_config = np.array(robot.GetCurrentConfiguration())
# my starting config
# start_config = np.array ([0., 0.])
# end_config = np.array([4. ,0.])
# ext_arr = SimpleEnvironment.Extend(planning_env, start_config, end_config)
# path = np.array([[0.,0.], [1.,0.], [2.,1.], [3.,0.], [4., 0.]])
# new_path = SimpleEnvironment.ShortenPath(planning_env, path, )
raw_input('Press any key to begin planning')
if robot.name == 'herb':
start_config = HerbEnvironment.GenerateRandomConfiguration(
planning_env)
goal_config = np.array([3.68, -1.90, 0.00, 2.20, 0.00, 0.00, 0.00])
else:
start_config = SimpleEnvironment.GenerateRandomConfiguration(
planning_env)
goal_config = np.array([2.0, -0.8])
plan = planner.Plan(start_config, goal_config)
# plan_short = planning_env.ShortenPath(plan)
plan_short = plan
traj = robot.ConvertPlanToTrajectory(plan_short)
robot.ExecuteTrajectory(traj)
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)
robot.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(
robot)
if not robot.irmodel.load():
print "irmodel didnt' load. "
robot.irmodel.autogenerate()
# add a table and move the robot into place
table = env.ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
env.Add(table)
# 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, table, [0.1] * 7)
herb_base = SimpleRobot(env, robot)
base_env = SimpleEnvironment(herb_base, table, resolution)
base_planner = AStarPlanner(base_env, visualize=False)
arm_planner = AStarPlanner(arm_env, visualize=False)
# TODO: Here initialize your arm planner
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()
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
env = openravepy.Environment()
env.SetViewer('qtcoin')
env.GetViewer().SetName('Homework 3 Viewer')
# First setup the environment and the robot
visualize = args.visualize
if args.robot == 'herb':
robot = HerbRobot(env, args.manip)
planning_env = HerbEnvironment(robot, args.resolution)
visualize = False
elif args.robot == 'simple':
robot = SimpleRobot(env)
planning_env = SimpleEnvironment(robot, args.resolution)
else:
print 'Unknown robot option: %s' % args.robot
exit(0)
# Next setup the planner
if args.planner == 'astar':
planner = AStarPlanner(planning_env, visualize)
elif args.planner == 'bfs':
planner = BreadthFirstPlanner(planning_env, visualize)
elif args.planner == 'dfs':
planner = DepthFirstPlanner(planning_env, visualize)
elif args.planner == 'hrrt':
planner = HeuristicRRTPlanner(planning_env, visualize)
else:
print 'Unknown planner option: %s' % args.planner
args = parser.parse_args()
# First setup the environment and the robot
if args.robot == 'herb':
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
env = openravepy.Environment()
env.SetViewer('qtcoin')
env.GetViewer().SetName('Homework 1 Viewer')
robot = HerbRobot(env, args.manip)
planning_env = HerbEnvironment(robot, args.resolution)
elif args.robot == 'simple':
robot = 'simple'
planning_env = SimpleEnvironment(0.0125, args.dimension)
else:
print 'Unknown robot option: %s' % args.robot
exit(0)
planner = AStarPlanner(planning_env)
main(robot, planning_env, planner)
import IPython
IPython.embed()
robot.SetActiveDOFValues(left_relaxed)
robot.controller = openravepy.RaveCreateController(robot.GetEnv(),
'IdealController')
# 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)
resolution = [0.1, 0.1, numpy.pi / 4.]
herb_base = SimpleRobot(env, robot)
base_env = SimpleEnvironment(herb_base, resolution)
base_env.PlotActionFootprints(0)
raw_input('Move robot to start config and press enter')
#import IPython
#IPython.embed()
#sid = base_env.discrete_env.ConfigurationToNodeId([-1.212695, 0.20981799, 0])
sid = base_env.discrete_env.ConfigurationToNodeId(
herb_base.GetCurrentConfiguration())
start_config = base_env.discrete_env.NodeIdToConfiguration(sid)
herb_base.SetCurrentConfiguration(start_config)
tstart = robot.GetTransform()
hstart = openravepy.misc.DrawAxes(env, tstart)
hstart.SetShow(True)