《机器人学一(Robotics(1))》_台大林沛群 第 7 周 【轨迹规划_综合】Quiz 7

题 4-5 存疑，仅供参考，欢迎交流

由

import numpy as np

## 杯子 中心 C 在 点 P0 处 
T_0C = [[1, 0, 0, 630],
        [0, 1, 0, 364],
        [0, 0, 1, 20],
        [0, 0, 0, 1]]

## 已知 T_6C
T_6C = [[0, 0, 1, 0],
       [0, -1, 0, 0],
       [1, 0, 0, 206],
       [0, 0, 0, 1]]

# T_0C = T_06 * T_6C 
T_06 = np.dot(T_0C, np.linalg.inv(T_6C))
print(T_06)

A = 1， B = -1， C = 424， D= 364， E = 20

第1题答案： 1//-1//424//364//20

只沿 X 轴 旋转 60°

import numpy as np

## 杯子 中心 C 在 点 P1 处 
## 只沿着 X 轴 转 60°
θ = np.pi * 60 / 180
R = [[1,0, 0],
     [0, np.cos(θ), -np.sin(θ)],
     [0, np.sin(θ), np.cos(θ)]]
a = np.row_stack((R,[[0, 0, 0]]))  ## 扩展 行    这样 不用 将 R 打印出来
P = np.array([[630, 304, 220, 1]])   ## 扩展 列
T_0C = np.column_stack((a, P.T))

## 已知 T_6C
T_6C = [[0, 0, 1, 0],
       [0, -1, 0, 0],
       [1, 0, 0, 206],
       [0, 0, 0, 1]]

# T_0C = T_06 * T_6C 
T_06 = np.dot(T_0C, np.linalg.inv(T_6C))

T_06 = [[float(format(x, '.3g')) for x in T_06[i]] for i in range(len(T_06))] 
print(np.array(T_06))

A = -0.866 B = 0.5 C = 424 D = 304

第2题答案： -0.866//0.5//424//304

import numpy as np
np.set_printoptions(suppress = True) 

## 杯子 中心 C 在 点 P2 处 
## 只沿着 X 轴 转 180°
θ = np.pi * 180 / 180
R = [[1, 0, 0],
     [0, np.cos(θ), -np.sin(θ)],
     [0, np.sin(θ), np.cos(θ)]]
a = np.row_stack((R,[[0, 0, 0]]))  ## 扩展 行    这样 不用 将 R 打印出来
P = np.array([[630, 220, 24, 1]])   ## 扩展 列
T_0C = np.column_stack((a, P.T))

## 已知 T_6C
T_6C = [[0, 0, 1, 0],
       [0, -1, 0, 0],
       [1, 0, 0, 206],
       [0, 0, 0, 1]]

# T_0C = T_06 * T_6C 
T_06 = np.dot(T_0C, np.linalg.inv(T_6C))

T_06 = [[float(format(x, '.3g')) for x in T_06[i]] for i in range(len(T_06))] 
print(np.array(T_06))

第3题答案： 1//-1//424//220

题4-9：

题4-5求解代码： Python

import numpy as np

## 杯子 中心 C 在 点 P0 处 
T_0C = [[1, 0, 0, 630],
        [0, 1, 0, 364],
        [0, 0, 1, 20],
        [0, 0, 0, 1]]

## 已知 T_6C
T_6C = [[0, 0, 1, 0],
       [0, -1, 0, 0],
       [1, 0, 0, 206],
       [0, 0, 0, 1]]

# T_0C = T_06 * T_6C 
T_06 = np.dot(T_0C, np.linalg.inv(T_6C))
print(T_06)

### 当 杯子中心 C 在 点 P0 

# T_06 = [[  0.   0.   1. 424.]
#     [  0.  -1.   0. 364.]
#     [  1.   0.   0.  20.]
#    [  0.   0.   0.   0.]]

########  求解 θ3  [-90, 0]

### 注意这里 的 x, y, z 是 T_06 的 
x, y, z = 424, 364, 20
################## 
import numpy as np

α2, a2, d3 = 0, 340, 0   ## θ3
α3, a3, d4 = np.pi*(-90)/180, -40, 338   ## θ4
'''
## 仅与 θ3 有关
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
#f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) -\
#        d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3)      
        
#f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + \
#       d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)
f3 = d4 * np.cos(α3)
# print(f3)
'''
# 对 i= 1 i= 2
α0, a0, d1 = 0, 0, 0   ## θ1
α1, a1, d2 = np.pi*(-90)/180, -30, 0  ## θ2

'''
## 和 θ2，θ3有关
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
# g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
#      np.sin(α1) * f3 - d2 * np.sin(α1)
## 化简
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3
# g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
#      np.cos(α1) * f3 + d2 * np.cos(α1)
## 化简
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 

'''

'''
## a1 不等于 0 
k1 = f1 
k2 = -f2 
#k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3
k3 = f1**2 + f2**2 + f3**2 + a1**2
#k4 = f3 * np.cos(α1) + d2 * np.cos(α1)
k4 = 0
'''

## 
r = x**2 + y**2 + z**2   ## 可解

