python的机器人基础原理工业场景模拟第九十八篇:搬运机器人，给定起始，目标关节角度，采样中间角度，查看末端空间运行轨迹。

《Python机器人基础原理》第九十八篇：离线仿真——搬运机器人（关节空间轨迹规划与末端轨迹可视化）

声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。

1. 实际工业应用场景

周一早晨，发动机缸体搬运工位。新到的2R平面搬运机械臂刚完成安装，上位机工程师小赵正在调试首条搬运路径。任务很简单：让机械臂从起始姿态移动到目标姿态，把缸体送到下料台。小赵在离线编程时设了起始关节角(0°, 30°)和目标关节角(60°, -45°)，用关节空间线性插值生成中间角度，想看看末端实际走的轨迹。

"老郑，我按起始和目标角线性插值，采样了50个中间点。末端走的到底是不是直线？"

老郑端着咖啡走过来："关节空间线性插值，末端在笛卡尔空间走的不是直线。两个旋转关节的角速度恒定，但末端位置涉及三角函数，画出来的是弧线叠加。你跑一下程序，把末端轨迹画出来就知道了。"

构型约定：本篇采用2-DOF平面搬运机械臂（2R），连杆1 a₁=0.40 m，连杆2 a₂=0.35 m，关节1基座回转范围±170°，关节2大臂俯仰范围±120°。起始关节角 q_start = [0.0 rad, 0.524 rad]（0°, 30°），目标关节角 q_target = [1.047 rad, -0.785 rad]（60°, -45°）。插值步数 N=50。技术栈限定 numpy + matplotlib。

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于关节空间轨迹规划与末端轨迹可视化。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。

2. 痛点分析

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

第3章 连杆参数及运动学求解 → 3.3 正运动学 关节角→末端坐标映射，理解关节空间与笛卡尔空间的非线性关系

第6章 轨迹规划与控制 → 6.1 关节空间规划 给定起止关节角，线性插值生成中间构型

第6章 → 6.2 轨迹平滑与约束 关节限位校验、采样点数对轨迹质量的影响

第9章 仿真实验 → 离线调试 搬运路径预演，轨迹形状可视化验证

本篇代码解决 L2 离线仿真校核层。输入：DH参数、起始/目标关节角、插值步数。处理：①校验起止关节角是否在限位内；②在关节空间对起止角进行线性插值，采样N个中间构型；③逐点校验中间关节角是否越限；④对每个构型调用正运动学计算末端坐标；⑤计算末端线速度曲线。输出：4张分析图表 + 轨迹数据CSV。

不覆盖：逆运动学、笛卡尔空间直线规划、动力学约束、实时控制。

3. 核心逻辑讲解

物理直觉

想象你的手臂从桌面左边拿一个杯子放到右边。你可以让肩膀和手肘匀速转动（关节空间线性），但你的手端画出的路径是一条弧线，不是水平直线。这是因为关节角和手端位置之间不是简单的正比关系——它涉及三角函数。

关节空间线性插值就是在每个关节的角变量之间等比例取中间值。对于2R臂，两个关节各自从起始角匀速走到目标角，末端位置由正运动学公式算出。因为正运动学里有cos和sin，两个线性变化的角经过非线性映射后，末端轨迹是两段圆弧运动的矢量和叠加，一般不是严格的圆弧，更不是直线。

理解这一点是轨迹规划的基础。工业现场中，如果你需要末端走直线（比如激光切割、涂胶），就不能用关节空间线性插值，而要做笛卡尔空间直线规划再反解逆运动学。但在此之前，你必须先搞清楚关节空间插值到底走出什么形状——这就是本篇的任务。

业务逻辑 → 代码映射

输入：q_start = [q1_s, q2_s], q_target = [q1_t, q2_t], N=50

↓

【步骤0：限位校验】

检查 q_start, q_target 每个分量是否在 JointLimit 范围内

↓

【步骤1：关节空间线性插值】

for k = 0..N:

t = k / N

q1_k = q1_s + t * (q1_t - q1_s)

q2_k = q2_s + t * (q2_t - q2_s)

检查 q1_k, q2_k 是否越限

↓

【步骤2：正运动学批量计算】

for k = 0..N:

x_k = a1 * cos(q1_k) + a2 * cos(q1_k + q2_k)

y_k = a1 * sin(q1_k) + a2 * sin(q1_k + q2_k)

↓

【步骤3：末端线速度】

