代码如下：

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 radiandef 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:3x3J = 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_oldtheta_old = np.array([theta_1, theta_2, theta_3]).Treturn J, theta_olddef 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_3theta_1 = theta_array[0]theta_2 = theta_array[1]theta_3 = theta_array[2]# 定义雅可比矩阵   shape:3x3J = 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_oldtheta_old = np.array([theta_1, theta_2, theta_3]).Treturn J, theta_olddef abtain_theta_der(J):"""根据提供的参数求关节速率theta_derabtain_theta_der(4, 3, 2, 10, 20, 30):return:"""# 定义笛卡尔速度Cartesian velocityCar_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_derreturn theta_derdef abtain_theta_new(theta_old, theta_der):"""计算求出theta_new:param theta_old::param theta_der::return:"""# 定义delta_t的值   "delta"（希腊字母 Δ）delta_t = 0.1# 计算新的thetatheta_new = theta_old + theta_der*delta_t# print("theta_new:",theta_new)return theta_newdef run():"""执行:return:"""# 定义循环次数loop_num = 50# 定义第一次循环时的参数L1 = 4L2 = 3L3 = 2theta1_orig = 10theta2_orig = 20theta3_orig = 30# 求第一次循环时的雅可比矩阵J，并返回theta_oldJ, 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_dertheta_der = abtain_theta_der(J)# 定义所有关节速率数组theta_der_all = theta_der.reshape((1,3))# 求第一次循环时的theta_newtheta_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_alldef 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_deraxes[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()# 创建折线图:thetaaxes[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()

绘制图像如下：