'''
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3) 
f3 = d4 * np.cos(α3)
k1 = f1 
k2 = -f2
k3 = f1**2 + f2**2 + f3**2 + a1**2
k4 = 0
'''
### 解 超越方程
from sympy import *
θ3 = symbols('θ3')

f = (r-a1**2- (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2 )**2 \
     - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2 \
     - (d4 * cos(α3))**2 )**2/(4 * a1**2) \
  + z**2/(sin(α1))**2 \
  - (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2)**2 \
  - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2

# f = ((r - k3)**2)/(4*a1**2) + ((z-k4)**2)/(np.sin(α1))**2 - k1**2 - k2**2
root3 = solve([f],[θ3])
print('θ3(弧度值): ', root3)

θ3_du = [180 * root3[i][0] / np.pi for i in range(len(root3))]  ## θ3 弧度换角度
print('θ3(以度为单位): ', θ3_du )  

## 求解 θ2  [-90, 0]

# θ3结果汇总
# θ3(弧度值):  [(-3.04783066903255,), (-2.73611423041923,), (-0.641069066911731,), (-0.329352628298420,)]
# θ3(以度为单位):  [-174.627834006099, -156.767797668707, -36.7305519104320, -18.8705155730404]

# θ3 = -0.641069066911731  【角度】 -37
# [-0.558019327136135, -0.490112155159801]
# θ2(以度为单位):  [-31.9721523316306, -28.0813579787176]
    
# θ3 = -0.329352628298420
# θ2  无 符合 条件的 

# θ3的有效解   -0.641069066911731 , -0.329352628298420
θ3 = -0.641069066911731

f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) - d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)

k1 = f1 
k2 = -f2 
k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3

θ2 = symbols('θ2')

f = (k1 * cos(θ2) + k2 * sin(θ2)) * 2 * a1 + k3 - r
root2 = solve([f],[θ2])
θ2 = [root2[i][0] for i in range(len(root2))]
print(θ2)

θ2_du = [180 * root2[i][0] / np.pi for i in range(len(root2))]  ## θ3 弧度换角度
print('θ2(以度为单位): ', θ2_du ) 

小结：
θ3 = -0.641069066911731 【角度】 -37
θ2(弧度): [-0.558019327136135, -0.490112155159801]
θ2(角度): [-31.9721523316306, -28.0813579787176]

## 求解  θ1 [-90, 90]

## 结果汇总

# θ2 = -0.558019327136135
# θ1： [-0.709402669487201, 0.709402669487201]
# θ1(以度为单位):  [-40.6457789369307, 40.6457789369307]

###
# θ2 = -0.490112155159801
# θ1(弧度)：[-0.709402669487201, 0.709402669487201]
# θ1(以度为单位):  [-40.6457789369307, 40.6457789369307]


θ2 = -0.490112155159801
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3 - d2 * np.sin(α1)
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 + d2 * np.cos(α1)

θ1 = symbols('θ1')

f = g1 * cos(θ1) - g2 * sin(θ1) - x
root1 = solve([f],[θ1])
θ1 = [root1[i][0] for i in range(len(root1))]
print(θ1)

θ1_du = [180 * root1[i][0] / np.pi for i in range(len(root1))]  ## θ3 弧度换角度
print('θ1(以度为单位): ', θ1_du )  

结果小结：
θ2 = -0.558019327136135
θ1： [-0.709402669487201, 0.709402669487201]
θ1(以度为单位): [-40.6457789369307, 40.6457789369307]

θ2 = -0.490112155159801
θ1(弧度)：[-0.709402669487201, 0.709402669487201]
θ1(以度为单位): [-40.6457789369307, 40.6457789369307]

import numpy as np

#  求解  θ4 [-180, 180],   θ5 [0, 90]        θ6   [-180, 180]

##  θ1

θ =  -0.709402669487201  ##   可选 [-1.12251898466604, 1.12251898466604]
α = 0
R_01 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

##  θ2
θ = -0.490112155159801   ## 
α = np.pi * (-90)/180
R_12 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

# θ3 仅一个 解
θ = -0.641069066911731  
α = 0
R_23 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

R = np.dot(R_01, R_12)
R_03 = np.dot(R, R_23)
# print(R_03)

## 由之前 计算的 T_06   ##############  记得 修改 R  ！！！！！！！
R_06 = [[0, 0, 1],
        [0, -1, 0],
        [1, 0, 0]]

### 注意这一步处理，这里 和 PPT 里不一样
θ = np.pi * (-90)/180
R_34X = [[1, 0, 0],
        [0, np.cos(θ), -np.sin(θ)],
        [0, np.sin(θ), np.cos(θ)]]

R_36 = np.dot(np.linalg.inv(np.dot(R_03, R_34X)), R_06)  ## 需要 先将 Z3 转到 Z4 ， 才能 继续 使用 ZYZ 欧拉角  计算
# print(R_36)
r31 = R_36[2][0]
r32 = R_36[2][1]
r33 = R_36[2][2]
r23 = R_36[1][2]
r13 = R_36[0][2]
import math
β = math.atan2(math.sqrt(r31**2 + r32**2), r33)  ## 此外， 当 β 选负时，还有 一种 姿态选项， 而后续的θ4和 θ6 仅与 β的选值有关

