《Python机器人基础原理》第二十篇：机械臂仿真——重载抓取仿真，在末端执行器附加工件质量，对比空载与负载状态下关节力矩差异
1. 实际工业应用场景
周三下午，某重型锻造车间的自动化上下料工位。一台3-DOF垂直关节机械臂（基座偏航+肘部俯仰+末端旋转）正在执行热锻件从传送带到液压机的转运任务。机器人构型：大臂长800 mm，小臂长600 mm，末端旋转轴位于小臂末端。连杆质量：大臂8.5 kg（质心距基座0.35 m），小臂5.2 kg（质心距肘部0.28 m），末端夹具2.3 kg（质心距肘部0.15 m，质心到旋转轴线横向偏置0.02 m）。关节额定扭矩：基座偏航轴80 N·m，肘部俯仰轴300 N·m，末端旋转轴45 N·m。
调试工程师小王盯着HMI屏幕上的力矩监控画面，额头上渗出汗珠。
"师傅，这段轨迹在空载调试时Joint2峰值才62 N·m，加上这个45 kg的锻件后仿真里直接飙到278 N·m——离额定300 N·m只剩7%余量。我是不是该把加速度再降一降？"
老周放下手里的工艺卡："你先别急着改参数。你现在的仿真模型里，负载质量加在哪了？质心怎么算的？"
"加在末端执行器上。夹具2.3 kg @ 0.15 m，锻件45 kg @ 0.75 m，组合质心我按加权平均算的：(2.3×0.15 + 45×0.75) / 47.3 ≈ 0.721 m。"
"对，加权平均是对的。那M₂₂你算过吗？空载时M₂₂ = m₂·com₂² + m₃empty·com₃empty² = 5.2×0.28² + 2.3×0.15² ≈ 0.46 kg·m²。加载后m₃变成47.3 kg，com₃变成0.721 m，M₂₂ ≈ 47.3×0.721² ≈ 24.6 kg·m²——涨了53倍。你再算重力项：θ₂=-45°时cos(-45°)=0.707，Δτ_G2 ≈ g·m_load·com₃_combined·cos(-45°) ≈ 9.81×45×0.721×0.707 ≈ 224 N·m。加上惯性项……"
小王在草稿纸上快速列式子："θ̈₂峰值大约0.55 rad/s²，τ_M2 ≈ 24.6×0.55 ≈ 13.5 N·m。科氏项h从空载1.9涨到约28 kg·m²，但速度低，τ_C2峰值也就个位数。加总：重力约224 + 惯性约14 + 科氏约3 + 空载原有约35 ≈ 276 N·m。跟仿真输出的278 N·m对得上。"
"好。那你看——重力项占了总力矩的80%以上。重载抓取的主要负担是静态重力，不是动态加速。如果你把轨迹分两段——先低速移到目标上方，再缓慢下探——重力项不变但惯性项可以大幅压缩。"
小王把duration从3秒改到4.5秒重新跑："Joint2峰值降到197 N·m，安全系数1.52。够了。"
本篇解决的问题：在动力学求解器中，通过修改末端执行器的质量和质心位置参数（加权平均），模拟空载与重载两种工况。给定相同的关节状态序列，分别计算全套力矩τ = M(q)q¨ + C(q,q˙)q˙ + G(q)，对比惯性项、重力项、合力矩的差异。用于重载抓取前的离线力矩校核、轨迹可行性评估。
⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于重载抓取工况下的关节力矩计算与可视化。不可直接用于真实机器人控制器、伺服驱动器或锻造产线设备。真实工业现场需结合负载称重传感器、实时力矩监控、安全PLC联锁。代码作者不对直接使用本代码于真实设备造成的任何后果负责。
2. 痛点分析
课程模块 本篇对应的工程痛点
第4章 机器人动力学建模 → 4.4 关节空间动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q) 负载质量与质心变化同时影响M、G两项，需定量分离惯性增量与重力增量
第5章 机器人关节驱动与控制 → 5.2 伺服驱动选型 重载工况下关节扭矩需求可能接近额定值，需离线校核安全系数
第6章 轨迹规划与控制 → 6.1 轨迹约束 负载增大时需在力矩约束下重新规划加速度曲线
第7章 机器人非线性控制 → 7.1 计算力矩控制 负载变化导致模型参数漂移，前馈补偿需在线更新
本篇代码解决 L2 离线动力学校核层。输入：机器人参数 + 空载/负载质量属性 + 同一组关节状态序列。输出：两种工况下的τ_M、τ_C、τ_G、τ_total对比。代码不是设备实时控制器，不做伺服闭环。它做的事情是：给定相同轨迹，对比有无负载时的力矩差异。
不覆盖：实时负载辨识、自适应控制、碰撞检测、多轴联动插补、6轴完整动力学。
3. 核心逻辑讲解
物理直觉
你有没有试过空手甩绳子和甩系着重物的绳子？空手时几乎感觉不到阻力，但系上一块砖头后，不仅启动时要花大力气（惯性），就算匀速转，你也得一直用力拉着不让它掉下去（重力）。机械臂抓取重物也一样——负载同时增加"启动难度"（惯性项M变大）和"持续负担"（重力项G变大）。
关键洞察：负载加在末端，对基座偏航轴的影响主要通过耦合项M₁₂间接传递；对肘部俯仰轴则是直接影响——重力矩 ∝ m_load·com₃·cosθ₂，惯性矩 ∝ m₃·com₃²。质心越远，影响越大，而且是非线性的（二次方）。
业务逻辑 → 代码映射
输入：机器人参数 + 空载质量属性 / 负载质量属性（从空载配置+负载参数生成） + 同一组关节状态序列
↓
步骤1：用空载参数创建DynamicsSolver，计算全套力矩（M, C, G, total）
↓
步骤2：从空载配置出发，叠加负载质量和质心，创建新MassPropertySet，再创建新DynamicsSolver
↓
步骤3：计算全套力矩，逐项对比——Δτ_M, Δτ_C, Δτ_G, Δτ_total
↓
步骤4：统计峰值，计算安全系数（额定扭矩 / 负载峰值）
↓
步骤5：输出4张对比图表
关键公式文字翻译
重力项增量 Δτ_G：主要来自肘部俯仰轴。加载后m₃从2.3 kg变为47.3 kg，com₃从0.15 m变为0.721 m（加权平均）。增量≈ g·m_load·com₃_combined·cosθ₂ = 9.81×45×0.721×cos(-45°) ≈ 224 N·m。这是负载力矩的主要来源。
惯性项增量 Δτ_M：主要来自M₂₂ = m₂com₂² + m₃com₃²。空载M₂₂ ≈ 0.46 kg·m²；加载后M₂₂ ≈ 24.6 kg·m²——涨了53倍。同样θ̈₂≈0.55 rad/s²，τ_M2从约0.25 N·m涨到约13.5 N·m。
科氏项增量 Δτ_C：耦合系数h = m₂L₁com₂ + m₃L₁com₃。空载h≈1.9 kg·m²，加载后h≈28.4 kg·m²（涨了15倍）。但本篇轨迹速度较低，科氏力矩峰值仍在个位数N·m——重申第十五篇的结论：重载低速时科氏项可忽略，惯性+重力才是主项。
末端旋转轴：质点模型下I₃ = m₃·r_offset²，r_offset统一为0.02 m。负载下I₃ ≈ 47.3×0.02² ≈ 0.019 kg·m²，τ_M3峰值约0.01 N·m——图①第三个子图基本是平线，这是正确物理结果（末端旋转轴轻载），不是画图错误。
单位：力矩 N·m，质量 kg，长度 m，角度 rad。
4. OOP代码实现
技术栈
严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。
项目结构树
heavy_load_sim/
├── run_heavy_load_sim.py
├── requirements.txt
├── dynamics/
│ ├── __init__.py
│ ├── models.py
│ ├── trajectory.py
│ ├── dynamics_solver.py
│ └── visualize.py
├── tests/
│ ├── __init__.py
│ └── test_heavy_load_sim.py
└── results/
核心源码
"dynamics/models.py" — dataclass数据模型
"""
3-DOF垂直关节机械臂数据模型
重载抓取仿真：支持空载/负载两种质量属性配置
构型说明：基座偏航(θ1, 竖直轴) + 肘部俯仰(θ2, 水平轴) + 末端旋转(θ3)
物理声明：
- θ1绕竖直轴旋转，重力力矩恒为零
- θ2为水平俯仰轴，重力矩 = -g·(Σm_i·com_i)·cos(θ₂)
- 负载通过从空载配置叠加质量和质心实现，质心用加权平均
- 末端旋转惯量 I₃ = m₃·r_offset²，r_offset统一为0.02 m
- gravity_enabled控制重力项开关；g为物理输入，仅在enabled=True时使用
"""
from dataclasses import dataclass, field
from typing import List, Tuple, Optional
import numpy as np
@dataclass
class JointLimit:
"""单关节硬件限位"""
min_val: float
max_val: float
joint_name: str = ""
torque_limit: float = 0.0
def __post_init__(self):
if self.min_val >= self.max_val:
raise ValueError(
f"JointLimit: min_val({self.min_val})必须严格小于"
f"max_val({self.max_val})")
if not np.isfinite(self.min_val) or not np.isfinite(self.max_val):
raise ValueError("JointLimit: 限位值必须为有限数值")
def contains(self, value: float) -> bool:
return (self.min_val - 1e-10) <= value <= (self.max_val + 1e-10)
@dataclass
class JointLimitSet:
"""3-DOF机械臂关节限位集合"""
limits: List[JointLimit]
def __post_init__(self):
if len(self.limits) != 3:
raise ValueError(f"需要3个关节限位, 实际{len(self.limits)}个")
@classmethod
def default_3dof_arm(cls):
"""
默认3-DOF机械臂限位
θ1: ±170° (基座偏航, 额定80 N·m)
θ2: ±140° (肘部俯仰, 额定300 N·m)
θ3: ±180° (末端旋转, 额定45 N·m)
"""
pi = 3.141592653589793
return cls(limits=[
JointLimit(-170 * pi / 180, 170 * pi / 180, 'joint1_base_yaw', 80.0),
JointLimit(-140 * pi / 180, 140 * pi / 180, 'joint2_elbow_pitch', 300.0),
JointLimit(-180 * pi / 180, 180 * pi / 180, 'joint3_end_rotate', 45.0),
])
def check_all_with_indices(self, joint_values: List[float]) -> Tuple[bool, List[int]]:
if len(joint_values) != 3:
raise ValueError(f"需要3个关节值, 实际{len(joint_values)}个")
all_pass = True
violation_indices = []
for i, (val, limit) in enumerate(zip(joint_values, self.limits)):
if not limit.contains(val):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
def get_torque_limits(self) -> List[float]:
return [lim.torque_limit for lim in self.limits]
@dataclass
class LinkMassProps:
"""
连杆质量与质心属性
mass: 质量 (kg)
com_x: 质心沿连杆方向距近端关节的距离 (m)
com_y, com_z: 垂直于连杆方向的质心偏移 (m)
r_offset_end: 末端质心到旋转轴线的横向距离 (m)，用于I₃计算
"""
mass: float
com_x: float
com_y: float
com_z: float
link_name: str = ""
max_com_x: Optional[float] = None
r_offset_end: float = 0.02 # 统一默认值0.02 m
def __post_init__(self):
if self.mass < 0:
raise ValueError(f"LinkMassProps: 质量不能为负, 实际{self.mass}")
for name, val in [('com_x', self.com_x), ('com_y', self.com_y),
('com_z', self.com_z), ('r_offset_end', self.r_offset_end)]:
if not np.isfinite(val):
raise ValueError(f"LinkMassProps.{name}必须为有限数值")
if self.max_com_x is not None and self.com_x > self.max_com_x:
raise ValueError(
f"LinkMassProps({self.link_name}): 质心x={self.com_x}m "
f"超出连杆长度上限{self.max_com_x}m")
@dataclass
class MassPropertySet:
"""完整连杆质量属性集合"""
link_props: List[LinkMassProps]
link_lengths: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0])
def __post_init__(self):
if len(self.link_props) != 3:
raise ValueError(f"需要3个连杆质量属性, 实际{len(self.link_props)}个")
for i, (prop, length) in enumerate(zip(self.link_props, self.link_lengths)):
if length > 0 and prop.com_x > length:
raise ValueError(
f"连杆{i}({prop.link_name}): 质心{prop.com_x}m超出长度{length}m")
@classmethod
def empty_end_effector(cls):
"""
空载配置：末端仅含夹具，无工件
大臂: 8.5 kg, com=0.35 m (L1=0.80m)
小臂: 5.2 kg, com=0.28 m (L2=0.60m)
末端夹具: 2.3 kg, com=0.15 m, r_offset=0.02 m
"""
link_lengths = [0.80, 0.60, 0.0]
props = [
LinkMassProps(8.5, 0.35, 0.0, 0.0, 'link1_upper_arm', link_lengths[0]),
LinkMassProps(5.2, 0.28, 0.0, 0.0, 'link2_forearm', link_lengths[1]),
LinkMassProps(2.3, 0.15, 0.0, 0.0, 'link3_end_effector_empty', None, 0.02),
]
return cls(link_props=props, link_lengths=link_lengths)
@classmethod
def with_load(cls, base_props: 'MassPropertySet',
load_mass: float, load_com_x: float,
load_r_offset: float = 0.02):
"""
从空载配置出发，叠加负载生成重载配置
质心按质量加权平均：(m_empty·com_empty + m_load·com_load) / (m_empty + m_load)
这是本篇的核心物理修正：负载质心不能简单替换，必须加权平均
"""
if load_mass < 0:
raise ValueError(f"负载质量不能为负, 实际{load_mass}")
new_props = []
for i, prop in enumerate(base_props.link_props):
if i == 2:
# 末端连杆：合并夹具+负载
total_mass = prop.mass + load_mass
combined_com = (prop.mass * prop.com_x + load_mass * load_com_x) / total_mass
new_props.append(LinkMassProps(
total_mass, combined_com, prop.com_y, prop.com_z,
f'link3_with_{load_mass}kg_load', prop.max_com_x,
load_r_offset # 统一使用传入的r_offset
))
else:
new_props.append(LinkMassProps(
prop.mass, prop.com_x, prop.com_y, prop.com_z,
prop.link_name, prop.max_com_x, prop.r_offset_end
))
return cls(link_props=new_props, link_lengths=base_props.link_lengths)
def total_mass(self) -> float:
return sum(prop.mass for prop in self.link_props)
@dataclass
class DHParam:
"""标准D-H参数"""
theta: float
d: float
a: float
alpha: float
def __post_init__(self):
for name, val in [('theta', self.theta), ('d', self.d),
('a', self.a), ('alpha', self.alpha)]:
if not np.isfinite(val):
raise ValueError(f"DHParam.{name}必须为有限数值")
@dataclass
class RobotParams:
"""
3-DOF机械臂完整参数
大臂800mm, 小臂600mm
gravity_enabled: 重力项计算开关。False时G(q)返回全零。
g: 重力加速度值。仅在gravity_enabled=True时参与计算。
"""
links: List[DHParam]
joint_names: List[str] = field(default_factory=lambda: [
'joint1_base_yaw', 'joint2_elbow_pitch', 'joint3_end_rotate'
])
g: float = 9.81
gravity_enabled: bool = True
@classmethod
def default_3dof_arm(cls):
links = [
DHParam(0.0, 0.0, 0.800, 0.0),
DHParam(0.0, 0.0, 0.600, 0.0),
DHParam(0.0, 0.0, 0.0, 0.0),
]
return cls(links=links)
def zero_gravity(self):
"""关闭重力：返回新副本，gravity_enabled=False, g=0"""
new_links = [DHParam(l.theta, l.d, l.a, l.alpha) for l in self.links]
return RobotParams(links=new_links, joint_names=self.joint_names,
g=0.0, gravity_enabled=False)
"dynamics/trajectory.py" — 关节状态序列生成器
"""
关节状态序列生成器：重载抓取仿真用轨迹
轨迹设计：从传送带取件位置→液压机工位
θ1: 0° → 90°（水平转）
θ2: 0° → -45°（下探取件，cos(-45°)=0.707）
θ3: 0° → 180°（末端翻转）
"""
import numpy as np
from dynamics.models import JointLimitSet
class JointStateSequence:
"""
关节状态序列：存储多组(q, q˙, q¨)用于批量力矩计算
"""
def __init__(self, times, q_array, q_dot_array, q_ddot_array,
limits: JointLimitSet = None):
self.times = np.array(times, dtype=float)
self.q_array = np.array(q_array, dtype=float)
self.q_dot_array = np.array(q_dot_array, dtype=float)
self.q_ddot_array = np.array(q_ddot_array, dtype=float)
self.limits = limits
n = len(times)
if self.q_array.shape != (n, 3):
raise ValueError(f"q_array形状应为({n}, 3), 实际{self.q_array.shape}")
if self.q_dot_array.shape != (n, 3):
raise ValueError(f"q_dot_array形状应为({n}, 3)")
if self.q_ddot_array.shape != (n, 3):
raise ValueError(f"q_ddot_array形状应为({n}, 3)")
for name, arr in [('q_array', self.q_array),
('q_dot_array', self.q_dot_array),
('q_ddot_array', self.q_ddot_array)]:
if not np.all(np.isfinite(arr)):
raise ValueError(f"{name}包含非有限值(NaN或Inf)")
if limits is not None:
for i, t in enumerate(times):
rot_angles = [self.q_array[i, 0], self.q_array[i, 1], self.q_array[i, 2]]
all_pass, violations = limits.check_all_with_indices(rot_angles)
if not all_pass:
err_msgs = []
for vi in violations:
err_msgs.append(
f"时刻t={t:.3f}s: 关节'{limits.limits[vi].joint_name}' "
f"值={rot_angles[vi]:.3f} 超出限位 "
f"[{limits.limits[vi].min_val:.3f}, "
f"{limits.limits[vi].max_val:.3f}]"
)
raise ValueError("；".join(err_msgs))
@classmethod
def pick_and_place_trajectory(cls, n_points=100, duration=3.0,
limits: JointLimitSet = None):
"""
重载取放轨迹：从传送带取件→放置到液压机
θ1: 0° → 90°（水平旋转）
θ2: 0° → -45°（下探，cos(-45°)=0.707用于重力矩计算）
θ3: 0° → 180°（末端翻转使工件就位）
使用五次多项式插值确保位置、速度、加速度连续
"""
times = np.linspace(0, duration, n_points)
pi = 3.141592653589793
q1_start, q1_end = 0.0, 90 * pi / 180
q2_start, q2_end = 0.0, -45 * pi / 180
q3_start, q3_end = 0.0, 180 * pi / 180
q_array = np.zeros((n_points, 3))
q_dot_array = np.zeros((n_points, 3))
q_ddot_array = np.zeros((n_points, 3))
for i, t in enumerate(times):
if duration > 0:
s_norm = t / duration
else:
s_norm = 0.0
# 五次多项式：s = 10t³ - 15t⁴ + 6t⁵
s = 10*s_norm**3 - 15*s_norm**4 + 6*s_norm**5
s_dot = 30*s_norm**2 - 60*s_norm**3 + 30*s_norm**4
s_ddot = 60*s_norm - 180*s_norm**2 + 120*s_norm**3
q_array[i, 0] = q1_start + s * (q1_end - q1_start)
q_array[i, 1] = q2_start + s * (q2_end - q2_start)
q_array[i, 2] = q3_start + s * (q3_end - q3_start)
if duration > 0:
q_dot_array[i, 0] = s_dot * (q1_end - q1_start) / duration
q_dot_array[i, 1] = s_dot * (q2_end - q2_start) / duration
q_dot_array[i, 2] = s_dot * (q3_end - q3_start) / duration
q_ddot_array[i, 0] = s_ddot * (q1_end - q1_start) / (duration**2)
q_ddot_array[i, 1] = s_ddot * (q2_end - q2_start) / (duration**2)
q_ddot_array[i, 2] = s_ddot * (q3_end - q3_start) / (duration**2)
return cls(times, q_array, q_dot_array, q_ddot_array, limits)
def __len__(self):
return len(self.times)
"dynamics/dynamics_solver.py" — 动力学求解器
"""
动力学求解器：重载抓取仿真，计算完整动力学方程各项
τ = M(q)q¨ + C(q,q˙)q˙ + G(q)
支持空载/负载两种质量属性配置，对比力矩差异
重力公式：τ_G2 = -g·(m₂·com₂ + m₃·com₃)·cos(θ₂)（修正：不是cos(θ₁+θ₂)）
末端惯量：I₃ = m₃·r_offset²（修正：不是m₃·com₃²）
"""
import numpy as np
from dynamics.models import RobotParams, MassPropertySet
class DynamicsSolver:
"""
基于拉格朗日方程的3-DOF机械臂动力学求解器
"""
@classmethod
def from_mass_props(cls, params: RobotParams, mass_props: MassPropertySet):
"""工厂方法"""
return cls(params, mass_props)
def __init__(self, params: RobotParams, mass_props: MassPropertySet):
self.params = params
self.mass_props = mass_props
self.gravity_enabled = params.gravity_enabled
self.g = params.g if self.gravity_enabled else 0.0
self.L1 = params.links[0].a
self.L2 = params.links[1].a
self.m1 = mass_props.link_props[0].mass
self.m2 = mass_props.link_props[1].mass
self.m3 = mass_props.link_props[2].mass
self.com1 = mass_props.link_props[0].com_x
self.com2 = mass_props.link_props[1].com_x
self.com3 = mass_props.link_props[2].com_x
self.r_offset3 = mass_props.link_props[2].r_offset_end
# 科氏力耦合系数 h = m₂·L₁·com₂ + m₃·L₁·com₃
self.h = self.m2 * self.L1 * self.com2 + self.m3 * self.L1 * self.com3
# 末端旋转惯量：I₃ = m₃·r_offset²
self.I3 = self.m3 * (self.r_offset3 ** 2)
def inertia_matrix(self, theta2):
"""计算3×3惯性矩阵 M(q)"""
c2 = np.cos(theta2)
M11 = (self.m1 * self.com1**2
+ self.m2 * (self.L1**2 + self.com2**2 + 2*self.L1*self.com2*c2)
+ self.m3 * (self.L1**2 + self.com3**2 + 2*self.L1*self.com3*c2))
M12 = (self.m2 * (self.com2**2 + self.L1*self.com2*c2)
+ self.m3 * (self.com3**2 + self.L1*self.com3*c2))
M22 = self.m2 * self.com2**2 + self.m3 * self.com3**2
M = np.zeros((3, 3))
M[0, 0] = M11
M[0, 1] = M12
M[1, 0] = M12
M[1, 1] = M22
M[2, 2] = self.I3
return M
def coriolis_vector(self, theta2, theta1_dot, theta2_dot, theta3_dot):
"""计算科氏力/离心力向量"""
s2 = np.sin(theta2)
tau_C1 = -self.h * s2 * (2 * theta1_dot * theta2_dot + theta2_dot**2)
tau_C2 = self.h * s2 * theta1_dot**2
tau_C3 = 0.0 # 质点模型下末端旋转无科氏耦合
return np.array([tau_C1, tau_C2, tau_C3], dtype=float)
def gravity_vector(self, theta1, theta2):
"""
计算重力项向量
τ_G1 = 0（竖直偏航轴，恒为零）
τ_G2 = -g·(m₂·com₂ + m₃·com₃)·cos(θ₂) ← 修正：cos(θ₂)不是cos(θ₁+θ₂)
τ_G3 = 0（末端旋转轴）
"""
if not self.gravity_enabled:
return np.zeros(3, dtype=float)
tau_G1 = 0.0
tau_G2 = -self.g * (self.m2 * self.com2 + self.m3 * self.com3) * np.cos(theta2)
tau_G3 = 0.0
return np.array([tau_G1, tau_G2, tau_G3], dtype=float)
def compute_all_terms(self, q, q_dot, q_ddot):
"""计算完整动力学方程的各项分解"""
if len(q) < 3 or len(q_dot) < 3 or len(q_ddot) < 3:
raise ValueError("需要3个关节的角度、速度、加速度")
theta1 = q[0]
theta2 = q[1]
theta3 = q[2]
theta1_dot = q_dot[0]
theta2_dot = q_dot[1]
theta3_dot = q_dot[2]
theta1_ddot = q_ddot[0]
theta2_ddot = q_ddot[1]
theta3_ddot = q_ddot[2]
M = self.inertia_matrix(theta2)
tau_M = M @ np.array([theta1_ddot, theta2_ddot, theta3_ddot])
tau_C = self.coriolis_vector(theta2, theta1_dot, theta2_dot, theta3_dot)
tau_G = self.gravity_vector(theta1, theta2)
tau_total = tau_M + tau_C + tau_G
return tau_M, tau_C, tau_G, tau_total
def compute_sequence(self, state_sequence):
"""对状态序列批量计算力矩"""
n = len(state_sequence)
tau_M_hist = np.zeros((n, 3))
tau_C_hist = np.zeros((n, 3))
tau_G_hist = np.zeros((n, 3))
tau_total_hist = np.zeros((n, 3))
for i in range(n):
tau_M, tau_C, tau_G, tau_total = self.compute_all_terms(
state_sequence.q_array[i],
state_sequence.q_dot_array[i],
state_sequence.q_ddot_array[i]
)
tau_M_hist[i, :] = tau_M
tau_C_hist[i, :] = tau_C
tau_G_hist[i, :] = tau_G
tau_total_hist[i, :] = tau_total
peaks = {
'tau_M_peak': np.max(np.abs(tau_M_hist), axis=0),
'tau_C_peak': np.max(np.abs(tau_C_hist), axis=0),
'tau_G_peak': np.max(np.abs(tau_G_hist), axis=0),
'tau_total_peak': np.max(np.abs(tau_total_hist), axis=0),
}
return {
'peaks': peaks,
'tau_M_hist': tau_M_hist,
'tau_C_hist': tau_C_hist,
'tau_G_hist': tau_G_hist,
'tau_total_hist': tau_total_hist,
}
"dynamics/visualize.py" — 可视化模块
"""
重载抓取仿真结果可视化：4张核心对比图表
"""
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
class Visualizer:
def __init__(self, results_dir="results"):
self.results_dir = results_dir
import os
os.makedirs(results_dir, exist_ok=True)
def plot_torque_comparison_overlay(self, times, empty_total, loaded_total):
"""
① 空载vs负载 合力矩曲线叠加对比
"""
fig, axes = plt.subplots(3, 1, figsize=(10, 9), sharex=True)
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
