-
Notifications
You must be signed in to change notification settings - Fork 2
/
run.py
120 lines (93 loc) · 4.55 KB
/
run.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
#!/usr/bin/env python
import argparse, numpy, openravepy, time
from HerbRobot import HerbRobot
from HerbEnvironment import HerbEnvironment
from SimpleRobot import SimpleRobot
from SimpleEnvironment import SimpleEnvironment
from GraspPlanner import GraspPlanner
from AStarPlanner import AStarPlanner
from RRTPlanner import RRTPlanner
# TODO: Import the applicable RRTPlanner
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='script for testing planners')
parser.add_argument('--test', type=int, default=1,
help='The test to run')
parser.add_argument('--hres', type=float, default=0.1,
help='xy resolution')
parser.add_argument('--tres', type=float, default=numpy.pi/8.,
help='angular resolution')
parser.add_argument('-m', '--manip', type=str,
help='The manipulator to grasp the bottle with (right or left)')
parser.add_argument('-d', '--debug', action='store_true',
help='Enable debug logging')
args = parser.parse_args()
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
# Create an environment
env = openravepy.Environment()
env.SetViewer('qtcoin')
env.GetViewer().SetName('Homework 4 Viewer')
# Load HERB into it
robot = env.ReadRobotXMLFile('models/robots/herb2_padded.robot.xml')
env.Add(robot)
theta = -numpy.pi/4.
robot_pose = numpy.array([[numpy.cos(theta), -numpy.sin(theta), 0, -1.25],
[numpy.sin(theta), numpy.cos(theta), 0, 0.82],
[0. , 0. , 1, 0. ],
[0. , 0. , 0, 1. ]])
robot.SetTransform(robot_pose)
right_relaxed = [ 5.65, -1.76, -0.26, 1.96, -1.15 , 0.87, -1.43 ]
left_relaxed = [ 0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43 ]
right_manip = robot.GetManipulator('right_wam')
robot.SetActiveDOFs(right_manip.GetArmIndices())
robot.SetActiveDOFValues(right_relaxed)
left_manip = robot.GetManipulator('left_wam')
robot.SetActiveDOFs(left_manip.GetArmIndices())
robot.SetActiveDOFValues(left_relaxed)
if args.manip == 'right':
robot.SetActiveManipulator('right_wam')
else:
robot.SetActiveManipulator('left_wam')
robot.controller = openravepy.RaveCreateController(robot.GetEnv(), 'IdealController')
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()
bottle_transform[2,3] = table_aabb.pos()[2] + table_aabb.extents()[2]
if args.test == 1:
bottle_transform[:2,3] = table_aabb.pos()[:2]
elif args.test == 2:
bottle_transform[0,3] = table_aabb.pos()[0] - 0.5*table_aabb.extents()[0]
bottle_transform[1,3] = table_aabb.pos()[1] - 0.5*table_aabb.extents()[1]
elif args.test == 3:
bottle_transform[0,3] = table_aabb.pos()[0] + 0.5*table_aabb.extents()[0]
bottle_transform[1,3] = table_aabb.pos()[1] + 0.5*table_aabb.extents()[1]
bottle.SetTransform(bottle_transform)
planner = GraspPlanner(herb.robot, base_planner, arm_planner)
planner.PlanToGrasp(bottle)
import IPython
IPython.embed()