print("解1：")
# print(β)  ## 1.1033617668479667  63
## 由PPT P25 DH定义  与 ZYZ 欧拉角度  转换关系
print('θ5：',180*β/np.pi)

# β = 1.1033617668479667
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)

γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

###
print("解2：")
β = -β  ## 另一组姿态
print('θ5：',180*β/np.pi)
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)
γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

全部结果汇总：
θ3 = -0.641069066911731 【角度】 -36.7305519104320

θ3 = -0.641069066911731 【角度】 -37
[-0.558019327136135, -0.490112155159801]
θ2(以度为单位): [-31.9721523316306, -28.0813579787176]

θ2 = -0.558019327136135 【角度】-32
θ1： [-0.709402669487201, 0.709402669487201]
θ1(以度为单位): [-40.6457789369307, 40.6457789369307]

θ2 = -0.490112155159801 【角度】-28
[-0.709402669487201, 0.709402669487201]
θ1(以度为单位): [-40.6457789369307, 40.6457789369307]

θ2	θ1	θ5[0-90]	θ4	θ6	与T_06一致
-31.9721523316306	-40.6457789369307	45.01393753545229	112.93208396493651	239.10090329561015-360	❌ P有负的
-31.9721523316306	40.6457789369307	45.01393753545229	247.0679160350635-360	120.89909670438985
-28.0813579787176	-40.6457789369307	46.63798552151713	116.36961993396041	234.16993087828823-360	❌ P有负的
-28.0813579787176	40.6457789369307	46.63798552151713	243.63038006603958-360	125.83006912171177

这两组看起来半斤八两。🤣

## 通过 T_06  再次验证 
import numpy as np
np.set_printoptions(suppress = True) 

def getT(α, a, d, θ):
    α = np.pi * α / 180
    θ = np.pi * θ / 180  ## 2和 3 不好区分， 增加精度
    T = [[np.cos(θ), -np.sin(θ), 0, a],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(θ), -np.sin(α), -np.sin(α) * d],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α) * d],
        [0, 0, 0, 1]]
    return T

T_01 = getT(0, 0, 0,   40.6457789369307)  ## θ1
T_12 = getT(-90, -30, 0,  -28.0813579787176)  ## θ2
T = np.dot(T_01, T_12)
T_23 = getT(0, 340, 0, -36.7305519104320)  ## θ3  不改 
T = np.dot(T, T_23)
T_34 = getT(-90, -40,338, 243.63038006603958-360)  ## θ4
T = np.dot(T, T_34)
T_45 = getT(90, 0, 0,   46.63798552151713) ## θ5
T = np.dot(T, T_45) 
T_56 = getT(-90, 0, 0, 125.83006912171177)  ## θ6
T_06 = np.dot(T, T_56)
print(T_06)

### 当 杯子中心 C 在 点 P0 

# T_06 = [[  0.   0.   1. 424.]
#     [  0.  -1.   0. 364.]
#     [  1.   0.   0.  20.]
#    [  0.   0.   0.   0.]]

【这两题都是在答案系统里试的。。🤣】
第4题答案： 41//-32//-37 【2选1】
第5题答案： -113//45//121 【题4定则题5定】

题6-7求解代码： Python

import numpy as np

## 杯子 中心 C 在 点 P1 处 
## 只沿着 X 轴 转 60°
θ = np.pi * 60 / 180
R = [[1,0, 0],
     [0, np.cos(θ), -np.sin(θ)],
     [0, np.sin(θ), np.cos(θ)]]
a = np.row_stack((R,[[0, 0, 0]]))  ## 扩展 行    这样 不用 将 R 打印出来
P = np.array([[630, 304, 220, 1]])   ## 扩展 列
T_0C = np.column_stack((a, P.T))

## 已知 T_6C
T_6C = [[0, 0, 1, 0],
       [0, -1, 0, 0],
       [1, 0, 0, 206],
       [0, 0, 0, 1]]

# T_0C = T_06 * T_6C 
T_06 = np.dot(T_0C, np.linalg.inv(T_6C))

T_06 = [[float(format(x, '.3g')) for x in T_06[i]] for i in range(len(T_06))] 
print(np.array(T_06))

### 当 杯子中心 C 在 点 P1

# # T_06 = [[  0.      0.      1.    424.   ]
#  [ -0.866  -0.5     0.    304.   ]
#  [  0.5    -0.866   0.    220.   ]
#  [  0.      0.      0.      1.   ]]

########  求解 θ3    [-90, 0]

### 注意这里 的 x, y, z 是 T_06 的 
x, y, z = 424, 304, 220
################## 
import numpy as np

α2, a2, d3 = 0, 340, 0   ## θ3
α3, a3, d4 = np.pi*(-90)/180, -40, 338   ## θ4
'''
## 仅与 θ3 有关
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
#f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) -\
#        d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3)      
        
#f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + \
#       d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)
f3 = d4 * np.cos(α3)
# print(f3)
'''
# 对 i= 1 i= 2
α0, a0, d1 = 0, 0, 0   ## θ1
α1, a1, d2 = np.pi*(-90)/180, -30, 0  ## θ2

