cpg 控制算法一种python实现（参考《仿生四足机器人技术》）

参考《仿生四足机器人技术》
使用python实现四足机器人CPG控制网络二，即xi作为髋关节输出，对yi输出整型作为膝关节输出。
因为没有经验，不知道输出信号是否正确，发到网上希望能够得到一些指正。

代码如下：

import numpy as np
import matplotlib.pyplot as plt
from scipy import integrate
import time
import math

def get_param():
    alpha = 10 #收敛速度
    leg_num = 4 #腿的数量
    gait = 2 #步态选择
    mu = 1
    a = 50
    psai = [1, 1, -1, -1] #关节形式 膝式-1 肘式 1
    omega_sw = 2*np.pi
    u1=0
    u2=0 #误差 影响x,y平衡位置
    h = 0.02 #抬腿高度
    v = 1 #行走速度
    gait_t = 0.4#步态周期
    s = v*gait_t
    l = 0.4 #腿节长度
    theta0 = math.radians(30)#髋关节和膝关节平衡位置与垂直线夹角
    L = 2*l*np.cos(theta0)#髋关节与足端之间长度
    ah = 15 #np.arcsin((beta*s)/(2*L)) 髋关节摆动幅度
    ak =10 #膝关节摆动幅度
    return [alpha,leg_num,gait,mu,a,psai,omega_sw,u1,u2]
def get_gait(gait_m):
    #walk
    global beta, phase, v, t, h, ah, ak
    if gait_m == 1:
        beta=0.75
        phase=[0,0.5,0.25,0.75]
        v=0.3
        t=0.8
        h=0.02
        ah=7.5
        ak=5.3
    #trot
    elif gait_m == 2:
        beta=0.5
        phase=[0,0.5,0,0.5]
        v = 1
        t = 0.4
        h = 0.02
        ah = 8.3
        ak = 5.3
    #pace
    elif gait_m == 3:
        beta=0.5
        phase=[0,0.5,0.5,0]
        v = 1.2
        t = 0.4
        h = 0.02
        ah = 10
        ak = 5.3
    #gallop
    elif gait_m == 4:
        beta = 0.5
        phase = [0,