v_k = sqrt((dx/dt)² + (dy/dt)²) (中心差分)

↓

【步骤4：4张图表】

关键公式文字翻译

正运动学解析解（平面2R臂）：

-

"x = a₁·cos(θ₁) + a₂·cos(θ₁+θ₂)"：末端X坐标 = 大臂水平投影 + 小臂水平投影

-

"y = a₁·sin(θ₁) + a₂·sin(θ₁+θ₂)"：末端Y坐标 = 大臂垂直投影 + 小臂垂直投影

- 为什么不用DH矩阵：2R平面臂有闭式解析解，直接三角函数比4×4矩阵快一个数量级，适合逐点计算。

线性插值：

-

"q(t) = q_start + t * (q_target - q_start)"，其中 t ∈ [0, 1]

- 物理含义：每个关节以恒定角速度从起始角走到目标角，角速度 = (q_target - q_start) / T

末端线速度（中心差分）：

-

"vx[i] ≈ (x[i+1] - x[i-1]) / (t[i+1] - t[i-1])"，vy同理

-

"v[i] = sqrt(vx² + vy²)"

- 为什么用中心差分：端点处用前向/后向差分，中间点用中心差分，精度O(h²)。这能清晰展示末端线速度在关节空间线性插值下如何变化——不是恒定的。

轨迹长度估算：

- 用折线累加各相邻点间的欧氏距离，近似末端实际走过的路径长度。这比直线距离（起点到终点的线段长）更能反映真实运动范围。

4. OOP代码实现

技术栈

限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。全部自实现。

项目结构树

trajectory_planner/

├── run_trajectory_planner.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── kinematics.py

│ ├── trajectory.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_trajectory_planner.py

├── results/

└── output/

核心源码

"robot/models.py" — 数据模型

"""

Data models: DH parameters, robot config, joint limits, trajectory params

All parameters built-in, self-contained

"""

import math

from dataclasses import dataclass, field

from typing import List, Tuple

@dataclass

class JointLimit:

"""Joint angle limits (rad)"""

joint_name: str

lower: float

upper: float

def __post_init__(self):

if self.lower >= self.upper:

raise ValueError(

f"{self.joint_name}: lower >= upper")

if not (math.isfinite(self.lower)

and math.isfinite(self.upper)):

raise ValueError(

f"{self.joint_name}: limits must be finite")

@dataclass

class RobotConfig:

"""

2-DOF planar搬运臂 (2R)

a1=0.40m, a2=0.35m

"""

a1: float = 0.40

a2: float = 0.35

joint_limits: List[JointLimit] = field(default_factory=list)

def __post_init__(self):

if not self.joint_limits:

self.joint_limits = [

JointLimit("J1", math.radians(-170),

math.radians(170)),

JointLimit("J2", math.radians(-120),

math.radians(120)),

]

for a in [self.a1, self.a2]:

if a <= 0:

raise ValueError("Link lengths must be positive")

@dataclass

class TrajectoryParams:

"""Joint-space trajectory parameters with limit checking"""

q_start: Tuple[float, float]

q_target: Tuple[float, float]

n_steps: int = 50

_config: RobotConfig = field(default=None, repr=False)

def __post_init__(self):

for q in list(self.q_start) + list(self.q_target):

if not math.isfinite(q):

raise ValueError("Joint angles must be finite")

if self.n_steps < 2:

raise ValueError("n_steps must be >= 2")

# Limit check on start and target

if self._config is not None:

limits = self._config.joint_limits

for i, (q_val, lim) in enumerate(

[(self.q_start[0], limits[0]),

(self.q_start[1], limits[1]),

(self.q_target[0], limits[0]),

(self.q_target[1], limits[1])]):

if not (lim.lower <= q_val <= lim.upper):

raise ValueError(

f"q{'_start' if i < 2 else '_target'}"

f"[{i % 2}] = {math.degrees(q_val):.1f}deg "

f"outside {lim.joint_name} limits "

f"[{(math.degrees(lim.lower)):.0f}deg, "

f"{(math.degrees(lim.upper)):.0f}deg]")

"robot/kinematics.py" — 正运动学

"""

Forward kinematics for 2-DOF planar arm

Analytical solution for speed

"""

import numpy as np

from robot.models import RobotConfig

class Planar2RKinematics:

def __init__(self, config: RobotConfig):

self.config = config

def fk_single(self, q1, q2):

"""

Single forward kinematics

Returns: np.array([x, y])

Why array: consistent API with batch method and cross-article

"""