'''
## 和 θ2，θ3有关
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
# g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
#      np.sin(α1) * f3 - d2 * np.sin(α1)
## 化简
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3
# g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
#      np.cos(α1) * f3 + d2 * np.cos(α1)
## 化简
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 

'''

'''
## a1 不等于 0 
k1 = f1 
k2 = -f2 
#k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3
k3 = f1**2 + f2**2 + f3**2 + a1**2
#k4 = f3 * np.cos(α1) + d2 * np.cos(α1)
k4 = 0
'''

## 
r = x**2 + y**2 + z**2   ## 可解

'''
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3) 
f3 = d4 * np.cos(α3)
k1 = f1 
k2 = -f2
k3 = f1**2 + f2**2 + f3**2 + a1**2
k4 = 0
'''
### 解 超越方程
from sympy import *
θ3 = symbols('θ3')

f = (r-a1**2- (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2 )**2 \
     - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2 \
     - (d4 * cos(α3))**2 )**2/(4 * a1**2) \
  + z**2/(sin(α1))**2 \
  - (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2)**2 \
  - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2

# f = ((r - k3)**2)/(4*a1**2) + ((z-k4)**2)/(np.sin(α1))**2 - k1**2 - k2**2
root3 = solve([f],[θ3])
print('θ3(弧度值): ', root3)

θ3_du = [180 * root3[i][0] / np.pi for i in range(len(root3))]  ## θ3 弧度换角度
print('θ3(以度为单位): ', θ3_du )  

θ3(弧度值): [(-3.00276174766702,), (-2.70746877163218,), (-0.669714525698784,), (-0.374421549663944,)]
θ3(以度为单位): [-172.045575024647, -155.126533777993, -38.3718158011460, -21.4527745544920]

## 求解 θ2  [-90, 0]

# θ3结果汇总
# θ3(弧度值):  [(-3.00276174766702,), (-2.70746877163218,), (-0.669714525698784,), (-0.374421549663944,)]
# θ3(以度为单位):  [-172.045575024647, -155.126533777993, -38.3718158011460, -21.4527745544920]

# θ3 = -0.669714525698784  【角度】 -38
# [-0.889163787914293, -0.130302262671859]
# θ2(以度为单位):  [-50.9453323433544, -7.46576971210258]
    
# θ3 = -0.374421549663944    角度  -21
# θ2  无符合 条件的 

# θ3的有效解  
θ3 = -0.374421549663944

f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) - d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)

k1 = f1 
k2 = -f2 
k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3

θ2 = symbols('θ2')

f = (k1 * cos(θ2) + k2 * sin(θ2)) * 2 * a1 + k3 - r
root2 = solve([f],[θ2])
θ2 = [root2[i][0] for i in range(len(root2))]
print(θ2)

θ2_du = [180 * root2[i][0] / np.pi for i in range(len(root2))]  ## θ3 弧度换角度
print('θ2(以度为单位): ', θ2_du )  # [-174.801389740395, -158.156748814047, -35.3416007650924, -18.6969598387445]

#θ3 = -0.669714525698784 【角度】 -38
#[-0.889163787914293, -0.130302262671859]
#θ2(以度为单位): [-50.9453323433544, -7.46576971210258]

## 求解  θ1 [-90, 90]

## 结果汇总

# θ2 = -0.889163787914293
# [-0.539863702101135, 0.539863702101135]
# θ1(以度为单位):  [-30.9319116427030, 30.9319116427030]

###
# θ2 = -0.130302262671859
# [-0.230814088745282, 0.230814088745282]
# θ1(以度为单位):  [-13.2246731372627, 13.2246731372627]


θ2 = -0.130302262671859
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3 - d2 * np.sin(α1)
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 + d2 * np.cos(α1)

θ1 = symbols('θ1')

f = g1 * cos(θ1) - g2 * sin(θ1) - x
root1 = solve([f],[θ1])
θ1 = [root1[i][0] for i in range(len(root1))]
print(θ1)

θ1_du = [180 * root1[i][0] / np.pi for i in range(len(root1))]  ## θ3 弧度换角度
print('θ1(以度为单位): ', θ1_du )  

θ2 = -0.889163787914293 -51
[-0.539863702101135, 0.539863702101135]
θ1(以度为单位): [-30.9319116427030, 30.9319116427030]

θ2 = -0.130302262671859 -7
[-0.230814088745282, 0.230814088745282]
θ1(以度为单位): [-13.2246731372627, 13.2246731372627]

求解 θ4-θ6 时， 记得 将 R 改成相应的！！！！

import numpy as np

#  求解  θ4 [-180, 180],   θ5 [0, 90]        θ6   [-180, 180]


##  θ1  

θ = 0.622032015289154
α = 0
R_01 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

##  θ2  
θ =  -0.130302262671859 ## 
α = np.pi * (-90)/180
R_12 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

# θ3 仅一个 解
θ = -0.669714525698784    #【角度】 
α = 0
R_23 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

R = np.dot(R_01, R_12)
R_03 = np.dot(R, R_23)
# print(R_03)


