Package robot_script :: Module example
[hide private]
[frames] | no frames]

Source Code for Module robot_script.example

  1  #!/usr/bin/env python 
  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  # Advanced: event simulators that generate events via callbacks: 
153   
154  from event_simulators.event_simulator import EventSimulator 
155  from Queue import Empty 
156  from collections import OrderedDict 
157  import sys 
158   
159 -def printWord(word):
160 print(word)
161 162 # Display 'This is a test,' with words printed at 2,5,6, and 7.2 seconds: 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 # Even more advanced: callback 173 174 # Note: this word upper-casing example will run simultaneously 175 # with the example above. While the example above will 176 # print the words within the callback function, the following 177 # example has the callback function do the work, but this application 178 # picks the result up from a thread-safe queue 179
180 -def timeToDoSomething(word):
181 return word.upper()
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