a1, a2 = self.config.a1, self.config.a2

x = a1 * np.cos(q1) + a2 * np.cos(q1 + q2)

y = a1 * np.sin(q1) + a2 * np.sin(q1 + q2)

return np.array([x, y], dtype=float)

def fk_batch(self, q1_array, q2_array):

"""

Vectorized forward kinematics for trajectory points

Returns: (x_array, y_array)

"""

a1, a2 = self.config.a1, self.config.a2

x = a1 * np.cos(q1_array) + a2 * np.cos(q1_array + q2_array)

y = a1 * np.sin(q1_array) + a2 * np.sin(q1_array + q2_array)

return x, y

"robot/trajectory.py" — 轨迹生成

"""

Joint-space linear interpolation trajectory generator

Samples intermediate joint angles and computes end-effector positions

Includes joint limit checking at every interpolated point

"""

import numpy as np

from robot.models import RobotConfig, TrajectoryParams

from robot.kinematics import Planar2RKinematics

class JointSpaceTrajectory:

def __init__(self, config: RobotConfig,

params: TrajectoryParams):

self.config = config

self.params = params

self.kin = Planar2RKinematics(config)

def interpolate(self):

"""

Linear interpolation in joint space

Returns: q1_traj, q2_traj (arrays of length n_steps+1)

Why n_steps+1: includes both start and end points

"""

q1_s, q2_s = self.params.q_start

q1_t, q2_t = self.params.q_target

n = self.params.n_steps

t = np.linspace(0.0, 1.0, n + 1)

q1_traj = q1_s + t * (q1_t - q1_s)

q2_traj = q2_s + t * (q2_t - q2_s)

# Per-point limit check

limits = self.config.joint_limits

for i in range(len(q1_traj)):

if not (limits[0].lower <= q1_traj[i] <= limits[0].upper):

raise ValueError(

f"q1 at step {i} ({np.degrees(q1_traj[i]):.1f}deg) "

f"exceeds J1 limit")

if not (limits[1].lower <= q2_traj[i] <= limits[1].upper):

raise ValueError(

f"q2 at step {i} ({np.degrees(q2_traj[i]):.1f}deg) "

f"exceeds J2 limit")

return q1_traj, q2_traj

def compute_trajectory_points(self):

"""

Full pipeline: interpolate -> FK -> collect points

Returns: q1_traj, q2_traj, x_traj, y_traj

"""

q1_traj, q2_traj = self.interpolate()

x_traj, y_traj = self.kin.fk_batch(q1_traj, q2_traj)

return q1_traj, q2_traj, x_traj, y_traj

def compute_end_effector_velocity(self, x, y):

"""

Compute end-effector linear velocity using central difference

Why: to demonstrate that constant joint velocity does NOT

produce constant Cartesian velocity

"""

n = len(x)

v = np.zeros(n)

# Central difference for interior points

for i in range(1, n - 1):

dx_dt = (x[i + 1] - x[i - 1]) / 2.0

dy_dt = (y[i + 1] - y[i - 1]) / 2.0

v[i] = np.sqrt(dx_dt**2 + dy_dt**2)

# Forward/backward difference for endpoints

if n >= 2:

dx_fwd = x[1] - x[0]

dy_fwd = y[1] - y[0]

v[0] = np.sqrt(dx_fwd**2 + dy_fwd**2)

dx_bwd = x[-1] - x[-2]

dy_bwd = y[-1] - y[-2]

v[-1] = np.sqrt(dx_bwd**2 + dy_bwd**2)

return v

def compute_trajectory_length(self, x_traj, y_traj):

"""

Estimate actual path length by summing segment lengths

Why: linear interpolation in joint space does NOT produce

straight line in Cartesian space, so path length > Euclidean dist

"""

dx = np.diff(x_traj)

dy = np.diff(y_traj)

segment_lengths = np.sqrt(dx**2 + dy**2)

return float(np.sum(segment_lengths))

def evaluate(self):

"""

End-to-end evaluation

Returns dict with all trajectory data

"""

q1, q2, x, y = self.compute_trajectory_points()

path_length = self.compute_trajectory_length(x, y)

euclidean_dist = float(

np.sqrt((x[-1] - x[0])**2 + (y[-1] - y[0])**2))

velocity = self.compute_end_effector_velocity(x, y)

