1
2
3 '''
4 B{List of available commands:}
5
6 Gripper and Head:
7 - pr2.openGripper(side, wait=True) # side is LEFT or RIGHT
8 - pr2.closeGripper(side, wait=True) # side is LEFT or RIGHT
9 - pr2.tiltHead(-30, wait=True) # can add something like 'duration=2.0' for motion duration
10 - pr2.rotateHead(30, wait=True) # can add something like 'duration=2.0' for motion duration
11 - pr2.lookAt(-100, 30, wait=False) # takes both pan and tilt angle for smooth motion
12
13 Moving the arm joints one at a time, or together:
14 - pr2.moveArmJoint(jointName, newAngle, duration=2.0, wait=False)
15 - pr2.moveArmJoint([jointName1, jointName2, ...], [newAngle1, newAngle2, ...], duration=2.0, wait=True)
16
17 Moving the whole robot (the base):
18 - pr2.moveBase(place=(x,y,z), rotation=deg, duration=2, wait=True)
19
20 Reading joint values:
21 - pr2.getSensorReading(sensorName)
22
23 Checking approximate joint value (when setting a joint to 0, it's often something like 0.0000134; aboutEq() does an approximate compare)
24 - aboutEq(jointName, value)
25
26 Raising/lowering the torso:
27 - pr2.setTorso(.05, 2.0, wait=True)
28
29 Temporarily pausing execution (but already initiated robot motions continue):
30 - rospy.timer.sleep(seconds)
31
32 More examples:
33 - pr2.tiltHead(15 + pr2.getSensorReading("head_tilt_joint"), 1)
34
35 - joints = ['l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint']
36 values = [-70, -90, -70]
37 pr2.moveArmJoint(joints, values, duration=2.0, wait=False)
38
39 ADVANCED USE: Simulated events impacting your script:
40
41 - A simulator, or sensor can provide callbacks to your
42 application at regular intervals, or on a schedule you
43 determine::
44
45 from event_simulator import EventSimulator
46
47 # Callback invoked at specified times. Keep this
48 # callback short. It runs in a different thread,
49 # which should be free to generate further events.
50 # For more sophisticated use, see next section
51 # 'Even more advanced use.'
52
53 def printWord(word):
54 print(word)
55
56 schedule = OrderedDict()
57 schedule[2.0] = 'This'
58 schedule[5.0] = 'is'
59 schedule[6.0] = 'a'
60 schedule[7.2] = 'test'
61
62 EventSimulator().start(schedule, printWord)
63
64 EVEN MORE ADVANCED USE:
65
66 - You can queue up results that are computed or sensed by
67 the callbacks:
68 The simulator callbacks may return values to the simulator.
69 Each simulator is equipped with a queue into which it will
70 place those returned values. Your application thread can
71 pick them up from that queue.
72
73 The following code creates a callback that upper-cases
74 every word it receives. These results are queued for the
75 application to pick from a queue::
76
77 # Callback function that returns a value:
78
79 def timeToDoSomething(word):
80 return word.upper()
81
82 mySimulator = EventSimulator()
83 eventSimulator.start(schedule, timeToDoSomething);
84 eventQueue = eventSimulator.getEventQueue()
85 while (True):
86 try:
87 event = eventQueue.get(block=True, timeout=4.0);
88 except Empty:
89 print("Queue empty for more than 4 seconds. Quitting.")
90 sys.exit();
91 print event;
92
93 The many 'if' statements below just ensure that the robot will do something
94 different each time you run this file.
95
96 '''
97
98 from robot_scripting import PR2RobotScript as pr2
99 from robot_scripting import aboutEq
100 from robot_scripting import FullPose
101
102 LEFT = pr2.LEFT
103 RIGHT = pr2.RIGHT
104 BOTH = pr2.BOTH
105
106 if aboutEq("l_gripper_joint", 0):
107 pr2.openGripper(LEFT)
108 else:
109 pr2.closeGripper(LEFT)
110 if aboutEq("r_gripper_joint", 0):
111 pr2.openGripper(RIGHT)
112 else:
113 pr2.closeGripper(RIGHT)
114
115 if aboutEq('head_tilt_joint', 30):
116 pr2.tiltHead(-30, duration=0.5)
117 else:
118 pr2.tiltHead(30, duration=0.5)
119
120 if aboutEq('head_pan_joint', 60):
121 pr2.panHead(-60)
122 else:
123 pr2.panHead(60)
124
125 if aboutEq('head_tilt_joint', 30):
126 pr2.lookAt(-100, -20 , 5, wait=False)
127 else:
128 pr2.lookAt(+100, +30 , 5, wait=False)
129
130 if aboutEq('l_shoulder_pan_joint', 90):
131 pr2.moveArmJoint('l_shoulder_pan_joint', 0, duration=2.0)
132 pr2.moveArmJoint(['l_forearm_roll_joint', 'l_elbow_flex_joint'], [30, 0], duration=2.0)
133 else:
134 pr2.moveArmJoint('l_shoulder_pan_joint', 90, duration=2.0)
135 pr2.moveArmJoint(['l_forearm_roll_joint', 'l_elbow_flex_joint'], [-30, -130], duration=2.0)
136
137 pr2.moveBase(place=(0.3,0.0,0.0))
138 pr2.moveBase(place=(-0.3,0.0,0.0))
139 pr2.moveBase(rotation=90)
140 pr2.moveBase(rotation=-90)
141
142 torsoState = pr2.getSensorReading('torso_lift_joint')
143
144 if aboutEq('torso_lift_joint', 0):
145 pr2.setTorso(0.3, wait=True)
146 print "Torso done"
147 else:
148 pr2.setTorso(0, duration=5, wait=True)
149 print "Torso done"
150
151
152
153
154 from event_simulators.event_simulator import EventSimulator
155 from Queue import Empty
156 from collections import OrderedDict
157 import sys
158
161
162
163 schedule = OrderedDict()
164 schedule[2.0] = 'This'
165 schedule[5.0] = 'is'
166 schedule[6.0] = 'a'
167 schedule[7.2] = 'test'
168
169 EventSimulator().start(schedule, printWord)
170
171
172
173
174
175
176
177
178
179
182
183 mySimulator = EventSimulator()
184 mySimulator.start(schedule, timeToDoSomething);
185 eventQueue = mySimulator.getEventQueue()
186 while (True):
187 try:
188 event = eventQueue.get(block=True, timeout=4.0);
189 except Empty:
190 print("Queue empty for more than 4 seconds. Quitting.")
191 sys.exit();
192 print("From the event queue: " + event);
193