walker.py

Go to the documentation of this file.

#!/usr/bin/env python

from threading import Thread
import rospy
import math
from darwin_gazebo.darwin import Darwin
from geometry_msgs.msg import Twist


class WJFunc:
    """
    Walk Joint Function CPG style
    Provides parameterized sine wave functions as y=offset+scale*(in_offset+in_scale*x)
    """
    def __init__(self):
        self.offset=0
        self.scale=1
        self.in_offset=0
        self.in_scale=1
        
    def get(self,x):
        """ x between 0 and 1"""
        f=math.sin(self.in_offset+self.in_scale*x)
        return self.offset+self.scale*f        
        
    def clone(self):
        z=WJFunc()
        z.offset=self.offset
        z.scale=self.scale
        z.in_offset=self.in_offset
        z.in_scale=self.in_scale
        return z
    
    def mirror(self):
        z=self.clone()
        z.offset*=-1
        z.scale*=-1
        return z
        
    def __str__(self):
        return "y=%f+%f*sin(%f+%f*x)"%(self.offset,self.scale,self.in_offset,self.in_scale)
        
class WFunc:
    """
    Multi-joint walk function for Darwin    
    """
    def __init__(self,**kwargs):
        self.parameters={}

        self.parameters["swing_scale"]=0
        self.parameters["step_scale"]=0.3
        self.parameters["step_offset"]=0.55
        self.parameters["ankle_offset"]=0
        self.parameters["vx_scale"]=0.5
        self.parameters["vy_scale"]=0.5
        self.parameters["vt_scale"]=0.4
        
        for k,v in kwargs.items():
            self.parameters[k]=v
        
        self.generate()
        
    def generate(self):
        """
        Build CPG functions for walk-on-spot (no translation or rotation, only legs up/down)
        """        
        # f1=THIGH1=ANKLE1=L=R in phase
        self.pfn={} # phase joint functions    
        self.afn={} # anti phase joint functions

        #~ print f        
        f1=WJFunc()
        f1.in_scale=math.pi
        f1.scale=-self.parameters["swing_scale"]
        self.pfn["j_ankle2_l"]=f1
        self.pfn["j_thigh1_l"]=f1
        
        # f2=mirror f1 in antiphase
        f2=f1.mirror()
        #~ f2=WJFunc()
        self.afn["j_ankle2_l"]=f2
        self.afn["j_thigh1_l"]=f2

        f3=WJFunc()
        f3.in_scale=math.pi
        f3.scale=self.parameters["step_scale"]
        f3.offset=self.parameters["step_offset"]
        self.pfn["j_thigh2_l"]=f3
        f33=f3.mirror()
        f33.offset+=self.parameters["ankle_offset"]
        self.pfn["j_ankle1_l"]=f33
        
        f4=f3.mirror()
        f4.offset*=2
        f4.scale*=2
        self.pfn["j_tibia_l"]=f4
        
        s2=0
        f5=f3.clone()
        f5.in_scale*=2
        f5.scale=s2
        self.afn["j_thigh2_l"]=f5
        
        
        f6=f3.mirror()
        f6.in_scale*=2
        f6.scale=f5.scale
        f6.offset+=self.parameters["ankle_offset"]
        self.afn["j_ankle1_l"]=f6
        
        f7=f4.clone()
        f7.scale=0
        self.afn["j_tibia_l"]=f7

        
        self.forward=[f5,f6]
        
        self.generate_right()
        self.joints=self.pfn.keys()
        
        self.show()
       
    def generate_right(self):
        """
        Mirror CPG functions from left to right and antiphase right
        """
        l=[ v[:-2] for v in self.pfn.keys()]
        for j in l:
            self.pfn[j+"_r"]=self.afn[j+"_l"].mirror()
            self.afn[j+"_r"]=self.pfn[j+"_l"].mirror()
        
    def get(self,phase,x,velocity):
        """ Obtain the joint angles for a given phase, position in cycle (x 0,1)) and velocity parameters """
        angles={}
        for j in self.pfn.keys():
            if phase:
                v=self.pfn[j].get(x)
                angles[j]=v
            else:
                angles[j]=self.afn[j].get(x)
        self.apply_velocity(angles,velocity,phase,x)
        return angles
            
            
        
        
    def show(self):
        """
        Display the CPG functions used
        """
        for j in self.pfn.keys():
            print j,"p",self.pfn[j],"a",self.afn[j]
        
    
    def apply_velocity(self,angles,velocity,phase,x):
        """ Modify on the walk-on-spot joint angles to apply the velocity vector"""
        
        # VX
        v=velocity[0]*self.parameters["vx_scale"]
        d=(x*2-1)*v
        if phase:
            angles["j_thigh2_l"]+=d
            angles["j_ankle1_l"]+=d
            angles["j_thigh2_r"]+=d
            angles["j_ankle1_r"]+=d
        else:
            angles["j_thigh2_l"]-=d
            angles["j_ankle1_l"]-=d
            angles["j_thigh2_r"]-=d
            angles["j_ankle1_r"]-=d

        # VY
        v=velocity[1]*self.parameters["vy_scale"]
        d=(x)*v
        d2=(1-x)*v
        if v>=0:
            if phase:
                angles["j_thigh1_l"]-=d
                angles["j_ankle2_l"]-=d
                angles["j_thigh1_r"]+=d
                angles["j_ankle2_r"]+=d
            else:
                angles["j_thigh1_l"]-=d2
                angles["j_ankle2_l"]-=d2
                angles["j_thigh1_r"]+=d2
                angles["j_ankle2_r"]+=d2
        else:
            if phase:
                angles["j_thigh1_l"]+=d2
                angles["j_ankle2_l"]+=d2
                angles["j_thigh1_r"]-=d2
                angles["j_ankle2_r"]-=d2
            else:
                angles["j_thigh1_l"]+=d
                angles["j_ankle2_l"]+=d
                angles["j_thigh1_r"]-=d
                angles["j_ankle2_r"]-=d
                
        # VT
        v=velocity[2]*self.parameters["vt_scale"]
        d=(x)*v
        d2=(1-x)*v
        if v>=0:
            if phase:
                angles["j_pelvis_l"]=-d
                angles["j_pelvis_r"]=d
            else:
                angles["j_pelvis_l"]=-d2
                angles["j_pelvis_r"]=d2
        else:
            if phase:
                angles["j_pelvis_l"]=d2
                angles["j_pelvis_r"]=-d2
            else:
                angles["j_pelvis_l"]=d
                angles["j_pelvis_r"]=-d



