《Python机器人基础原理》第十九篇：机械臂仿真——零重力工况模拟，关闭重力项，输出仅由运动产生的关节力矩曲线

 

1. 实际工业应用场景

 

周一早晨，某航天部件装配车间的地面测试大厅。一台3-DOF垂直关节机械臂（基座偏航+肘部俯仰+末端旋转）正在进行卫星天线展开机构的微重力装配模拟。机器人构型：大臂长600 mm，小臂长450 mm，末端旋转轴位于小臂末端。连杆质量：大臂5.2 kg（质心距基座0.28 m），小臂3.1 kg（质心距肘部0.20 m），末端执行器1.5 kg（质心距肘部0.45 m，距末端旋转轴线横向偏置0.02 m）。关节额定扭矩：基座偏航轴60 N·m，肘部俯仰轴180 N·m，末端旋转轴30 N·m。

 

调试工程师小李盯着屏幕上的力矩曲线，眉头紧锁。

 

"师傅，这个Joint1的力矩曲线怎么有个周期性的小波动，幅度大概±3 N·m？"小李指着示波器上基座偏航轴的力矩轨迹，"我们明明在地面模拟微重力工况，把重力补偿关了。"

 

旁边的老周放下咖啡杯："你关的是哪部分？"

 

"求解器里gravity_enabled=False，g直接设成0。前馈里也把重力项摘了。"

 

"那波动不是重力。是惯性耦合。基座偏航轴虽然不扛重力，但肘部加速的时候，惯性会通过M₁₂传给Joint1。你算过纯运动产生的力矩吗？不考虑重力，只算M(q)q¨ + C(q,q˙)q˙。"

 

"还没。我直接看控制器的前馈输出，里面混着重力项。我想单独把重力拿掉，看纯运动力矩长什么样。"

 

"对。你把动力学方程里的G(q)整个摘掉，只留M和C。给定一段轨迹的(q, q˙, q¨)，算出来M·q¨ + C·q˙。在零重力下，Joint1的力矩应该完全来自M₁₁自惯性和M₁₂耦合——你粗算一下：M₁₂峰值约0.78 kg·m²，五次多项式加速度峰值约0.50 rad/s²，耦合贡献≈0.39 N·m；加上M₁₁自项≈1.4 N·m，Joint1总力矩应该在1~2 N·m量级。你看到的±3 N·m，差不太多，说明耦合确实存在。"

 

小李打开编辑器："那我写个离线仿真，零重力模式，跑一段θ₁和θ₂同时反向运动的轨迹——让M₁₂耦合和科氏项的特征都出来。"

 

"科氏项τ_C ∝ sinθ₂，θ₂从0°→-60°的过程中sinθ₂先增后减，图里应该能看到这个变化。"

 

"好。末端旋转轴我就不大幅动了，质点模型下它几乎无力矩，正好验证轻载。"

 

本篇解决的问题：在动力学求解器中关闭重力项（gravity_enabled=False, g=0），给定关节状态序列，仅计算惯性项M(q)q¨和科氏/离心项C(q,q˙)q˙产生的关节力矩，输出纯运动力矩曲线。用于微重力模拟、太空机械臂地面测试、重力补偿算法验证。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于零重力工况下纯运动关节力矩的计算与可视化。不可直接用于真实机器人控制器、伺服驱动器或航天实验设备。真实航天任务需结合在轨参数辨识、零重力悬浮平台校准。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

课程模块 本篇对应的工程痛点

第4章 机器人动力学建模 → 4.4 关节空间动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q) 零重力工况下G(q)≡0，需验证纯运动力矩（惯性+科氏）的计算正确性

第5章 机器人关节驱动与控制 → 5.3 前馈补偿 微重力模拟时前馈仅需M·q¨des + C·q˙des，重力补偿模块必须关闭

第6章 轨迹规划与控制 → 6.2 轨迹评价 评估无重力环境下轨迹的纯动力学可行性

第9章 协作机器人 → 零力拖动示教 零重力模式下去除重力项后的力矩曲线是拖动示教阻抗控制的基础

 

本篇代码解决 L2 离线动力学校核层。输入：关节状态数组(q, q˙, q¨) + 机器人参数 + 重力开关标志。输出：各关节的τ_M、τ_C、τ_total（无τ_G）。代码不是设备实时控制器，不做伺服闭环，不做轨迹规划。它做的事情是：在零重力假设下，给定状态直接算纯运动力矩。

 

不覆盖：轨迹生成、实时控制、摩擦力建模、6轴完整动力学、零重力悬浮平台硬件接口。

 

3. 核心逻辑讲解

 

物理直觉

 

