args = parser.parse_args()
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 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:
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('BIT* Viewer')
# First setup the environment and the robot
visualize = args.visualize
if args.robot == 'herb':
robot = HerbRobot(env, args.manip)
visualize = False
elif args.robot == 'simple':
robot = SimpleRobot(env)
else:
print 'Unknown robot option: %s' % args.robot
exit(0)
start_config = numpy.array(robot.GetCurrentConfiguration())
if robot.name == 'herb':
goal_config = numpy.array([4.6, -1.76, 0.00, 1.96, -1.15, 0.87, -1.43])
# goal_config = numpy.array([ 3.68, -1.90, 0.00, 2.20, 0.00, 0.00, 0.00 ])
else:
goal_config = numpy.array([3.0, 0.0])
planning_env = RandomEnvironment(robot, args.resolution, start_config,
if not robot.ikmodel.load():
robot.ikmodel.autogenerate()
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')
args = parser.parse_args()
# args.robot_xml = 'models/robots/herb2_padded.robot.xml'
herbpy_args = {'sim':args.sim,
'attach_viewer':args.viewer,
# 'robot_xml':args.robot_xml}
# 'env_path':args.env_path,
'segway_sim':True,
'vision_sim':True}
if args.sim == "True":
# uncomment for simulation
env = openravepy.Environment()
robot = HerbRobot(env, args.manip)
print "simple move"
else:
env, robot = herbpy.initialize(**herbpy_args)
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
left_manip = robot.GetManipulator('left')
robot.SetActiveDOFs(left_manip.GetArmIndices())
robot.manip = left_manip