return {

'q1': q1,

'q2': q2,

'x': x,

'y': y,

'path_length': path_length,

'euclidean_dist': euclidean_dist,

'velocity': velocity,

}

"robot/visualize.py" — 可视化

"""

Trajectory visualization: 4 charts

1. End-effector Cartesian trajectory (X-Y plane)

2. Joint angle profiles over time

3. End-effector linear velocity profile

4. Trajectory overlaid on workspace boundary arcs

"""

import os

import numpy as np

import matplotlib.pyplot as plt

plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans',

'Noto Sans CJK SC']

plt.rcParams['axes.unicode_minus'] = False

class TrajectoryVisualizer:

def __init__(self, results_dir="results"):

self.results_dir = results_dir

os.makedirs(results_dir, exist_ok=True)

def plot_cartesian_trajectory(self, x, y):

"""

1. End-effector path in X-Y plane

"""

fig, ax = plt.subplots(figsize=(8, 8))

ax.plot(x, y, 'b.-', linewidth=1.5, markersize=3,

label='End-Effector Trajectory (joint-space linear)')

ax.plot(x[0], y[0], 'go', markersize=10, label='Start', zorder=5)

ax.plot(x[-1], y[-1], 'ro', markersize=10, label='Target', zorder=5)

# Annotate key points

for idx in [0, len(x)//4, len(x)//2, 3*len(x)//4, -1]:

ax.annotate(f'{idx}', (x[idx], y[idx]),

fontsize=7, ha='center', va='bottom')

ax.set_xlabel('X (m)')

ax.set_ylabel('Y (m)')

ax.set_title('End-Effector Cartesian Trajectory '

'(Joint-Space Linear Interpolation)')

ax.legend()

ax.grid(True, alpha=0.3)

ax.set_aspect('equal', adjustable='box')

plt.tight_layout()

path = os.path.join(self.results_dir,

"01_cartesian_trajectory.png")

plt.savefig(path, dpi=150)

plt.close()

return path

def plot_joint_profiles(self, q1, q2):

"""

2. Joint angle profiles over interpolation parameter t

"""

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True)

t = np.linspace(0, 1, len(q1))

ax1.plot(t, np.degrees(q1), 'b.-', linewidth=1.5)

ax1.set_ylabel('J1 Angle (deg)')

ax1.set_title('Joint 1 Angle Profile (Linear Interpolation)')

ax1.grid(True, alpha=0.3)

ax2.plot(t, np.degrees(q2), 'r.-', linewidth=1.5)

ax2.set_xlabel('Interpolation Parameter t (0→1)')

ax2.set_ylabel('J2 Angle (deg)')

ax2.set_title('Joint 2 Angle Profile (Linear Interpolation)')

ax2.grid(True, alpha=0.3)

plt.tight_layout()

path = os.path.join(self.results_dir,

"02_joint_angle_profiles.png")

plt.savefig(path, dpi=150)

plt.close()

return path

def plot_velocity_profile(self, velocity):

"""

3. End-effector linear velocity over time

"""

fig, ax = plt.subplots(figsize=(10, 5))

t = np.linspace(0, 1, len(velocity))

ax.plot(t, velocity, 'm.-', linewidth=1.5, markersize=3)

ax.set_xlabel('Interpolation Parameter t (0→1)')

ax.set_ylabel('End-Effector Linear Velocity (m/s, arbitrary time scale)')

ax.set_title('End-Effector Linear Velocity '

'(Constant Joint Velocity → Non-Constant Cartesian Velocity)')

ax.grid(True, alpha=0.3)

plt.tight_layout()

path = os.path.join(self.results_dir,

"03_velocity_profile.png")

plt.savefig(path, dpi=150)

plt.close()

return path

def plot_workspace_with_trajectory(self, x, y,

a1=0.40, a2=0.35):

"""

4. Trajectory overlaid on workspace boundary arcs

"""

fig, ax = plt.subplots(figsize=(8, 8))

theta = np.linspace(0, 2*np.pi, 200)

ax.plot((a1+a2)*np.cos(theta), (a1+a2)*np.sin(theta),

'k:', linewidth=1, label='Max Reach Boundary')

r_inner = abs(a1 - a2)

ax.plot(r_inner*np.cos(theta), r_inner*np.sin(theta),

'k:', linewidth=1, label='Min Reach Boundary')

ax.plot(x, y, 'b.-', linewidth=1.5, markersize=3,

label='Planned Trajectory')

ax.plot(0, 0, 'ks', markersize=8, label='Base (0,0)')