###########################################   记得   修改  R ！！！！
## 由之前 计算的 T_06 
R_06 = [[0, 0, 1],
 [-0.866, -0.5, 0],
 [0.5, -0.866, 0]]

### 注意这一步处理，这里 和 PPT 里不一样
θ = np.pi * (-90)/180
R_34X = [[1, 0, 0],
        [0, np.cos(θ), -np.sin(θ)],
        [0, np.sin(θ), np.cos(θ)]]

R_36 = np.dot(np.linalg.inv(np.dot(R_03, R_34X)), R_06)  ## 需要 先将 Z3 转到 Z4 ， 才能 继续 使用 ZYZ 欧拉角  计算
# print(R_36)
r31 = R_36[2][0]
r32 = R_36[2][1]
r33 = R_36[2][2]
r23 = R_36[1][2]
r13 = R_36[0][2]
import math
β = math.atan2(math.sqrt(r31**2 + r32**2), r33)  ## 此外， 当 β 选负时，还有 一种 姿态选项， 而后续的θ4和 θ6 仅与 β的选值有关

print("解1：")
# print(β)  ## 1.1033617668479667  63
## 由PPT P25 DH定义  与 ZYZ 欧拉角度  转换关系
print('θ5：',180*β/np.pi)

# β = 1.1033617668479667
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)

γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

###
print("解2：")
β = -β  ## 另一组姿态
print('θ5：',180*β/np.pi)
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)
γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

答案汇总
θ3 = -0.669714525698784 【角度】 -38.3718158011460
θ2：
[-0.889163787914293, -0.130302262671859]
θ2(以度为单位): [-50.9453323433544, -7.46576971210258]
θ1：
[-0.622032015289154, 0.622032015289154]
θ1(以度为单位): [-35.6398091980856, 35.6398091980856]

[-0.622032015289152, 0.622032015289151]
θ1(以度为单位): [-35.6398091980855, 35.6398091980855]

θ2	θ1	θ5[0-90]	θ4[-180-180]	θ6[-180-180]	与T_06一致
-50.9453323433544	-35.6398091980856	35.64488712689306	90.95228839698065	328.8274800404136-360	❌ P有负的
-50.9453323433544	35.6398091980856	35.64488712689307	269.04771160301937-360	151.17106439793167
-7.46576971210258	-35.6398091980856	54.33743693103992	134.1778531447378	270.96209986285226-360	❌ P有负的
-7.46576971210258	35.6398091980856	54.337436931039925	225.8221468552622-360	209.03644457549302-360	❌ P有负的

第6题答案：： 36//-51//-38
第7题答案：：-91//36//151

## 通过 T_06  再次验证 
import numpy as np
np.set_printoptions(suppress = True) 

def getT(α, a, d, θ):
    α = np.pi * α / 180
    θ = np.pi * θ / 180  ## 2和 3 不好区分， 增加精度
    T = [[np.cos(θ), -np.sin(θ), 0, a],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(θ), -np.sin(α), -np.sin(α) * d],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α) * d],
        [0, 0, 0, 1]]
    return T

T_01 = getT(0, 0, 0,   35.6398091980856)  ## θ1
T_12 = getT(-90, -30, 0,  -7.46576971210258)  ## θ2
T = np.dot(T_01, T_12)
T_23 = getT(0, 340, 0, -38.3718158011460)  ## θ3  不改 
T = np.dot(T, T_23)
T_34 = getT(-90, -40,338, 225.8221468552622-360)  ## θ4
T = np.dot(T, T_34)
T_45 = getT(90, 0, 0,    54.33743693103992) ## θ5
T = np.dot(T, T_45) 
T_56 = getT(-90, 0, 0, 209.03644457549302-360)  ## θ6
T_06 = np.dot(T, T_56)
print(T_06)

### 当 杯子中心 C 在 点 P1

# # T_06 = [[  0.      0.      1.    424.   ]
#  [ -0.866  -0.5     0.    304.   ]
#  [  0.5    -0.866   0.    220.   ]
#  [  0.      0.      0.      1.   ]]

题8-9求解代码： Python

import numpy as np
np.set_printoptions(suppress = True) 

## 杯子 中心 C 在 点 P2 处 
## 只沿着 X 轴 转 180°
θ = np.pi * 180 / 180
R = [[1, 0, 0],
     [0, np.cos(θ), -np.sin(θ)],
     [0, np.sin(θ), np.cos(θ)]]
a = np.row_stack((R,[[0, 0, 0]]))  ## 扩展 行    这样 不用 将 R 打印出来
P = np.array([[630, 220, 24, 1]])   ## 扩展 列
T_0C = np.column_stack((a, P.T))

## 已知 T_6C
T_6C = [[0, 0, 1, 0],
       [0, -1, 0, 0],
       [1, 0, 0, 206],
       [0, 0, 0, 1]]

# T_0C = T_06 * T_6C 
T_06 = np.dot(T_0C, np.linalg.inv(T_6C))

T_06 = [[float(format(x, '.3g')) for x in T_06[i]] for i in range(len(T_06))] 
print(np.array(T_06))

### 当 杯子中心 C 在 点 P2

