walker.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from threading import Thread
00004 import rospy
00005 import math
00006 from darwin_gazebo.darwin import Darwin
00007 from geometry_msgs.msg import Twist
00008 
00009 
00010 class WJFunc:
00011     """
00012     Walk Joint Function CPG style
00013     Provides parameterized sine wave functions as y=offset+scale*(in_offset+in_scale*x)
00014     """
00015     def __init__(self):
00016         self.offset=0
00017         self.scale=1
00018         self.in_offset=0
00019         self.in_scale=1
00020         
00021     def get(self,x):
00022         """ x between 0 and 1"""
00023         f=math.sin(self.in_offset+self.in_scale*x)
00024         return self.offset+self.scale*f        
00025         
00026     def clone(self):
00027         z=WJFunc()
00028         z.offset=self.offset
00029         z.scale=self.scale
00030         z.in_offset=self.in_offset
00031         z.in_scale=self.in_scale
00032         return z
00033     
00034     def mirror(self):
00035         z=self.clone()
00036         z.offset*=-1
00037         z.scale*=-1
00038         return z
00039         
00040     def __str__(self):
00041         return "y=%f+%f*sin(%f+%f*x)"%(self.offset,self.scale,self.in_offset,self.in_scale)
00042         
00043 class WFunc:
00044     """
00045     Multi-joint walk function for Darwin    
00046     """
00047     def __init__(self,**kwargs):
00048         self.parameters={}
00049 
00050         self.parameters["swing_scale"]=0
00051         self.parameters["step_scale"]=0.3
00052         self.parameters["step_offset"]=0.55
00053         self.parameters["ankle_offset"]=0
00054         self.parameters["vx_scale"]=0.5
00055         self.parameters["vy_scale"]=0.5
00056         self.parameters["vt_scale"]=0.4
00057         
00058         for k,v in kwargs.items():
00059             self.parameters[k]=v
00060         
00061         self.generate()
00062         
00063     def generate(self):
00064         """
00065         Build CPG functions for walk-on-spot (no translation or rotation, only legs up/down)
00066         """        
00067         # f1=THIGH1=ANKLE1=L=R in phase
00068         self.pfn={} # phase joint functions    
00069         self.afn={} # anti phase joint functions
00070 
00071         #~ print f        
00072         f1=WJFunc()
00073         f1.in_scale=math.pi
00074         f1.scale=-self.parameters["swing_scale"]
00075         self.pfn["j_ankle2_l"]=f1
00076         self.pfn["j_thigh1_l"]=f1
00077         
00078         # f2=mirror f1 in antiphase
00079         f2=f1.mirror()
00080         #~ f2=WJFunc()
00081         self.afn["j_ankle2_l"]=f2
00082         self.afn["j_thigh1_l"]=f2
00083 
00084         f3=WJFunc()
00085         f3.in_scale=math.pi
00086         f3.scale=self.parameters["step_scale"]
00087         f3.offset=self.parameters["step_offset"]
00088         self.pfn["j_thigh2_l"]=f3
00089         f33=f3.mirror()
00090         f33.offset+=self.parameters["ankle_offset"]
00091         self.pfn["j_ankle1_l"]=f33
00092         
00093         f4=f3.mirror()
00094         f4.offset*=2
00095         f4.scale*=2
00096         self.pfn["j_tibia_l"]=f4
00097         
00098         s2=0
00099         f5=f3.clone()
00100         f5.in_scale*=2
00101         f5.scale=s2
00102         self.afn["j_thigh2_l"]=f5
00103         
00104         
00105         f6=f3.mirror()
00106         f6.in_scale*=2
00107         f6.scale=f5.scale
00108         f6.offset+=self.parameters["ankle_offset"]
00109         self.afn["j_ankle1_l"]=f6
00110         
00111         f7=f4.clone()
00112         f7.scale=0
00113         self.afn["j_tibia_l"]=f7
00114 
00115         
00116         self.forward=[f5,f6]
00117         
00118         self.generate_right()
00119         self.joints=self.pfn.keys()
00120         
00121         self.show()
00122        
00123     def generate_right(self):
00124         """
00125         Mirror CPG functions from left to right and antiphase right
00126         """
00127         l=[ v[:-2] for v in self.pfn.keys()]
00128         for j in l:
00129             self.pfn[j+"_r"]=self.afn[j+"_l"].mirror()
00130             self.afn[j+"_r"]=self.pfn[j+"_l"].mirror()
00131         
00132     def get(self,phase,x,velocity):
00133         """ Obtain the joint angles for a given phase, position in cycle (x 0,1)) and velocity parameters """
00134         angles={}
00135         for j in self.pfn.keys():
00136             if phase:
00137                 v=self.pfn[j].get(x)
00138                 angles[j]=v
00139             else:
00140                 angles[j]=self.afn[j].get(x)
00141         self.apply_velocity(angles,velocity,phase,x)
00142         return angles
00143             
00144             
00145         
00146         
00147     def show(self):
00148         """
00149         Display the CPG functions used
00150         """
00151         for j in self.pfn.keys():
00152             print j,"p",self.pfn[j],"a",self.afn[j]
00153         
00154     
00155     def apply_velocity(self,angles,velocity,phase,x):
00156         """ Modify on the walk-on-spot joint angles to apply the velocity vector"""
00157         
00158         # VX
00159         v=velocity[0]*self.parameters["vx_scale"]
00160         d=(x*2-1)*v
00161         if phase:
00162             angles["j_thigh2_l"]+=d
00163             angles["j_ankle1_l"]+=d
00164             angles["j_thigh2_r"]+=d
00165             angles["j_ankle1_r"]+=d
00166         else:
00167             angles["j_thigh2_l"]-=d
00168             angles["j_ankle1_l"]-=d
00169             angles["j_thigh2_r"]-=d
00170             angles["j_ankle1_r"]-=d
00171 
00172         # VY
00173         v=velocity[1]*self.parameters["vy_scale"]
00174         d=(x)*v
00175         d2=(1-x)*v
00176         if v>=0:
00177             if phase:
00178                 angles["j_thigh1_l"]-=d
00179                 angles["j_ankle2_l"]-=d
00180                 angles["j_thigh1_r"]+=d
00181                 angles["j_ankle2_r"]+=d
00182             else:
00183                 angles["j_thigh1_l"]-=d2
00184                 angles["j_ankle2_l"]-=d2
00185                 angles["j_thigh1_r"]+=d2
00186                 angles["j_ankle2_r"]+=d2
00187         else:
00188             if phase:
00189                 angles["j_thigh1_l"]+=d2
00190                 angles["j_ankle2_l"]+=d2
00191                 angles["j_thigh1_r"]-=d2
00192                 angles["j_ankle2_r"]-=d2
00193             else:
00194                 angles["j_thigh1_l"]+=d
00195                 angles["j_ankle2_l"]+=d
00196                 angles["j_thigh1_r"]-=d
00197                 angles["j_ankle2_r"]-=d
00198                 
00199         # VT
00200         v=velocity[2]*self.parameters["vt_scale"]
00201         d=(x)*v
00202         d2=(1-x)*v
00203         if v>=0:
00204             if phase:
00205                 angles["j_pelvis_l"]=-d
00206                 angles["j_pelvis_r"]=d
00207             else:
00208                 angles["j_pelvis_l"]=-d2
00209                 angles["j_pelvis_r"]=d2
00210         else:
00211             if phase:
00212                 angles["j_pelvis_l"]=d2
00213                 angles["j_pelvis_r"]=-d2
00214             else:
00215                 angles["j_pelvis_l"]=d
00216                 angles["j_pelvis_r"]=-d
00217 
00218 
00219 
00220 class Walker:
00221     """
00222     Class for making Darwin walk
00223     """
00224     def __init__(self,darwin):
00225         self.darwin=darwin
00226         self.running=False
00227 
00228         self.velocity=[0,0,0]
00229         self.walking=False
00230         self.func=WFunc()
00231 
00232         #~ self.ready_pos=get_walk_angles(10)
00233         self.ready_pos=self.func.get(True,0,[0,0,0])
00234         
00235         self._th_walk=None
00236 
00237         self._sub_cmd_vel=rospy.Subscriber(darwin.ns+"cmd_vel",Twist,self._cb_cmd_vel,queue_size=1)
00238 
00239 
00240     def _cb_cmd_vel(self,msg):
00241         """
00242         Catches cmd_vel and update walker speed
00243         """
00244         print "cmdvel",msg
00245         vx=msg.linear.x
00246         vy=msg.linear.y
00247         vt=msg.angular.z
00248         self.start()
00249         self.set_velocity(vx,vy,vt)
00250         
00251     def init_walk(self):
00252         """
00253         If not there yet, go to initial walk position
00254         """
00255         rospy.loginfo("Going to walk position")
00256         if self.get_dist_to_ready()>0.02:                    
00257             self.darwin.set_angles_slow(self.ready_pos)        
00258 
00259     def start(self):
00260         if not self.running:
00261             self.running=True                    
00262             self.init_walk()
00263             self._th_walk=Thread(target=self._do_walk)
00264             self._th_walk.start()
00265             self.walking=True
00266     
00267     def stop(self):
00268         if self.running:
00269             self.walking=False
00270             rospy.loginfo("Waiting for stopped")
00271             while not rospy.is_shutdown() and self._th_walk is not None:
00272                 rospy.sleep(0.1)                
00273             rospy.loginfo("Stopped")
00274             self.running=False
00275             
00276     def set_velocity(self,x,y,t):
00277         self.velocity=[x,y,t]
00278         
00279 
00280     def _do_walk(self):
00281         """
00282         Main walking loop, smoothly update velocity vectors and apply corresponding angles
00283         """
00284         r=rospy.Rate(100)
00285         rospy.loginfo("Started walking thread")
00286         func=self.func
00287         
00288         # Global walk loop
00289         n=50
00290         p=True
00291         i=0
00292         self.current_velocity=[0,0,0]
00293         while not rospy.is_shutdown() and (self.walking or i<n or self.is_walking()):
00294             if not self.walking:
00295                 self.velocity=[0,0,0]
00296             if not self.is_walking() and i==0: # Do not move if nothing to do and already at 0
00297                 self.update_velocity(self.velocity,n)
00298                 r.sleep()
00299                 continue
00300             x=float(i)/n            
00301             angles=func.get(p,x,self.current_velocity)
00302             self.update_velocity(self.velocity,n)
00303             self.darwin.set_angles(angles)
00304             i+=1
00305             if i>n:
00306                 i=0
00307                 p=not p
00308             r.sleep()
00309         rospy.loginfo("Finished walking thread")
00310         
00311         self._th_walk=None
00312 
00313     def is_walking(self):
00314         e=0.02
00315         for v in self.current_velocity:
00316             if abs(v)>e: return True
00317         return False
00318         
00319     def rescale(self,angles,coef):
00320         z={}
00321         for j,v in angles.items():            
00322             offset=self.ready_pos[j]
00323             v-=offset
00324             v*=coef
00325             v+=offset
00326             z[j]=v
00327         return z
00328             
00329 
00330     def update_velocity(self,target,n):
00331         a=3/float(n)
00332         b=1-a
00333         self.current_velocity=[a*t+b*v for (t,v) in zip(target,self.current_velocity)]
00334         
00335     def get_dist_to_ready(self):
00336         angles=self.darwin.get_angles()
00337         return get_distance(self.ready_pos,angles)
00338             
00339                 
00340 
00341 def interpolate(anglesa,anglesb,coefa):
00342     z={}
00343     joints=anglesa.keys()
00344     for j in joints:
00345         z[j]=anglesa[j]*coefa+anglesb[j]*(1-coefa)
00346     return z
00347 
00348 def get_distance(anglesa,anglesb):
00349     d=0
00350     joints=anglesa.keys()
00351     if len(joints)==0: return 0
00352     for j in joints:
00353         d+=abs(anglesb[j]-anglesa[j])
00354     d/=len(joints)
00355     return d
00356 
00357 
00358 if __name__=="__main__":
00359     rospy.init_node("walker")
00360     rospy.sleep(1)
00361     
00362     rospy.loginfo("Instantiating Darwin Client")
00363     darwin=Darwin()
00364     rospy.loginfo("Instantiating Darwin Walker")
00365     walker=Walker(darwin)
00366  
00367     rospy.loginfo("Darwin Walker Ready")
00368     while not rospy.is_shutdown():
00369         rospy.sleep(1)


darwin_gazebo
Author(s): Philippe Capdepuy
autogenerated on Fri Aug 28 2015 10:27:42