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


phantomx_gazebo
Author(s): Philippe Capdepuy
autogenerated on Thu Aug 27 2015 14:22:51