00001
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
00068 self.pfn={}
00069 self.afn={}
00070
00071
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
00079 f2=f1.mirror()
00080
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
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
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
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
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
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:
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)