上一小结

【单足坐标运动学正解】https://www.jianshu.com/writer#/notebooks/53230205/notes/106287268/preview

公式

扭矩=jacobi_T * force

舵狗上可以等价于：
舵机角度 = jacobi_T * 运动方向

代码

上一章用的sympy现在要开始发力了
全自动求导：

# 正向解 转换 numpy 比 python快一丁点
fx=sy.lambdify((w,f,b),x4,"numpy")
fz=sy.lambdify((w,f,b),z4,"numpy")
fy=sy.lambdify((w,f,b),y4,"numpy")

transform = [
    [sy.lambdify((w,f,b),sy.diff(x4,w),"numpy"),sy.lambdify((w,f,b),sy.diff(x4,f),"numpy"),sy.lambdify((w,f,b),sy.diff(x4,b),"numpy")],
    [sy.lambdify((w,f,b),sy.diff(y4,w),"numpy"),sy.lambdify((w,f,b),sy.diff(y4,f),"numpy"),sy.lambdify((w,f,b),sy.diff(y4,b),"numpy")],
    [sy.lambdify((w,f,b),sy.diff(z4,w),"numpy"),sy.lambdify((w,f,b),sy.diff(z4,f),"numpy"),sy.lambdify((w,f,b),sy.diff(z4,b),"numpy")],
]# 正向解 转换 numpy 比 python快一丁点
fx=sy.lambdify((w,f,b),x4,"numpy")
fz=sy.lambdify((w,f,b),z4,"numpy")
fy=sy.lambdify((w,f,b),y4,"numpy")

transform = [
    [sy.lambdify((w,f,b),sy.diff(x4,w),"numpy"),sy.lambdify((w,f,b),sy.diff(x4,f),"numpy"),sy.lambdify((w,f,b),sy.diff(x4,b),"numpy")],
    [sy.lambdify((w,f,b),sy.diff(y4,w),"numpy"),sy.lambdify((w,f,b),sy.diff(y4,f),"numpy"),sy.lambdify((w,f,b),sy.diff(y4,b),"numpy")],
    [sy.lambdify((w,f,b),sy.diff(z4,w),"numpy"),sy.lambdify((w,f,b),sy.diff(z4,f),"numpy"),sy.lambdify((w,f,b),sy.diff(z4,b),"numpy")],
]

#当前位置到矩阵
def angle_to_pos(nowAngle):
    return np.array([fx(*nowAngle),fy(*nowAngle),fz(*nowAngle)])

#通过当前位置和方向确定 角度的差值
def nowAng_and_force_to_angDiff(nowAngle,direct):
    result = [0,0,0]
    for i in range(0,3):
        for j in range(0,3):
            result[i]+=(transform[j][i](nowAngle[0],nowAngle[1],nowAngle[2]))*direct[j]
    return np.array(result)

验证

als = []

now = np.array([0.0,45.0,-45.0])
targetPos = np.array([10, 100, 12 ])
tick = 40
for i in range(0,tick+1):
    targetPos = np.array([40+i/tick*30, 140-i/tick*160, 30+i/tick*30 ])
    
    nowPos = angle_to_pos(now)
    force = (targetPos - nowPos)/2
    ang = angle_to_speed_expect_force(now,force)
    now += ang

    #舵狗真实操练
    legCtrl(1,now[0],now[1],-now[2],500)
    
    time.sleep(0.1)
    
    als.append([now[0],now[1],now[2]])

效果稳的一批

image.png

als = []

now = np.array([0.0,45.0,-45.0])
targetPos = np.array([10, 100, 12 ])
tick = 100
for i in range(0,tick+1):
    targetPos = np.array([0, 100+i/tick*80, 12 ])
    
    nowPos = angle_to_pos(now)
    force = normalize(targetPos - nowPos)
    ang = angle_to_speed_expect_force(now,force)
    now += ang
    
    legCtrl(1,now[0],now[1],-now[2],500)
    
    time.sleep(0.05)
    
    als.append([now[0],now[1],now[2]])

image.png