# T_06 = [[  0.   0.   1. 424.]
#  [ -0.   1.   0. 220.]
#  [ -1.  -0.   0.  24.]
#  [  0.   0.   0.   1.]]

########  求解 θ3    [-90, 0]

### 注意这里 的 x, y, z 是 T_06 的 
x, y, z = 424, 220, 24
################## 
import numpy as np

α2, a2, d3 = 0, 340, 0   ## θ3
α3, a3, d4 = np.pi*(-90)/180, -40, 338   ## θ4
'''
## 仅与 θ3 有关
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
#f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) -\
#        d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3)      
        
#f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + \
#       d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)
f3 = d4 * np.cos(α3)
# print(f3)
'''
# 对 i= 1 i= 2
α0, a0, d1 = 0, 0, 0   ## θ1
α1, a1, d2 = np.pi*(-90)/180, -30, 0  ## θ2

'''
## 和 θ2，θ3有关
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
# g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
#      np.sin(α1) * f3 - d2 * np.sin(α1)
## 化简
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3
# g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
#      np.cos(α1) * f3 + d2 * np.cos(α1)
## 化简
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 

'''

'''
## a1 不等于 0 
k1 = f1 
k2 = -f2 
#k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3
k3 = f1**2 + f2**2 + f3**2 + a1**2
#k4 = f3 * np.cos(α1) + d2 * np.cos(α1)
k4 = 0
'''

## 
r = x**2 + y**2 + z**2   ## 可解

'''
f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2
f2 = a3 * np.sin(θ3) - d4 * np.sin(α3) * np.cos(θ3) 
f3 = d4 * np.cos(α3)
k1 = f1 
k2 = -f2
k3 = f1**2 + f2**2 + f3**2 + a1**2
k4 = 0
'''
### 解 超越方程
from sympy import *
θ3 = symbols('θ3')

f = (r-a1**2- (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2 )**2 \
     - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2 \
     - (d4 * cos(α3))**2 )**2/(4 * a1**2) \
  + z**2/(sin(α1))**2 \
  - (a3 * cos(θ3) + d4 * sin(α3)*sin(θ3) + a2)**2 \
  - (a3 * sin(θ3) - d4 * sin(α3) * cos(θ3))**2

# f = ((r - k3)**2)/(4*a1**2) + ((z-k4)**2)/(np.sin(α1))**2 - k1**2 - k2**2
root3 = solve([f],[θ3])
print('θ3(弧度值): ', root3)

θ3_du = [180 * root3[i][0] / np.pi for i in range(len(root3))]  ## θ3 弧度换角度
print('θ3(以度为单位): ', θ3_du )  

θ3(弧度值): -0.234149167415370
θ3(以度为单位): -13.4157590694029

## 求解 θ2  [-90, 0]

# θ3的有效解  
θ3 = -0.234149167415370

f1 = a3 * np.cos(θ3) + d4 * np.sin(α3)*np.sin(θ3) + a2 
f2 = a3 * np.cos(α2)*np.sin(θ3) - d4 * np.sin(α3)*np.cos(α2)*np.cos(θ3) - d4 * np.sin(α2)*np.cos(α3) - d3 * np.sin(α2)
f3 = a3 * np.sin(α2)* np.sin(θ3)- d4 * np.sin(α3)* np.sin(α2) * np.cos(θ3) + d4 * np.cos(α2) * np.cos(α3) + d3 * np.cos(α2)

k1 = f1 
k2 = -f2 
k3 = f1**2 + f2**2 + f3**2 + a1**2 + d2**2 + 2*d2*f3

θ2 = symbols('θ2')

f = (k1 * cos(θ2) + k2 * sin(θ2)) * 2 * a1 + k3 - r
root2 = solve([f],[θ2])
θ2 = [root2[i][0] for i in range(len(root2))]
print(θ2)

θ2_du = [180 * root2[i][0] / np.pi for i in range(len(root2))]  ## θ3 弧度换角度
print('θ2(以度为单位): ', θ2_du ) 

[-0.774929259641440, -0.680451426622777]
θ2(以度为单位): [-44.4001759986521, -38.9869949091409]

## 求解  θ1 [-90, 90]

## 结果汇总

# θ2 = -0.774929259641440
# [-0.478627761768163, 0.478627761768163]
# θ1(以度为单位):  [-27.4233507071088, 27.4233507071088]

###
# θ2 = -0.680451426622777
# [-0.478627761768163, 0.478627761768163]
# θ1(以度为单位):  [-27.4233507071088, 27.4233507071088]


θ2 = -0.680451426622777
g1 = np.cos(θ2)* f1 - np.sin(θ2) * f2 + a1
g2 = np.sin(θ2)* np.cos(α1) * f1 + np.cos(θ2) * np.cos(α1)* f2 -\
     np.sin(α1) * f3 - d2 * np.sin(α1)
g3 = np.sin(θ2)* np.sin(α1) * f1 + np.cos(θ2) * np.sin(α1)* f2 +\
     np.cos(α1) * f3 + d2 * np.cos(α1)

θ1 = symbols('θ1')

f = g1 * cos(θ1) - g2 * sin(θ1) - x
root1 = solve([f],[θ1])
θ1 = [root1[i][0] for i in range(len(root1))]
print(θ1)

