四足机器人步态仿真（三）四足机器人基础步态仿真

观前提醒，本章主要内容为分析四足机器人步态实现和姿态控制，碰撞体积等程序
步态效果：

一、完整代码如下

# -*- coding: utf-8 -*-import pybullet as pimport timeimport numpy as npp.connect(p.GUI)p.createCollisionShape(p.GEOM_PLANE)p.createMultiBody(0,0)#Dog robot part shapessh_body = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.45, 0.08, 0.02])sh_extraweight = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.45, 0.08, 0.025])sh_roll = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.02, 0.02, 0.02])sh_hip = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.02, 0.02, 0.02])sh_knee = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.02, 0.02, 0.02])sh_foot = p.createCollisionShape(p.GEOM_SPHERE,radius=0.04)#The Dog robot is the body with other shapes linked to itbody_Mass = 1visualShapeId = -1link_Masses=[.1, .1, .1, .1,             .1, .1, .1, .1,             .1, .1, .1, .1,             .1, .1, .1, .1,             20]linkCollisionShapeIndices=[sh_roll, sh_hip, sh_knee, sh_foot,                           sh_roll, sh_hip, sh_knee, sh_foot,                           sh_roll, sh_hip, sh_knee, sh_foot,                           sh_roll, sh_hip, sh_knee, sh_foot,                           sh_extraweight]nlnk=len(link_Masses)linkVisualShapeIndices=[-1]*nlnk    #=[-1,-1,-1, ... , -1]#link positions wrt the link they are attached toxhipf=0.4xhipb=-0.4yhipl=0.1xoffh=0.05yoffh=0.05hu=0.3hl=0.3linkPositions=[[xhipf, yhipl, 0], [xoffh, yoffh, 0], [0, 0, -hu], [0, 0, -hl],               [xhipf, -yhipl, 0], [xoffh, -yoffh, 0], [0, 0, -hu], [0, 0, -hl],               [xhipb, yhipl, 0], [xoffh, yoffh, 0], [0, 0, -hu], [0, 0, -hl],               [xhipb, -yhipl, 0], [xoffh, -yoffh, 0], [0, 0, -hu], [0, 0, -hl],               [0,0,+0.029]]linkOrientations=[[0,0,0,1]]*nlnklinkInertialFramePositions=[[0,0,0]]*nlnk#Note the orientations are given in quaternions (4 params). There are function to convert of Euler angles and backlinkInertialFrameOrientations=[[0,0,0,1]]*nlnk#indices determine for each link which other link it is attached to# for example 3rd index = 2 means that the front left knee jjoint is attached to the front left hipindices=[0, 1, 2, 3,         0, 5, 6, 7,         0, 9,10,11,         0,13,14,15,         0]#Most joint are revolving. The prismatic joints are kept fixed for nowjointTypes=[p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_PRISMATIC,            p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_PRISMATIC,            p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_PRISMATIC,            p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_PRISMATIC,            p.JOINT_PRISMATIC]#revolution axis for each revolving jointaxis=[[1,0,0], [0,1,0], [0,1,0], [0,0,1],      [1,0,0], [0,1,0], [0,1,0], [0,0,1],      [1,0,0], [0,1,0], [0,1,0], [0,0,1],      [1,0,0], [0,1,0], [0,1,0], [0,0,1],      [0,0,1]]#Drop the body in the scene at the following body coordinatesbasePosition = [0,0,1]baseOrientation = [0,0,0,1]#Main function that creates the dogdog = p.createMultiBody(body_Mass,sh_body,visualShapeId,basePosition,baseOrientation,                        linkMasses=link_Masses,                        linkCollisionShapeIndices=linkCollisionShapeIndices,                        linkVisualShapeIndices=linkVisualShapeIndices,                        linkPositions=linkPositions,                        linkOrientations=linkOrientations,                        linkInertialFramePositions=linkInertialFramePositions,                        linkInertialFrameOrientations=linkInertialFrameOrientations,                        linkParentIndices=indices,                        linkJointTypes=jointTypes,                        linkJointAxis=axis)            #Due to the weight the prismatic extraweight block needs to be motored upjoint=16p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=0.01,force=1000,maxVelocity=3)#Same for the prismatic feet spheresjoint=3p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=0.0,force=1000,maxVelocity=3)joint=7p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=0.0,force=1000,maxVelocity=3)joint=11p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=0.0,force=1000,maxVelocity=3)joint=15p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=0.0,force=1000,maxVelocity=3)#Add earth like gravityp.setGravity(0,0,-9.81)p.setRealTimeSimulation(1)#Point the camera at the robot at the desired angle and distancep.resetDebugVisualizerCamera( cameraDistance=1.5, cameraYaw=-30, cameraPitch=-30, cameraTargetPosition=[0.0, 0.0, 0.25])if (0):    t0=time.time()    t=time.time()    while ((t-t0)&lt;10):        t=time.time()    p.disconnect()    brake#Scenery e.g. an inclined boxboxHalfLength = 2.5boxHalfWidth = 2.5boxHalfHeight = 0.2sh_colBox = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])mass = 1block=p.createMultiBody(baseMass=0,baseCollisionShapeIndex = sh_colBox,                        basePosition = [-2,0,-0.1],baseOrientation=[0.0,0.1,0.0,1])#Add extra lateral friction to the feet spheresp.changeDynamics(dog,3,lateralFriction=2)p.changeDynamics(dog,7,lateralFriction=2)p.changeDynamics(dog,11,lateralFriction=2)p.changeDynamics(dog,15,lateralFriction=2)#Function to calculate roll, hip and knee angles from the x,y,z coords of the foot wrt the hip.def xyztoang(x,y,z,yoffh,hu,hl):    dyz=np.sqrt(y**2+z**2)    lyz=np.sqrt(dyz**2-yoffh**2)    gamma_yz=-np.arctan(y/z)    gamma_h_offset=-np.arctan(-yoffh/lyz)    gamma=gamma_yz-gamma_h_offset    lxzp=np.sqrt(lyz**2+x**2)    n=(lxzp**2-hl**2-hu**2)/(2*hu)    beta=-np.arccos(n/hl)    alfa_xzp=-np.arctan(x/lyz)    alfa_off=np.arccos((hu+n)/lxzp)    alfa=alfa_xzp+alfa_off    if any( np.isnan([gamma,alfa,beta])):        print(x,y,z,yoffh,hu,hl)    return [gamma,alfa,beta]def setlegsxyz(xvec,yvec,zvec,vvec):    #[a1,a2]=xztoang(xvec[0],zvec[0],1,1)    a=xyztoang(xvec[0]-xhipf,yvec[0]-yhipl,zvec[0],yoffh,hu,hl)  #(x,y,z,yoffh,hu,hl)    spd=1    #any(np.isnan(a))    joint=0    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[0],force=1000,maxVelocity=spd)    joint=1    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[1],force=1000,maxVelocity=vvec[0])    joint=2    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[2],force=1000,maxVelocity=vvec[0])    a=xyztoang(xvec[1]-xhipf,yvec[1]+yhipl,zvec[1],-yoffh,hu,hl)  #(x,y,z,yoffh,hu,hl)    spd=1.0    joint=4    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[0],force=1000,maxVelocity=spd)    joint=5    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[1],force=1000,maxVelocity=vvec[1])    joint=6    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[2],force=1000,maxVelocity=vvec[1])    a=xyztoang(xvec[2]-xhipb,yvec[2]-yhipl,zvec[2],yoffh,hu,hl)  #(x,y,z,yoffh,hu,hl)    spd=1.0    joint=8    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[0],force=1000,maxVelocity=spd)    joint=9    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[1],force=1000,maxVelocity=vvec[2])    joint=10    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[2],force=1000,maxVelocity=vvec[2])    a=xyztoang(xvec[3]-xhipb,yvec[3]+yhipl,zvec[3],-yoffh,hu,hl)  #(x,y,z,yoffh,hu,hl)    spd=1.0    joint=12    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[0],force=1000,maxVelocity=spd)    joint=13    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[1],force=1000,maxVelocity=vvec[3])    joint=14    p.setJointMotorControl2(dog,joint,p.POSITION_CONTROL,targetPosition=a[2],force=1000,maxVelocity=vvec[3])#Pre-init robot positionsetlegsxyz([xhipf,xhipf,xhipb,xhipb],[yhipl+0.1,-yhipl-0.1,yhipl+0.1,-yhipl-0.1],[-0.5,-0.5,-0.5,-0.5],[1,1,1,1])t0=time.time()t=time.time()while ((t-t0)&lt;4):    t=time.time()#Rotation matrix for yaw only between robot-frame and world-framedef RotYawr(yawr):    Rhor=np.array([[np.cos(yawr),-np.sin(yawr),0], [np.sin(yawr),np.cos(yawr),0], [0,0,1]])    return Rhor#Init robot position, orientation and pose params# O means in world-centered coordinates# R means in robot-centered coordinates# r is for "of the robot"# i is initialyawri=1.3xrOi=np.array([1,1,0.5])legsRi=np.array([[xhipf,xhipf,xhipb,xhipb],               [yhipl+0.1,-yhipl-0.1,yhipl+0.1,-yhipl-0.1],               [-0.5,-0.5,-0.5,-0.5]])#Set body to the robot posxbOi=xrOi#Init body position and orientationquat=p.getQuaternionFromEuler([0,0,yawri])p.resetBasePositionAndOrientation(dog,xbOi,quat)#Init leg abs posRyawri=RotYawr(yawri)legsO=(np.dot(Ryawri,legsRi).T + xbOi).T   #Apply rotation plus translation#Set the non-initial variables and matrixyawr=yawrixrO=xrOixbO=xrORyawr=RotYawr(yawri)#Recalc leg rel pos in robot frame and set the legsdlegsO=(legsO.T-xbO).TdlegsR=np.dot(Ryawr.T,dlegsO)setlegsxyz(dlegsR[0],dlegsR[1],dlegsR[2],[1,1,1,1])#Calculate a new robot center position from the average of the feet positions#Calculate a new robot yaw ditrection also from the feet positionsxfO=(legsO[:,0]+legsO[:,1])/2.0xbO=(legsO[:,2]+legsO[:,3])/2.0xrOn=(xfO+xbO)/2.0 + np.array([0,0,0.5])xfmbO=xfO-xbOyawrn=np.arctan2(xfmbO[1],xfmbO[0])#Camera paramers to be able to yaw pitch and zoom the camera (Focus remains on the robot) cyaw=10cpitch=-15cdist=1.5#Walking speed (changes the walking loop time)walkLoopSpd=400#Change general motor speedvvec=[12]*4#Current leg to change positionl=0#Init the center for the robot rotation to the current robot posxrcO=xrO#Set the body position to the robot positionxoff=0yoff=0#Init to walking fwddr=0drp=0#Leg sequence (for rotating the robot, I chose to chg legs in the order front-left, fr, br, bl)lseq=[0,1,3,2]lseqp=[0,1,3,2]#lseq=[2,0,3,1]#lseqp=[2,0,3,1]while (1):    cubePos, cubeOrn = p.getBasePositionAndOrientation(dog)    p.resetDebugVisualizerCamera( cameraDistance=cdist, cameraYaw=cyaw, cameraPitch=cpitch, cameraTargetPosition=cubePos)    keys = p.getKeyboardEvents()    #Keys to change camera    if keys.get(100):  #D        cyaw+=1    if keys.get(97):   #A        cyaw-=1    if keys.get(99):   #C        cpitch+=1    if keys.get(102):  #F        cpitch-=1    if keys.get(122):  #Z        cdist+=.01    if keys.get(120):  #X        cdist-=.01    #Keys to change the robot walk (fwd, bkw, rot right, rot left)    if keys.get(65297): #Up        drp=0    if keys.get(65298): #Down        drp=2    if keys.get(65296): #Right        drp=1        xrcO=xrO        #Set the center for the robot rotation to the current robot pos        lseqp=[1,0,2,3] #Change the leg sequence to open up the front arms rather than close them    if keys.get(65295): #Left        drp=3        xrcO=xrO        lseqp=[0,1,3,2] #Change the leg sequence to open up the front arms rather than close them    #Time cycle    tv=int(((time.time()-t0)*walkLoopSpd)  % 800)    #One leg movement in 200 units. one 4-leg walk cycle in 800 units    #Using &lt;, &gt;, % (modulo) and divide we can easily do something in a specific part of the cycle    #Apply new walking cycle type (e.g. chg from fwd to bkw) only at the start of next cycle    if tv&lt;20 and (not dr==drp):        dr=drp        lseq=lseqp    #Index of the leg to move    l=int(tv/200)    #Actual leg to move    k=lseq[l]    #In the beginning of the leg cycle the body is centered at the robot center    #then it gradually moves in the opposite direction of the leg to be moved     #to ensure the center of gravity remains on the other 3 legs    #when the moving leg goes down again the body center returns to the robot center    #The vars xoff and yoff move the body w.r.t. the robot center in the robot frame    if int(tv%200)&lt;10:        xoff=0        yoff=0    elif int(tv%200)&lt;80:        xoff+=0.002*(-1+2*int(k/2))  #Work it out on paper to see it moves opposite to the stepping leg        yoff+=0.002*(-1+2*(k%2))         elif int(tv%200)&gt;160:        xoff-=0.004*(-1+2*int(k/2))        yoff-=0.004*(-1+2*(k%2))         #Recalc leg rel pos in desired robot frame    dlegsO=(legsO.T-xrO).T  #Translate    dlegsR=np.dot(Ryawr.T,dlegsO)  #Rotate (Note the inverse rotation is the transposed matrix)    #Then apply the body movement and set the legs    setlegsxyz(dlegsR[0]-xoff-0.03,dlegsR[1]-yoff,dlegsR[2],vvec)  #0.03 is for tweaking the center of grav.    if int(tv%200)&gt;80:        dlegsO=(legsO.T-xrcO).T        yawlO=np.arctan2(dlegsO[1,k],dlegsO[0,k])        rlO=np.sqrt(dlegsO[0,k]**2+dlegsO[1,k]**2)        if dr==0:            legsO[0,k]=rlO*np.cos(yawlO)+xrcO[0]+0.01*np.cos(yawr)            legsO[1,k]=rlO*np.sin(yawlO)+xrcO[1]+0.01*np.sin(yawr)        elif dr==1:            yawlO-=0.015             legsO[0,k]=rlO*np.cos(yawlO)+xrcO[0]            legsO[1,k]=rlO*np.sin(yawlO)+xrcO[1]        elif dr==2:            legsO[0,k]=rlO*np.cos(yawlO)+xrcO[0]-0.01*np.cos(yawr)            legsO[1,k]=rlO*np.sin(yawlO)+xrcO[1]-0.01*np.sin(yawr)        elif dr==3:            yawlO+=0.015             legsO[0,k]=rlO*np.cos(yawlO)+xrcO[0]            legsO[1,k]=rlO*np.sin(yawlO)+xrcO[1]        if int(tv%200)&lt;150:            #Move leg k upwards             legsO[2,k]+=.006        else:            #Move leg k wards             legsO[2,k]-=.006    else:        #Move/keep all legs down to the ground        legsO[2,0]=0.0        legsO[2,1]=0.0        legsO[2,2]=0.0        legsO[2,3]=0.0    #Calculate vectors and matrix for the next loop    xfrO=(legsO[:,0]+legsO[:,1])/2.0    xbkO=(legsO[:,2]+legsO[:,3])/2.0    xrO=(xfrO+xbkO)/2.0     xrO[2]=0.5    xfmbO=xfrO-xbkO    yawr=np.arctan2(xfmbO[1],xfmbO[0])    Ryawr=RotYawr(yawr)    time.sleep(0.01)p.disconnect()

运行上述代码，我们可以看到四足机器人的形态、碰撞体积、基础步态
另外此程序还在场景中仿真了一个斜坡用于测试