class Walker:
    """
    Class for making Darwin walk
    """
    def __init__(self,darwin):
        self.darwin=darwin
        self.running=False

        self.velocity=[0,0,0]
        self.walking=False
        self.func=WFunc()

        #~ self.ready_pos=get_walk_angles(10)
        self.ready_pos=self.func.get(True,0,[0,0,0])
        
        self._th_walk=None

        self._sub_cmd_vel=rospy.Subscriber(darwin.ns+"cmd_vel",Twist,self._cb_cmd_vel,queue_size=1)


    def _cb_cmd_vel(self,msg):
        """
        Catches cmd_vel and update walker speed
        """
        print "cmdvel",msg
        vx=msg.linear.x
        vy=msg.linear.y
        vt=msg.angular.z
        self.start()
        self.set_velocity(vx,vy,vt)
        
    def init_walk(self):
        """
        If not there yet, go to initial walk position
        """
        rospy.loginfo("Going to walk position")
        if self.get_dist_to_ready()>0.02:                    
            self.darwin.set_angles_slow(self.ready_pos)        

    def start(self):
        if not self.running:
            self.running=True                    
            self.init_walk()
            self._th_walk=Thread(target=self._do_walk)
            self._th_walk.start()
            self.walking=True
    
    def stop(self):
        if self.running:
            self.walking=False
            rospy.loginfo("Waiting for stopped")
            while not rospy.is_shutdown() and self._th_walk is not None:
                rospy.sleep(0.1)                
            rospy.loginfo("Stopped")
            self.running=False
            
    def set_velocity(self,x,y,t):
        self.velocity=[x,y,t]
        

    def _do_walk(self):
        """
        Main walking loop, smoothly update velocity vectors and apply corresponding angles
        """
        r=rospy.Rate(100)
        rospy.loginfo("Started walking thread")
        func=self.func
        
        # Global walk loop
        n=50
        p=True
        i=0
        self.current_velocity=[0,0,0]
        while not rospy.is_shutdown() and (self.walking or i<n or self.is_walking()):
            if not self.walking:
                self.velocity=[0,0,0]
            if not self.is_walking() and i==0: # Do not move if nothing to do and already at 0
                self.update_velocity(self.velocity,n)
                r.sleep()
                continue
            x=float(i)/n            
            angles=func.get(p,x,self.current_velocity)
            self.update_velocity(self.velocity,n)
            self.darwin.set_angles(angles)
            i+=1
            if i>n:
                i=0
                p=not p
            r.sleep()
        rospy.loginfo("Finished walking thread")
        
        self._th_walk=None

    def is_walking(self):
        e=0.02
        for v in self.current_velocity:
            if abs(v)>e: return True
        return False
        
    def rescale(self,angles,coef):
        z={}
        for j,v in angles.items():            
            offset=self.ready_pos[j]
            v-=offset
            v*=coef
            v+=offset
            z[j]=v
        return z
            

    def update_velocity(self,target,n):
        a=3/float(n)
        b=1-a
        self.current_velocity=[a*t+b*v for (t,v) in zip(target,self.current_velocity)]
        
    def get_dist_to_ready(self):
        angles=self.darwin.get_angles()
        return get_distance(self.ready_pos,angles)
            
                

def interpolate(anglesa,anglesb,coefa):
    z={}
    joints=anglesa.keys()
    for j in joints:
        z[j]=anglesa[j]*coefa+anglesb[j]*(1-coefa)
    return z

def get_distance(anglesa,anglesb):
    d=0
    joints=anglesa.keys()
    if len(joints)==0: return 0
    for j in joints:
        d+=abs(anglesb[j]-anglesa[j])
    d/=len(joints)
    return d


if __name__=="__main__":
    rospy.init_node("walker")
    rospy.sleep(1)
    
    rospy.loginfo("Instantiating Darwin Client")
    darwin=Darwin()
    rospy.loginfo("Instantiating Darwin Walker")
    walker=Walker(darwin)
 
    rospy.loginfo("Darwin Walker Ready")
    while not rospy.is_shutdown():
        rospy.sleep(1)