代码如下：
import numpy as np
import matplotlib.pyplot as plt
import seaborn as sns
import plotly.express as px
import plotly.graph_objects as go
from plotly.subplots import make_subplots
from numpy import sin
from numpy import cos
plt.rcParams["font.sans-serif"]=["SimHei"] #设置字体
plt.rcParams["axes.unicode_minus"]=False #该语句解决图像中的“-”负号的乱码问题
sns.set_style('whitegrid', {'font.sans-serif': ['simhei', 'Arial']})
sns.set(rc={"axes.facecolor":"#FFF9ED","figure.facecolor":"#FFF9ED", 'font.sans-serif': ['simhei', 'Arial']}) # 对整个文件配置生效，不单单对一个图形！
# 将角度值数据转换为弧度制
def d_to_r(degree):
"""
将角度值数据转换为弧度制 公式：弧度= (角度×π)/180
:param degree:
:return:
"""
radian = degree * np.pi / 180 # 将角度转换为弧度
return radian
def abtain_J_orig(L1, L2, L3, theta_1, theta_2, theta_3):
"""
根据提供的参数求初始时（即第一次循环）的雅可比矩阵
abtain_theta_der(4, 3, 2, 10, 20, 30)
:return:J, theta_old
"""
# 将输入的角度值的theta_1, theta_2, theta_3转化为弧度制
theta_1 = d_to_r(theta_1)
theta_2 = d_to_r(theta_2)
theta_3 = d_to_r(theta_3)
# 定义雅可比矩阵 shape:3x3
J = np.array(
[[-L1*sin(theta_1)-L2*sin(theta_1+theta_2)-L3*sin(theta_1+theta_2+theta_3), -L2*sin(theta_1+theta_2)-L3*sin(theta_1+theta_2+theta_3), -L3*sin(theta_1+theta_2+theta_3)],
[L1*cos(theta_1)+L2*cos(theta_1+theta_2)+L3*cos(theta_1+theta_2+theta_3), L2*cos(theta_1+theta_2)+L3*cos(theta_1+theta_2+theta_3), L3*cos(theta_1+theta_2+theta_3)],
[1, 1, 1]])
# 定义theta_old
theta_old = np.array([theta_1, theta_2, theta_3]).T
return J, theta_old
def abtain_J(L1, L2, L3, theta_array):
"""
根据提供的参数求第2次到第n次循环时的雅可比矩阵
abtain_theta_der(4, 3, 2, 10, 20, 30)
:return:J, theta_old
"""
# 把theta_array拆包为theta_1, theta_2, theta_3
theta_1 = theta_array[0]
theta_2 = theta_array[1]
theta_3 = theta_array[2]
# 定义雅可比矩阵 shape:3x3
J = np.array(
[[-L1*sin(theta_1)-L2*sin(theta_1+theta_2)-L3*sin(theta_1+theta_2+theta_3), -L2*sin(theta_1+theta_2)-L3*sin(theta_1+theta_2+theta_3), -L3*sin(theta_1+theta_2+theta_3)],
[L1*cos(theta_1)+L2*cos(theta_1+theta_2)+L3*cos(theta_1+theta_2+theta_3), L2*cos(theta_1+theta_2)+L3*cos(theta_1+theta_2+theta_3), L3*cos(theta_1+theta_2+theta_3)],
[1, 1, 1]])
# 定义theta_old
theta_old = np.array([theta_1, theta_2, theta_3]).T
return J, theta_old
def abtain_theta_der(J):
"""
根据提供的参数求关节速率theta_der
abtain_theta_der(4, 3, 2, 10, 20, 30)
:return:
"""
# 定义笛卡尔速度Cartesian velocity
Car_vel = np.array([0.2, -0.3, -0.2]).T
# 求雅可比矩阵的逆矩阵
J_inv = np.linalg.inv(J)
# 求关节速率 theta_derivative 即theta求导
theta_der = np.dot(J_inv, Car_vel) # 执行矩阵相乘
# print("theta_der:", theta_der)
# 返回关节速率theta_der
return theta_der
def abtain_theta_new(theta_old, theta_der):
"""
计算求出theta_new
:param theta_old:
:param theta_der:
:return:
"""
# 定义delta_t的值 "delta"（希腊字母 Δ）
delta_t = 0.1
# 计算新的theta
theta_new = theta_old + theta_der*delta_t
# print("theta_new:",theta_new)
return theta_new
def run():
"""
执行
:return:
"""
# 定义循环次数
loop_num = 50
# 定义第一次循环时的参数
L1 = 4
L2 = 3
L3 = 2
theta1_orig = 10
theta2_orig = 20
theta3_orig = 30
# 求第一次循环时的雅可比矩阵J，并返回theta_old
J, theta_old = abtain_J_orig(L1, L2, L3, theta1_orig, theta2_orig, theta3_orig)
print("初始时的J（雅可比矩阵）：\n", J)
# 定义所有关节角数组
theta_all = theta_old.reshape((1,3))
# 求第一次循环时的关节速率theta_der
theta_der = abtain_theta_der(J)
# 定义所有关节速率数组
theta_der_all = theta_der.reshape((1,3))
# 求第一次循环时的theta_new
theta_new = abtain_theta_new(theta_old, theta_der)
# 循环第2次到第n次
for i in range(loop_num-1): # 循环次数需减去初始时的一次计算
J, theta_old = abtain_J(L1, L2, L3, theta_new)
print(f"执行了{i+1}次循环后的J（雅可比矩阵）：\n", J)
theta_der = abtain_theta_der(J)
# 把关节角数组拼接到所有关节角数组
theta_all = np.concatenate((theta_all, theta_new.reshape((1, 3))))
theta_new = abtain_theta_new(theta_old, theta_der)
# 将关节速率数组拼接到所有关节速率数组
theta_der_all = np.concatenate((theta_der_all, theta_der.reshape((1,3))))
print(theta_der_all)
return theta_der_all,theta_all
def plot():
"""
画图
画图可用三个库实现：matplotlib、seaborn、plotly。
用哪个就需要安装对应的库。
:return:
"""
# 拆包run函数返回的两个数组
theta_der_all, theta_all = run()
# print("打印输入50个周期内的关节速率：\n", theta_der_all)
# print(theta_der_all.shape)
# 从theta_der_all数组中取出单独的每一列，即每个主动关节速率数据
theta_der_1 = theta_der_all[:,0]
theta_der_2 = theta_der_all[:,1]
theta_der_3 = theta_der_all[:,2]
# 从theta_all数组中取出单独的每一列，即每个主动关节角数据
theta_1 = theta_all[:,0]
theta_2 = theta_all[:,1]
theta_3 = theta_all[:,2]
# 时间数组
time_array = np.arange(0,5,0.1)
# 创建画布（容器层）
figure, axes = plt.subplots(nrows=1, ncols=2,figsize=(8,4))
# 创建折线图:theta_der
axes[0].plot(time_array, theta_der_1, label='theta_der_1', color='blue')
axes[0].plot(time_array, theta_der_2, label='theta_der_2', color='green')
axes[0].plot(time_array, theta_der_3, label='theta_der_3', color='red')
# 设置图形标题和轴标签
axes[0].set_title('3个主动关节速率与时间的关系')
axes[0].set_xlabel("时间")
axes[0].set_ylabel("关节速率")
# 修改x刻度 刻度有三个要素：一是起始值，二是末尾值，三是步长
x_ticks = np.arange(0,5.5,0.5)
axes[0].set_xticks(x_ticks, fontsize=2) # 面向对象设置刻度说明需要另外set_xticklabels()函数
# 显示图例
axes[0].legend()
# 创建折线图:theta
axes[1].plot(time_array, theta_1, label='theta_1', color='blue')
axes[1].plot(time_array, theta_2, label='theta_2', color='green')
axes[1].plot(time_array, theta_3, label='theta_3', color='red')
# 设置图形标题和轴标签
axes[1].set_title('3个主动关节角与时间的关系')
axes[1].set_xlabel("时间")
axes[1].set_ylabel("关节角")
# 修改x刻度 刻度有三个要素：一是起始值，二是末尾值，三是步长
x_ticks = np.arange(0,5.5,0.5)
axes[1].set_xticks(x_ticks) # 面向对象设置刻度说明需要另外set_xticklabels()函数
# 显示图例
axes[0].legend()
axes[1].legend()
# 保存图形
plt.savefig('result.png')
# 显示图形
plt.show()
# # 用seaborn绘图
# # 创建画布（容器层）
# figure, axes = plt.subplots(nrows=1, ncols=2, figsize=(8, 4))
#
# # 创建折线图: theta_der
# sns.lineplot(x=time_array, y=theta_der_1, label='theta_der_1', ax=axes[0], color='blue')
# sns.lineplot(x=time_array, y=theta_der_2, label='theta_der_2', ax=axes[0], color='green')
# sns.lineplot(x=time_array, y=theta_der_3, label='theta_der_3', ax=axes[0], color='red')
#
# # 设置图形标题和轴标签
# axes[0].set_title('3个主动关节速率与时间的关系')
# axes[0].set_xlabel("时间")
# axes[0].set_ylabel("关节速率")
#
# # 修改x刻度
# x_ticks = np.arange(0, 5.5, 0.5)
# axes[0].set_xticks(x_ticks)
#
# # 显示图例
# axes[0].legend()
#
# # 创建折线图: theta
# sns.lineplot(x=time_array, y=theta_1, label='theta_1', ax=axes[1], color='blue')
# sns.lineplot(x=time_array, y=theta_2, label='theta_2', ax=axes[1], color='green')
# sns.lineplot(x=time_array, y=theta_3, label='theta_3', ax=axes[1], color='red')
#
# # 设置图形标题和轴标签
# axes[1].set_title('3个主动关节角与时间的关系')
# axes[1].set_xlabel("时间")
# axes[1].set_ylabel("关节角")
#
# # 修改x刻度
# axes[1].set_xticks(x_ticks)
#
# # 显示图例
# axes[1].legend()
#
# # 保存图形
# plt.savefig('result.png')
#
# # 显示图形
# plt.show()
# # 用px绘制图形
# # 创建子图
# fig = make_subplots(rows=1, cols=2, subplot_titles=("3个主动关节速率与时间的关系", "3个主动关节角与时间的关系")) # subplot_titles这是单个子图的标题
#
# # 添加第一个子图
# trace1_1 = go.Scatter(x=time_array, y=theta_der_1, mode='lines', name="theta_der_1")
# trace1_2 = go.Scatter(x=time_array, y=theta_der_2, mode='lines', name="theta_der_2")
# trace1_3 = go.Scatter(x=time_array, y=theta_der_3, mode='lines', name="theta_der_3")
# fig.add_trace(trace1_1, row=1, col=1)
# fig.add_trace(trace1_2, row=1, col=1)
# fig.add_trace(trace1_3, row=1, col=1)
#
# # 添加第二个子图
# trace2_1 = go.Scatter(x=time_array, y=theta_1, mode='lines', name="theta_1")
# trace2_2 = go.Scatter(x=time_array, y=theta_2, mode='lines', name="theta_2")
# trace2_3 = go.Scatter(x=time_array, y=theta_3, mode='lines', name="theta_3")
# fig.add_trace(trace2_1, row=1, col=2)
# fig.add_trace(trace2_2, row=1, col=2)
# fig.add_trace(trace2_3, row=1, col=2)
#
# # 更新图表布局
# fig.update_layout(
# title="可视化", # 这是整个图幅的标题
# title_x=0.5, # 设置标题的水平位置为中央
# width=1000, # 设置图幅宽度
# height=600, # 设置图幅高度
# xaxis=dict(title="时间"),
# yaxis=dict(title="关节速率"),
# xaxis2=dict(title="时间"),
# yaxis2=dict(title="关节角"),
# )
#
# # 设置横坐标刻度值
# fig.update_xaxes(tickvals=np.arange(0,5.5,0.5), row=1, col=1)
# fig.update_xaxes(tickvals=np.arange(0,5.5,0.5), row=1, col=2)
#
# # 展示图像
# fig.show()
if __name__ == '__main__':
plot()
绘制图像如下：
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