你有没有在游泳池里推过一个大泡沫板？水里有浮力抵消了重力，你几乎感觉不到重量——但你要让泡沫板加速，还是要用力。这就是惯性。你要让它转弯，还是要用力。这就是科氏力。重力没了，但运动本身的"懒惰性"还在。

 

太空里的机械臂就是这样。国际空间站上的机械臂在微重力环境下，重力几乎为零。但加速移动太阳能板的时候，关节电机依然要出力——出的是惯性力和科氏力的力矩。本篇就是把重力项从动力学方程里整个拿掉，只看纯运动产生的力矩。

 

业务逻辑 → 代码映射

 

输入：机器人参数（DH、质量、质心）+ 关节状态序列(q, q˙, q¨) + gravity_enabled=False

  ↓

步骤1：对每一组状态，计算惯性矩阵 M(q)

  ↓

步骤2：计算科氏力/离心力向量 C(q,q˙)·q˙

  ↓

步骤3：重力项 → gravity_enabled=False时G(q)返回全零向量

  ↓

步骤4：τ_total = M·q¨ + C·q˙ + 0

  ↓

步骤5：汇总所有状态点的力矩，统计峰值，与硬件限位对比

  ↓

步骤6：输出4张图表

 

关键公式文字翻译

 

惯性项 τ_M = M(q)·q¨：M是3×3矩阵。零重力下这是力矩的主要来源。M₁₁≈1.9 kg·m²（基座自惯性），M₁₂≈0.78 kg·m²（耦合项，来自m₂L₁com₂+m₃L₁com₃），M₂₂≈0.66 kg·m²（肘部自惯性）。末端旋转轴的I₃ = m₃·r_offset²，r_offset是质心到旋转轴线的横向距离，本篇取0.02 m，I₃≈0.0006 kg·m²——量级极小，几乎无力矩。

 

科氏力项 τ_C = C(q,q˙)·q˙：τ_C1 = -h·sinθ₂·(2θ̇₁θ̇₂+θ̇₂²)，τ_C2 = h·sinθ₂·θ̇₁²。h = m₂L₁com₂ + m₃L₁com₃ ≈ 0.78 kg·m²。科氏力矩与sinθ₂成正比——θ₂接近0°时科氏项接近零，θ₂到-60°时sin(-60°)≈-0.866达到最大。

 

重力项 τ_G = 0：本篇核心设定。g=0或G(q)强制返回零。验证：输出力矩中不含任何与姿态相关的常数项。

 

单位：力矩 N·m，角度 rad，角速度 rad/s，角加速度 rad/s²。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。

 

项目结构树

 

zero_g_torque_sim/

├── run_zero_g_sim.py

├── requirements.txt

├── dynamics/

│ ├── __init__.py

│ ├── models.py

│ ├── trajectory.py

│ ├── dynamics_solver.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_zero_g_sim.py

└── results/

 

核心源码

 

"dynamics/models.py" — dataclass数据模型

 