θ1_du = [180 * root1[i][0] / np.pi for i in range(len(root1))]  ## θ3 弧度换角度
print('θ1(以度为单位): ', θ1_du )  

import numpy as np

#  求解  θ4 [-180, 180],   θ5 [0, 90]        θ6   [-180, 180]


##  θ1  

θ = 0.478627761768163
α = 0
R_01 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

##  θ2  
θ =  -0.680451426622777## 
α = np.pi * (-90)/180
R_12 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

# θ3 仅一个 解
θ =  -0.234149167415370    #【角度】 
α = 0
R_23 = [[np.cos(θ), -np.sin(θ), 0],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α)],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α)]]

R = np.dot(R_01, R_12)
R_03 = np.dot(R, R_23)
# print(R_03)

## 由之前 计算的 T_06                 ！！！！记得修改 R  ！！！检查
R_06 = [[0, 0, 1],
    [0, 1, 0],
    [-1, 0, 0]]

### 注意这一步处理，这里 和 PPT 里不一样
θ = np.pi * (-90)/180
R_34X = [[1, 0, 0],
        [0, np.cos(θ), -np.sin(θ)],
        [0, np.sin(θ), np.cos(θ)]]

R_36 = np.dot(np.linalg.inv(np.dot(R_03, R_34X)), R_06)  ## 需要 先将 Z3 转到 Z4 ， 才能 继续 使用 ZYZ 欧拉角  计算
# print(R_36)
r31 = R_36[2][0]
r32 = R_36[2][1]
r33 = R_36[2][2]
r23 = R_36[1][2]
r13 = R_36[0][2]
import math
β = math.atan2(math.sqrt(r31**2 + r32**2), r33)  ## 此外， 当 β 选负时，还有 一种 姿态选项， 而后续的θ4和 θ6 仅与 β的选值有关

print("解1：")
# print(β)  ## 1.1033617668479667  63
## 由PPT P25 DH定义  与 ZYZ 欧拉角度  转换关系
print('θ5：',180*β/np.pi)

# β = 1.1033617668479667
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)

γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

###
print("解2：")
β = -β  ## 另一组姿态
print('θ5：',180*β/np.pi)
α = math.atan2(r23/np.sin(β), r13/np.sin(β))
print('θ4：',180*α/np.pi + 180)
γ = math.atan2(r32/np.sin(β), -r31/np.sin(β))
print('θ6：', 180*γ/np.pi + 180)

答案汇总
θ3 = -0.234149167415370 【角度】 -13.4157590694029
[-0.774929259641440, -0.680451426622777]
θ2(以度为单位): [-44.4001759986521, -38.9869949091409]

#θ2 = -0.774929259641440
#[-0.478627761768163, 0.478627761768163]
#θ1(以度为单位): [-27.4233507071088, 27.4233507071088]

#θ2 = -0.680451426622777
#[-0.478627761768163, 0.478627761768163]
#θ1(以度为单位): [-27.4233507071088, 27.4233507071088]

θ2	θ1	θ5[0-90]	θ4[-180-180]	θ6[-180-180]	与T_06一致
-44.4001759986521	27.4233507071088	41.30243673701658	224.249565060963-360	323.8029365281974-360
-44.4001759986521	-27.4233507071088	41.30243673701657	135.750434939037	36.197063471802636	❌ P 有负的
-38.9869949091409	-27.4233507071088	45.30889772059033	139.62036985874533	30.884102139831043	❌ P 有负的
-38.9869949091409	27.4233507071088	45.30889772059034	220.37963014125467-360	329.11589786016896-360	❌ P 有负的

第8题答案： 27//-44//-13
第9题答案： -136//41//-36

## 通过 T_06  再次验证 
import numpy as np
np.set_printoptions(suppress = True) 

def getT(α, a, d, θ):
    α = np.pi * α / 180
    θ = np.pi * θ / 180  ## 2和 3 不好区分， 增加精度
    T = [[np.cos(θ), -np.sin(θ), 0, a],
       [np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(θ), -np.sin(α), -np.sin(α) * d],
       [np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α) * d],
        [0, 0, 0, 1]]
    return T

T_01 = getT(0, 0, 0,   27.4233507071088)  ## θ1
T_12 = getT(-90, -30, 0,  -44.4001759986521)  ## θ2
T = np.dot(T_01, T_12)
T_23 = getT(0, 340, 0, -13.4157590694029)  ## θ3  不改 
T = np.dot(T, T_23)
T_34 = getT(-90, -40,338, 224.249565060963-360)  ## θ4
T = np.dot(T, T_34)
T_45 = getT(90, 0, 0,    41.30243673701658	) ## θ5
T = np.dot(T, T_45) 
T_56 = getT(-90, 0, 0, 143.80293652819736)  ## θ6
T_06 = np.dot(T, T_56)
print(T_06)

### 当 杯子中心 C 在 点 P2

# T_06 = [[  0.   0.   1. 424.]
#  [ -0.   1.   0. 220.]
#  [ -1.  -0.   0.  24.]
#  [  0.   0.   0.   1.]]

