import numpy as np
import matplotlib.pyplot as plt
plt.figure(1)
plt.ion()
# Input the desired position and rotation of he platform
p = np.array([[3.0], [2.5]])
R = np.array([[1.0, 0.0], [0.0, 1.0]])
# Design variables
a1 = np.array([[2.0], [1.0]])
a2 = np.array([[4.0], [1.0]])
b1 = np.array([[-0.5], [0]])
b2 = np.array([[0.5], [0]])
i = 0
while i < 50:
# Calculate the outputs
plt.clf()
s1 = p+np.dot(R, b1)-a1
s2 = p+np.dot(R, b2)-a2
Length_sl = np.sqrt(s1[0]**2+s1[1]**2)
Length_s2 = np.sqrt(s2[0]**2+s2[1]**2)
print("Length s1 = ", Length_sl, "Length s2 = ", Length_s2)
plt.plot([a1[0], s1[0]+a1[0], p[0], s2[0]+a2[0], a2[0]], [a1[1], s1[1]+a1[1], p[1], s2[1]+a2[1], a2[1]])
plt.draw()
plt.pause(0.01)
i = i+1
p[0] = p[0]+0.01
plt.ioff()
plt.show()
高精度六轴平台(Hexapod platform)逆运动学分析(三)部分运动仿真
最新推荐文章于 2023-03-12 18:38:14 发布