Source Code for Script script-demo_arm_py

 1  from joy import * 
 2   
 3 -class MoveArm( Plan ): 
 4 -  def __init__(self,*arg,**kw): 
 5      Plan.__init__(self,*arg,**kw) 
 6      r = self.app.robot.at 
 7      self.seg = [r.arm1,  r.arm3, r.arm4, r.arm5] # r.arm2, 
 8      self.grip = r.grip 
 9       
10 -  def onStart( self ): 
11      progress("Arm motion started") 
12     
13 -  def onStop( self ): 
14      progress("Going slack") 
15      for mod in [self.grip]+self.seg: 
16        mod.go_slack() 
17      progress("Arm motion stopped") 
18   
19 -  def behavior( self ): 
20      oncePer = self.app.onceEvery(0.5) 
21      while True: 
22        yield 
23        # Read joystick from application's StickFilter  
24        sf = self.app.sf 
25        bend = sf.getValue('joy0axis0') 
26        skew = sf.getValue('joy0axis1') 
27        wrist = sf.getValue('joy0axis4') 
28        grip = abs(sf.getValue('joy0axis3')) # must be positive! 
29        # 
30        # Set arm angles 
31        ang = [(bend+skew*k)*self.app.cfg.armGain for k in xrange(-len(self.seg)/2,len(self.seg)/2+1)] 
32        # Set angles of all except wrist 
33        for mod,pos in zip(self.seg[:-1],ang[:-1]): 
34          mod.set_pos(pos) 
35        # Set wrist angle 
36        w = ang[-1]+wrist*self.app.cfg.wristGain 
37        self.seg[-1].set_pos(w) 
38        # Set gripper 
39        self.grip.set_pos(grip*self.app.cfg.gripGain) 
40        # 
41        if oncePer(): 
42          progress("Arm: bend %6f skew %6f wrist %6f grip %6f" 
43            % (bend,skew,wrist,grip)) 
44       
45 -class ArmApp( JoyApp ): 
46 -  def __init__(self,robot=dict(count=5),*arg,**kw): 
47      cfg = dict( 
48        armGain = 50, #250, 
49        gripGain = 8500, 
50        wristGain = 50, #250, 
51        ) 
52      JoyApp.__init__(self,robot=robot,cfg=cfg,*arg,**kw) 
53     
54 -  def onStart(self): 
55      sf = StickFilter(self,dt=0.05) 
56      sf.setIntegrator( 'joy0axis0',10 ) 
57      sf.setIntegrator( 'joy0axis1',10 ) 
58      sf.setIntegrator( 'joy0axis4' ) 
59      sf.setLowpass( 'joy0axis3', 20 ) 
60      sf.start() 
61      self.sf = sf 
62      self.ma = MoveArm(self)  
63         
64 -  def onEvent(self,evt): 
65      if evt.type == JOYAXISMOTION: 
66        # Forward a copy of the event to the StickFilter plan 
67        self.sf.push(evt) 
68      # 
69      # Buttons --> press button0 to stop arm; release to start 
70      elif evt.type==JOYBUTTONDOWN and evt.joy==0 and evt.button==0: 
71        self.ma.stop() 
72      elif evt.type==JOYBUTTONUP and evt.joy==0 and evt.button==0: 
73        self.ma.start() 
74      # 
75      # Hide robot position events 
76      elif evt.type==CKBOTPOSITION: 
77        return 
78      JoyApp.onEvent(self,evt) 
79   
80 -  def onStop(self): 
81      self.ma.onStop() 
82         
83  if __name__=="__main__": 
84    app = ArmApp() 
85    app.run() 
86