ax.plot(x[0], y[0], 'go', markersize=10, label='Start')

ax.plot(x[-1], y[-1], 'ro', markersize=10, label='Target')

ax.set_xlabel('X (m)')

ax.set_ylabel('Y (m)')

ax.set_title('Trajectory Overlaid on Workspace Boundaries')

ax.legend(fontsize=8)

ax.grid(True, alpha=0.3)

ax.set_aspect('equal', adjustable='box')

plt.tight_layout()

path = os.path.join(self.results_dir,

"04_workspace_with_trajectory.png")

plt.savefig(path, dpi=150)

plt.close()

return path

5. 可视化要求

序号 图表文件 服务结论

①

"01_cartesian_trajectory.png" 末端在笛卡尔空间的真实运动轨迹，可直观确认轨迹形状（弧线叠加，非直线）

②

"02_joint_angle_profiles.png" 关节1和关节2的角度随时间线性变化曲线，验证插值正确性和限位合规性

③

"03_velocity_profile.png" 末端线速度曲线，证明恒定角速度→非恒定笛卡尔速度，支撑第7节速度不均匀论点

④

"04_workspace_with_trajectory.png" 轨迹叠加在工作空间边界上，检查是否超出可达范围

6. README使用说明 + 核心知识点卡片

快速开始

pip install -r requirements.txt

python -m pytest tests/test_trajectory_planner.py -v

python run_trajectory_planner.py

数据格式说明

# 修改起始/目标关节角：编辑 TrajectoryParams.q_start, q_target

# 修改插值步数：编辑 TrajectoryParams.n_steps

# 修改臂长：编辑 RobotConfig.a1, a2

参数 修改位置 说明

起始关节角

"TrajectoryParams.q_start" rad, (q1, q2)

目标关节角

"TrajectoryParams.q_target" rad, (q1, q2)

插值步数

"TrajectoryParams.n_steps" 无量纲

L1臂长

"RobotConfig.a1" m

L2臂长

"RobotConfig.a2" m

📌 知识点卡片

关节空间轨迹规划（对应课本第6章6.1节）

- 关节空间线性插值：q(t) = q_start + t·(q_target - q_start)，t∈[0,1]

- 非线性映射：关节空间线性 ≠ 笛卡尔空间直线，因为正运动学含三角函数，末端轨迹是两段圆弧运动的矢量和

- 速度不均匀：恒定关节角速度经非线性映射后，末端线速度非恒定，搬运中可能导致负载晃动

- 轨迹长度：折线累加距离 > 欧氏距离，比值量化弯曲程度

- 工程铁律：需要末端走直线时，必须做笛卡尔空间规划+逆运动学，而非关节空间插值

7. 总结与工程师思考

工业落地难处

一、关节空间插值导致末端路径不可控。 本篇演示了关节空间线性插值走出的弧线叠加轨迹。工业现场中，如果末端需要严格沿直线移动（如激光焊接、密封涂胶），弧线轨迹会导致加工质量不合格。解决方案是笛卡尔空间直线规划，但这需要实时逆运动学求解，对控制器算力要求更高。

二、末端线速度不均匀。 关节空间线性插值只保证角速度恒定，末端在笛卡尔空间的速度是变化的。图3清晰展示了速度波动——在关节角变化率恒定的情况下，末端线速度在轨迹中段出现峰值。实际搬运中，速度突变会导致负载晃动甚至脱落。需要加入速度/加速度平滑（如梯形速度规划、S曲线）。

三、关节限位必须逐点校验。 即使起始和目标角在限位内，中间插值点也可能越限（例如经过奇异构型时）。本篇在interpolate()中逐点检查，确保生成轨迹物理可行。但真实控制器还需要考虑速度/加速度限制，不是简单的位置限位。

工程师心得

小赵看着终端输出的数字："起点位置(0.7030, 0.1751) m——0.40×cos(0°)+0.35×cos(30°)=0.7031 m，0.35×sin(30°)=0.1750 m，对上了。终点(0.5381, 0.4370) m，欧氏距离0.3095 m，实际路径长度0.3360 m，比值1.086——多了8.6%。"

老郑指着图3的线速度曲线："你看，速度不是平的。关节匀速转，末端速度却忽快忽慢。如果搬运的是易碎品，这个速度波动就可能把东西甩出去。"

"好，关节空间插值只适合精度要求不高的搬运。下一步该做逆运动学了，有了逆解才能做笛卡尔空间直线规划。"