题10-15 方法一 Cartesian space

• 第10题答案： 0//-21.82//72.73

• 第11题答案： 0//-22.4//-52.27

• 第12题答案： 0//-43.64//145.45

• 第13题答案： 0//-1.16//-249.99

• 第14题答案： 0//44.8//104.53

• 第15题答案： 630//336.73//110.91

求解代码【10-15】_Python

## import numpy as np
np.set_printoptions(precision=2,suppress = True) 

t0, t1, tf = 0, 3, 7
x0, x1, xf = 630, 630, 630
y0, y1, yf = 364, 304, 220
z0, z1, zf = 20, 220, 24
tk = 0.5

### 1、求 各 DOF(X, Y, θ) 在每段的速度  及 加速度

## 中间 线段 计算
def getV_in(x1, x2, t1, t2):
    return (x2 - x1)/(t2 - t1)

## 头尾 线段  计算 
def getV_0f(x1, x2, t1, t2):
    return (x2 - x1)/(t2 - t1 - tk/2)

### 2、 建立 各 DOF(X, Y, θ) 在每段的方程
"""
平滑  t ∈ [0, 0.5]
直线  t ∈ [0.5, 2.75]
平滑  t ∈ [2.75, 3.25]
直线  t ∈ [3.25, 6.5]
平滑  t ∈ [6.5, 7]
"""
## 求解 X(t)
##  平滑化  段  
def getX_parabolic(x, V, a, ti0, ti1, t):
    return x + V * (t - ti0) + 0.5 * a * (t - ti1)**2

## 直线段  
def getX_linear(x, V, ti, t):
    return x + V * (t - ti)


############# 求解 X 部分
print('X:')
V0 = 0
V1 = getV_0f(x0, x1, t0, t1)
V2 = getV_0f(x1, xf, t1, tf)   ## 这里 也属于 端点了
Vf = 0
print('V1[0.5 ~ 2.75]:', np.round(V1, 2))
print('V2[3.25 ~ 6.5]:', np.round(V2, 2))

def geta(V1, V2):
    return (V2 - V1)/tk

a0 = geta(V0, V1)
a1 = geta(V1, V2)
af = geta(V2, Vf)
print('a0:', np.round(a0, 2))
print('a1:', np.round(a1, 2))
print('af:', np.round(af, 2))

## 属于 直线部分
print('t ∈ [0.5, 2.75]  t = 1.5 , X2 = :', np.round(getX_linear(x0, V1, 0.25, 1.5), 2))
print('t ∈ [3.25, 6.5]  t = 5 , X4 = :', np.round(getX_linear(x1, V2, 3, 5), 2))

## 平滑部分  
print('t ∈ [2.75, 3.25]  t = 3 , X3 = :', np.round(getX_parabolic(x0, V1, a1, 0.25, 2.75,3), 2))

############# 求解 Y 部分
print('Y:')
V0 = 0
V1 = getV_0f(y0, y1, t0, t1)
V2 = getV_0f(y1, yf, t1, tf)
Vf = 0
print('V1[0.5 ~ 2.75]:', np.round(V1, 2))
print('V2[3.25 ~ 6.5]:', np.round(V2, 2))

def geta(V1, V2):
    return (V2 - V1)/tk

a0 = geta(V0, V1)
a1 = geta(V1, V2)
af = geta(V2, Vf)
print('a0:', np.round(a0, 2))
print('a1:', np.round(a1, 2))
print('af:', np.round(af, 2))

###  
print('t ∈ [0.5, 2.75]  t = 1.5 , Y2 = :', np.round(getX_linear(y0, V1, 0.25, 1.5), 2))
print('t ∈ [3.25, 6.5]  t = 5 , Y4 = :', np.round(getX_linear(y1, V2, 3, 5), 2))

## 平滑部分  
print('t ∈ [2.75, 3.25]  t = 3 , Y3 = :', np.round(getX_parabolic(y0, V1, a1, 0.25, 2.75,3), 2))

############# 求解 θ 部分
print('Z:')
V0 = 0
V1 = getV_0f(z0, z1, t0, t1)
V2 = getV_0f(z1, zf, t1, tf)
Vf = 0
print('V1[0.5 ~ 2.75]:', np.round(V1, 2))
print('V2[3.25 ~ 6.5]:', np.round(V2, 2))

def geta(V1, V2):
    return (V2 - V1)/tk

a0 = geta(V0, V1)
a1 = geta(V1, V2)
af = geta(V2, Vf)
print('a0:', np.round(a0, 2))
print('a1:', np.round(a1, 2))
print('af:', np.round(af, 2))

print('t ∈ [0.5, 2.75]  t = 1.5 , Z2 = :', np.round(getX_linear(z0, V1, 0.25, 1.5), 2))
print('t ∈ [3.25, 6.5]  t = 5 , Z4 = :', np.round(getX_linear(z1, V2, 3, 5), 2))

## 平滑部分  
print('t ∈ [2.75, 3.25]  t = 3 , Z3 = :', np.round(getX_parabolic(z0, V1, a1, 0.25, 2.75,3), 2))

其它题：

630//259.2//115.47

630//303.96//212.19