"""

3-DOF垂直关节机械臂数据模型

零重力仿真专用：支持重力开关

 

构型说明：基座偏航(θ1, 竖直轴) + 肘部俯仰(θ2, 水平轴) + 末端旋转(θ3)

物理声明：

  - θ1绕竖直轴旋转，重力对其力矩恒为零（无论gravity_enabled为何值）

  - θ2为水平俯仰轴，重力矩 = -g·(m₂com₂+m₃com₃)·cos(θ₂)，注意是cos(θ₂)不是cos(θ₁+θ₂)

  - θ3为末端旋转轴，质点模型下重力矩为零

  - 零重力模式：gravity_enabled=False → G(q)返回全零

  - 末端旋转惯量I₃ = m₃·r_offset²，r_offset为质心到旋转轴线的横向距离

 

质心坐标定义（全局统一）：

  - com₁: 大臂质心沿连杆方向距基座关节的距离 (m)

  - com₂: 小臂质心沿连杆方向距肘部关节的距离 (m)

  - com₃: 末端负载质心沿小臂方向距肘部关节的距离 (m)

  - r_offset₃: 末端负载质心到末端旋转轴线的横向距离 (m)

"""

 

 

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° (基座偏航, 额定60 N·m)

        θ2: ±140° (肘部俯仰, 额定180 N·m)

        θ3: ±180° (末端旋转, 额定30 N·m)

        """

        pi = 3.141592653589793

        return cls(limits=[

            JointLimit(-170 * pi / 180, 170 * pi / 180, 'joint1_base_yaw', 60.0),

            JointLimit(-140 * pi / 180, 140 * pi / 180, 'joint2_elbow_pitch', 180.0),

            JointLimit(-180 * pi / 180, 180 * pi / 180, 'joint3_end_rotate', 30.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.0 # 末端质心到旋转轴线的横向偏置

 

    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 default_3dof_arm(cls):

        """

        3-DOF机械臂质量参数

        大臂: 5.2 kg, 质心距基座0.28 m (L1=0.60m)

        小臂: 3.1 kg, 质心距肘部0.20 m (L2=0.45m)

        末端: 执行器1.5 kg, 质心距肘部0.45 m, 横向偏置0.02 m

        """

        link_lengths = [0.60, 0.45, 0.0]

        props = [

            LinkMassProps(5.2, 0.28, 0.0, 0.0, 'link1_upper_arm', link_lengths[0]),

            LinkMassProps(3.1, 0.20, 0.0, 0.0, 'link2_forearm', link_lengths[1]),

            LinkMassProps(1.5, 0.45, 0.0, 0.0, 'link3_end_effector', None, 0.02),

        ]

        return cls(link_props=props, link_lengths=link_lengths)

 

    def copy_with_modified_com(self, link_index: int, new_com_x: float):

        """返回修改后的质心属性副本"""

        new_props = []

        for i, prop in enumerate(self.link_props):

            if i == link_index:

                new_props.append(LinkMassProps(

                    prop.mass, new_com_x, prop.com_y, prop.com_z,

                    prop.link_name, prop.max_com_x, prop.r_offset_end))

            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 MassPropertySet(link_props=new_props, link_lengths=self.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机械臂完整参数

    大臂600mm, 小臂450mm

    gravity_enabled: 重力项计算开关。False时G(q)返回全零。

    g: 重力加速度值。gravity_enabled=False时此值被忽略（内部强制为0）。

    """

    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.600, 0.0),

            DHParam(0.0, 0.0, 0.450, 0.0),

            DHParam(0.0, 0.0, 0.0, 0.0),

        ]

        return cls(links=links)

 

    def zero_gravity(self):

        """

        关闭重力：返回新的RobotParams副本

        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" — 关节状态序列生成器

 

"""

关节状态序列生成器：生成平滑轨迹状态用于零重力力矩计算

使用五次多项式插值生成连续的位置、速度、加速度

轨迹设计：θ₁和θ₂同时反向运动，最大化M₁₂耦合和科氏项特征

"""

 

 

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 smooth_trajectory(cls, n_points=100, duration=2.0, limits: JointLimitSet = None):

        """

        生成平滑轨迹状态序列

        θ1: 0° → 90°（正向）

        θ2: 0° → -60°（反向，最大化科氏项sinθ₂变化）

        θ3: 0° → 10°（微小运动，验证末端旋转轴轻载）

        使用五次多项式插值确保位置、速度、加速度连续

        """

        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, -60 * pi / 180

        q3_start, q3_end = 0.0, 10 * 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) if gravity_enabled else 0]

 

3-DOF机械臂：基座偏航(θ1) + 肘部俯仰(θ2) + 末端旋转(θ3)

零重力模式：gravity_enabled=False → G(q)返回全零

 

重力公式修正（第十八篇遗留问题彻底了结）：

  - θ1为竖直偏航轴 → τ_G1恒为0（无论gravity_enabled为何值）

  - θ2为水平俯仰轴 → τ_G2 = -g·(m₂·com₂ + m₃·com₃)·cos(θ₂)

    注意是cos(θ₂)不是cos(θ₁+θ₂)，θ₁绕竖直轴转不改变俯仰角

  - θ3为末端旋转轴 → τ_G3 = 0

 

末端旋转惯量修正：

  - I₃ = m₃·r_offset²，r_offset为质心到旋转轴线的横向距离

  - 不再错误使用com₃（沿臂方向距肘部距离）作为回转半径

 

注意：每次修改质量/质心参数后必须创建新的DynamicsSolver实例

"""

 

 

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

        # g值仅在gravity_enabled=True时使用；False时内部强制为0

        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²

        # r_offset是质心到末端旋转轴线的横向距离，不是com₃

        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):

        """

        计算科氏力/离心力向量（3维）

        质点模型下θ3无速度耦合，τ_C3=0

        """

        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 # 基座偏航轴，竖直轴，重力力矩恒为零

 

        # θ2为水平俯仰轴，重力矩取决于θ₂（不是θ₁+θ₂）

        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_motion_only_torque(self, times, tau_M_hist, tau_C_hist, tau_total_hist):

        """

        ① 纯运动力矩曲线（无重力项）

        """

        fig, axes = plt.subplots(3, 1, figsize=(10, 9), sharex=True)

 

        jo

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！