"对。下一篇就写带关节限位的逆解。"

8. 写作与风格限制

本篇全部代码为课程离线仿真，运行在普通PC上，用于关节空间轨迹规划与末端轨迹可视化。代码中不包含任何真实机器人控制器实时运算或安全逻辑。 完全去营销，中立客观。拒绝论文式生硬开头，保持故事叙事感。全部物理量标注完整单位。

9. 文末附录

环境配置

pip install numpy>=1.24.0 matplotlib>=3.7.0

"requirements.txt"：

numpy>=1.24.0

matplotlib>=3.7.0

运行说明

cd trajectory_planner

pip install -r requirements.txt

python -m pytest tests/ -v

python run_trajectory_planner.py

参数修改指南

想改什么 改哪里 示例

起始角

"TrajectoryParams.q_start" 改(0,0.524)→(0,0)

目标角

"TrajectoryParams.q_target" 改(1.047,-0.785)→(1.57,0)

插值步数

"TrajectoryParams.n_steps" 改50→100

L1臂长

"RobotConfig.a1" 改0.40→0.50

L2臂长

"RobotConfig.a2" 改0.35→0.30

输出文件清单

文件 含义

"results/01_cartesian_trajectory.png" 末端笛卡尔空间轨迹

"results/02_joint_angle_profiles.png" 关节角随时间变化曲线

"results/03_velocity_profile.png" 末端线速度曲线

"results/04_workspace_with_trajectory.png" 轨迹叠加工作空间边界

"output/trajectory_points.csv" 轨迹点坐标数据

仿真结果摘要（真实运行数据，从终端输出复制）

统计项 数值 数据来源

机器人模型 2-DOF平面搬运臂 (2R) 配置

连杆总长 0.75 m a1+a2

起始关节角 (0.000 rad, 0.524 rad) 内置

目标关节角 (1.047 rad, -0.785 rad) 内置

插值步数 50 内置

起点位置 (0.7030, 0.1751) m 仿真输出

终点位置 (0.5381, 0.4370) m 仿真输出

欧氏距离 0.3095 m 仿真输出

实际路径长度 0.3360 m 仿真输出

路径长度/欧氏距离 1.086 仿真输出

测试通过 3/3 pytest

正文完。以下为

"run_trajectory_planner.py" 完整主入口 +

"tests/test_trajectory_planner.py" 完整测试文件。

"run_trajectory_planner.py"

"""

Joint-Space Trajectory Planner - Main Entry

Run: python run_trajectory_planner.py

"""

import os

import csv

import numpy as np

from robot.models import RobotConfig, TrajectoryParams

from robot.trajectory import JointSpaceTrajectory

from robot.visualize import TrajectoryVisualizer

def main():

print("=" * 65)

print("Offline Sim: Transfer Robot - Joint Space Trajectory Planning")

print("=" * 65)

# 1. Init

config = RobotConfig()

params = TrajectoryParams(

q_start=(0.0, 0.524), # (0 deg, 30 deg)

q_target=(1.047, -0.785), # (60 deg, -45 deg)

n_steps=50

)

# Attach config for limit checking in TrajectoryParams

params._config = config

planner = JointSpaceTrajectory(config, params)

viz = TrajectoryVisualizer(results_dir="results")

print(f"\n--- Robot Config ---")

print(f" a1 = {config.a1:.2f} m, a2 = {config.a2:.2f} m")

print(f" q_start = ({params.q_start[0]:.3f}, "

f"{params.q_start[1]:.3f}) rad")

print(f" q_target = ({params.q_target[0]:.3f}, "

f"{params.q_target[1]:.3f}) rad")

print(f" n_steps = {params.n_steps}")

# 2. Evaluate trajectory

print("\n--- Computing Trajectory ---")

result = planner.evaluate()

q1, q2, x, y = result['q1'], result['q2'], result['x'], result['y']

velocity = result['velocity']

print(f"\n--- Results (Real Computation) ---")

print(f" Start position: ({x[0]:.4f}, {y[0]:.4f}) m")

print(f" End position: ({x[-1]:.4f}, {y[-1]:.4f}) m")

print(f" Euclidean distance: {result['euclidean_dist']:.4f} m")

print(f" Actual path length: {result['path_length']:.4f} m")

print(f" Path length / Euclidean: "

f"{result['path_length']/result['euclidean_dist']:.4f}")

print(f" Max

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