添加了config.yaml，所有参数都从config文件中加载，优化了文件结构

soft_arm_sim/config/config.yaml

@@ -0,0 +1,14 @@
+soft_arm_simulator:
+  ros__parameters:
+    # --- 机器人物理参数 ---
+    num_sections: 3           # PCC 段数
+    section_length: 0.240     # 每段长度 (m)
+    disks_per_section: 3      # 每段的圆盘数量
+    disk_radius: 0.033        # 绳索孔距圆心的半径 (m) (用于运动学计算)
+    
+    # --- 视觉参数 ---
+    visual_disk_radius: 0.04  # 圆盘实际显示半径 (m) (可视化的红色圆盘大小)
+    visual_disk_thickness: 0.005 # 圆盘厚度 (m)
+    
+    # --- 仿真参数 ---
+    sim_rate: 30.0            # 仿真频率 (Hz)

soft_arm_sim/launch/simulate.launch.py

@@ -5,14 +5,20 @@ from launch.substitutions import Command
 from launch_ros.actions import Node
 
 def generate_launch_description():
+    # 定义包名，方便后续路径拼接
     pkg_name = 'soft_arm_sim'
+    # 获取安装后的 share 目录路径
     share_dir = get_package_share_directory(pkg_name)
     
-    # 获取 xacro 文件路径
+    # 1. 路径定义
+    # 定位 xacro 模型文件 (用于 robot_state_publisher)
     xacro_file = os.path.join(share_dir, 'urdf', 'soft_arm.urdf.xacro')
+    # 定位 yaml 配置文件 (用于模拟器参数)
+    config_file = os.path.join(share_dir, 'config', 'soft_arm_params.yaml')
 
-    # 1. Robot State Publisher
+    # 2. Robot State Publisher 节点
-    # 使用 Command 进行转换，这样更稳定，且能在终端看到详细报错
+    # 作用：发布静态 TF 树 (如 base_link)，并向 Rviz 提供 robot_description 话题。
+    # Command(['xacro ', xacro_file]) 会在运行时动态解析 xacro 文件生成 XML 字符串。
     robot_description = Command(['xacro ', xacro_file])
     
     node_robot_state_publisher = Node(
@@ -22,20 +28,26 @@ def generate_launch_description():
         parameters=[{'robot_description': robot_description}]
     )
 
-    # 2. 仿真节点
+    # 3. 自定义仿真节点 (Simulator)
+    # 这是我们编写的核心 Python 节点。
     node_simulator = Node(
         package=pkg_name,
-        executable='simulator',
+        executable='simulator',    # setup.py 中 entry_points 定义的可执行文件名
-        output='screen'
+        name='soft_arm_simulator', # 节点名，必须与 yaml 文件中的根键一致，否则参数加载失败
+        output='screen',
+        parameters=[config_file]   # <--- 关键：在这里加载 .yaml 参数文件
     )
     
-    # 3. Rviz2
+    # 4. Rviz2 节点
+    # 启动可视化界面
     node_rviz = Node(
         package='rviz2',
         executable='rviz2',
         name='rviz2',
+        # (可选) 可以在这里添加 arguments=['-d', rviz_config_path] 来加载保存的 rviz 配置
     )
 
+    # 返回 Launch 描述符，ROS 2 会并行启动列表中的所有节点
     return LaunchDescription([
         node_robot_state_publisher,
         node_simulator,

soft_arm_sim/soft_arm_sim/base/simulation_node.py

@@ -2,77 +2,127 @@ import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
 from geometry_msgs.msg import TransformStamped, Point
-from visualization_msgs.msg import Marker, MarkerArray # 引入 MarkerArray
+from visualization_msgs.msg import Marker, MarkerArray
 from std_msgs.msg import Float64MultiArray
 from builtin_interfaces.msg import Time
 import tf2_ros
 import numpy as np
 from scipy.spatial.transform import Rotation
+# 导入第一部分定义的数学类 (假设放在 soft_arm_sim.model 包下)
 from soft_arm_sim.model.pcc_kinematics import SoftArmKinematics
 
 class SoftArmSimulator(Node):
     def __init__(self):
         super().__init__('soft_arm_simulator')
         
-        self.kinematics = SoftArmKinematics()
+        # --- 1. 声明并读取 ROS 参数 ---
+        # 声明参数名及其默认值。这允许我们在不修改代码的情况下通过 yaml 调整机械臂结构。
+        self.declare_parameter('num_sections', 3)          # 分段数
+        self.declare_parameter('section_length', 0.240)    # 每段长度
+        self.declare_parameter('disks_per_section', 3)     # 每段 Disk 数
+        self.declare_parameter('disk_radius', 0.033)       # 物理半径
+        self.declare_parameter('visual_disk_radius', 0.04) # 可视化半径 (通常比物理稍大)
+        self.declare_parameter('visual_disk_thickness', 0.005) # Disk 厚度
+        self.declare_parameter('sim_rate', 30.0)           # 仿真频率 (Hz)
+
+        # 获取参数的实际值
+        self.num_sections = self.get_parameter('num_sections').value
+        self.section_length = self.get_parameter('section_length').value
+        self.disks_per_section = self.get_parameter('disks_per_section').value
+        self.disk_radius_kinematics = self.get_parameter('disk_radius').value
+        self.visual_disk_r = self.get_parameter('visual_disk_radius').value
+        self.visual_disk_h = self.get_parameter('visual_disk_thickness').value
+        sim_rate = self.get_parameter('sim_rate').value
+
+        # --- 2. 使用参数初始化数学模型 ---
+        # 实例化我们在第一部分编写的运动学类
+        self.kinematics = SoftArmKinematics(
+            num_sections=self.num_sections,
+            section_length=self.section_length,
+            disks_per_section=self.disks_per_section,
+            disk_radius=self.disk_radius_kinematics
+        )
         
-        # TF 广播器 (依然保留，为了保证 base_link 存在，且供其他节点使用)
+        # --- 3. 初始化通信接口 ---
+        # TF 广播器：用于发布每个 Disk 的坐标系，让 Rviz 知道它们在哪里
         self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
         
-        # QoS 设置
+        # Marker QoS 设置：对于可视化 Marker，使用 RELIABLE 比较稳妥，防止丢包导致模型闪烁
         marker_qos = QoSProfile(
             depth=1,
             reliability=ReliabilityPolicy.RELIABLE,
             history=HistoryPolicy.KEEP_LAST,
             durability=DurabilityPolicy.VOLATILE
         )
-        
+        # 发布者：发送 MarkerArray (包含所有 disk 和 骨架线)
-        # 注意：这里改成了 MarkerArray
         self.marker_pub = self.create_publisher(MarkerArray, 'soft_arm_visual', marker_qos)
         
+        # 订阅者：接收控制命令
+        # 消息类型 Float64MultiArray，格式预期为 [theta1, phi1, theta2, phi2, ...]
         self.create_subscription(Float64MultiArray, 'soft_arm/command', self.cmd_callback, 10)
         
-        self.current_config = [
+        # --- 4. 动态初始化 current_config ---
-            (0.0, 0.0, 0.24),
+        # current_config 存储当前的机械臂状态。
-            (0.0, 0.0, 0.24),
+        # 格式：[(theta, phi, length), (theta, phi, length), ...]
-            (0.0, 0.0, 0.24)
+        # 初始状态全为 0 (直立)
-        ]
+        self.current_config = []
+        for _ in range(self.num_sections):
+            self.current_config.append((0.0, 0.0, self.section_length))
         
-        # 30Hz
+        # 创建定时器：以固定频率 (sim_rate) 运行 update_loop
-        self.timer = self.create_timer(0.033, self.update_loop)
+        self.timer = self.create_timer(1.0 / sim_rate, self.update_loop)
+        
+        self.get_logger().info(f"Soft Arm initialized with {self.num_sections} sections, rate={sim_rate}Hz")
 
     def cmd_callback(self, msg):
+        """
+        回调函数：处理收到的控制指令。
+        """
         data = msg.data
-        if len(data) >= 6:
+        # 简单校验：确保数据长度足够覆盖所有段。
-            self.current_config[0] = (data[0], data[1], 0.24)
+        # 因为每段需要 2 个控制量 (弯曲角 theta, 偏转角 phi)
-            self.current_config[1] = (data[2], data[3], 0.24)
+        if len(data) >= 2 * self.num_sections:
-            self.current_config[2] = (data[4], data[5], 0.24)
+            for i in range(self.num_sections):
+                idx = i * 2
+                # 更新状态配置。注意：此处假设长度 section_length 是不可变的。
+                # 如果要做伸缩机器人，可以在这里更新第 3 个参数。
+                self.current_config[i] = (data[idx], data[idx+1], self.section_length)
 
     def update_loop(self):
+        """
+        主循环：计算运动学 -> 发布 TF -> 发布 Marker
+        """
+        # 1. 调用数学模型计算所有位置
+        # transforms: 所有 Disk 的 4x4 矩阵
+        # curve_points: 骨架曲线上的点坐标 (用于画线)
         transforms, curve_points = self.kinematics.forward(self.current_config)
         
-        # --- 策略：TF 用真实时间（保住 base_link），Marker 用 0 时间（保住同步）---
-        
         real_time = self.get_clock().now().to_msg()
         
-        # 1. 发布 TF (这是为了系统健全性，不用于显示)
+        # 2. 动态生成 disk 名字
-        disk_names = [
+        # 这种生成方式 (secX_diskY) 必须与 URDF 或后续的控制逻辑对应。
-            "sec1_disk1", "sec1_disk2", "sec1_disk3",
+        disk_names = []
-            "sec2_disk1", "sec2_disk2", "sec2_disk3",
+        for s in range(1, self.num_sections + 1):
-            "sec3_disk1", "sec3_disk2", "sec3_disk3"
+            for d in range(1, self.disks_per_section + 1):
-        ]
+                disk_names.append(f"sec{s}_disk{d}")
+
+        # 3. 发布 TF 变换
+        # 使用 min() 是为了防止数学模型计算出的 transform 数量与预期的 name 数量不一致导致数组越界。
+        count = min(len(transforms), len(disk_names))
         
-        for i, T in enumerate(transforms):
+        for i in range(count):
             t = TransformStamped()
-            t.header.stamp = real_time # 真实时间，保证 base_link 不丢
+            t.header.stamp = real_time
-            t.header.frame_id = "base_link"
+            t.header.frame_id = "base_link"  # 所有变换都是相对于世界/基座的
-            t.child_frame_id = disk_names[i]
+            t.child_frame_id = disk_names[i] # 子坐标系名字
             
-            t.transform.translation.x = T[0, 3]
+            # 填充位置
-            t.transform.translation.y = T[1, 3]
+            t.transform.translation.x = transforms[i][0, 3]
-            t.transform.translation.z = T[2, 3]
+            t.transform.translation.y = transforms[i][1, 3]
+            t.transform.translation.z = transforms[i][2, 3]
             
-            r = Rotation.from_matrix(T[:3, :3])
+            # 填充旋转 (将旋转矩阵转换为四元数)
+            r = Rotation.from_matrix(transforms[i][:3, :3])
             q = r.as_quat()
             t.transform.rotation.x = q[0]
             t.transform.rotation.y = q[1]
@@ -81,45 +131,49 @@ class SoftArmSimulator(Node):
             
             self.tf_broadcaster.sendTransform(t)
 
-        # 2. 发布全套 Marker (Line + Disks)
+        # 4. 发布可视化 Marker
         self.publish_all_visuals(transforms, curve_points)
 
     def publish_all_visuals(self, transforms, curve_points):
+        """
+        辅助函数：构建并发布 MarkerArray 消息，用于在 Rviz 中画出圆盘和线条。
+        """
         marker_array = MarkerArray()
+        zero_time = Time() # Marker 时间戳通常设为 0，表示“最新”
         
-        # 通用设置
+        # --- A. 构建 Disk Markers (圆柱体) ---
-        zero_time = Time() # 强制零时间，立刻渲染
-        
-        # --- A. 创建 Disk Markers (替代 RobotModel) ---
         for i, T in enumerate(transforms):
             disk = Marker()
             disk.header.stamp = zero_time
             disk.header.frame_id = "base_link"
-            disk.ns = "disks"
+            disk.ns = "disks"    # 命名空间
-            disk.id = i + 1 # ID 从 1 开始
+            disk.id = i + 1      # 唯一 ID
-            disk.type = Marker.CYLINDER
+            disk.type = Marker.CYLINDER # 形状：圆柱
-            disk.action = Marker.ADD
+            disk.action = Marker.ADD    # 动作：添加/修改
             
-            # 尺寸 (80mm 直径, 5mm 厚)
+            # 设置尺寸 (来自参数)
-            disk.scale.x = 0.08
+            disk.scale.x = self.visual_disk_r * 2  # 直径 = 半径 * 2
-            disk.scale.y = 0.08
+            disk.scale.y = self.visual_disk_r * 2
-            disk.scale.z = 0.005
+            disk.scale.z = self.visual_disk_h
             
-            # 颜色 (根据段区分)
+            disk.color.a = 1.0 # 不透明度
-            disk.color.a = 1.0
-            if i < 3:   # Sec 1: Red
-                disk.color.r, disk.color.g, disk.color.b = 1.0, 0.0, 0.0
-            elif i < 6: # Sec 2: Green
-                disk.color.r, disk.color.g, disk.color.b = 0.0, 1.0, 0.0
-            else:       # Sec 3: Blue
-                disk.color.r, disk.color.g, disk.color.b = 0.0, 0.0, 1.0
             
-            # 位置
+            # 颜色逻辑：为了区分不同的 PCC 段，给每段设置不同的颜色 (红/绿/蓝 循环)
+            # i // self.disks_per_section 计算当前 disk 属于第几段
+            section_idx = i // self.disks_per_section
+            
+            if section_idx % 3 == 0:
+                disk.color.r, disk.color.g, disk.color.b = 1.0, 0.0, 0.0 # 红
+            elif section_idx % 3 == 1:
+                disk.color.r, disk.color.g, disk.color.b = 0.0, 1.0, 0.0 # 绿
+            else:
+                disk.color.r, disk.color.g, disk.color.b = 0.0, 0.0, 1.0 # 蓝
+            
+            # 设置位姿 (直接使用数学模型计算出的矩阵)
             disk.pose.position.x = T[0, 3]
             disk.pose.position.y = T[1, 3]
             disk.pose.position.z = T[2, 3]
             
-            # 姿态
             r = Rotation.from_matrix(T[:3, :3])
             q = r.as_quat()
             disk.pose.orientation.x = q[0]
@@ -127,28 +181,29 @@ class SoftArmSimulator(Node):
             disk.pose.orientation.z = q[2]
             disk.pose.orientation.w = q[3]
             
+            # lifetime=0 表示永久显示，直到收到新的更新覆盖它
             disk.lifetime.sec = 0
             disk.lifetime.nanosec = 0
             
             marker_array.markers.append(disk)
             
-        # --- B. 创建 Backbone Marker (白色中轴) ---
+        # --- B. 构建 Backbone Marker (中心连线) ---
         line = Marker()
         line.header.stamp = zero_time
         line.header.frame_id = "base_link"
         line.ns = "backbone"
         line.id = 0
-        line.type = Marker.LINE_STRIP
+        line.type = Marker.LINE_STRIP # 形状：线条带
         line.action = Marker.ADD
-        line.scale.x = 0.008
+        line.scale.x = 0.008 # 线条粗细
         line.color.a = 1.0
-        line.color.r = 0.9
+        line.color.r = 0.9   # 浅灰色
         line.color.g = 0.9
         line.color.b = 0.9
         line.lifetime.sec = 0
         line.lifetime.nanosec = 0
         
-        # 填充点
+        # 填充线条的点
         for p_np in curve_points:
             p = Point()
             p.x, p.y, p.z = float(p_np[0]), float(p_np[1]), float(p_np[2])
@@ -156,12 +211,12 @@ class SoftArmSimulator(Node):
             
         marker_array.markers.append(line)
         
-        # 发布
+        # 统一发布
         self.marker_pub.publish(marker_array)
 
 def main(args=None):
     rclpy.init(args=args)
     node = SoftArmSimulator()
-    rclpy.spin(node)
+    rclpy.spin(node) # 保持节点运行，直到按 Ctrl+C
     node.destroy_node()
     rclpy.shutdown()

soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py

@@ -1,47 +1,65 @@
 import numpy as np
 
 class PCCSection:
-    def __init__(self, length=0.240, disk_num=3, disk_radius=0.033):
+    """
-        self.L0 = length
+    PCCSection 类：表示柔性臂的单个独立分段 (Segment)。
-        self.n_disks = disk_num
+    每个分段由若干个 Disk (圆盘) 组成，且假设该段内部曲率恒定。
-        self.d_per_segment = length / disk_num
+    """
+    def __init__(self, length, disk_num, disk_radius):
+        self.L0 = length           # 该段的总长度 (弧长)
+        self.n_disks = disk_num    # 该段包含的 Disk 数量
+        self.r = disk_radius       # Disk 半径 (用于物理计算，此处暂未深度使用)
+        self.d_per_segment = length / disk_num  # 两个 Disk 之间的沿弧长距离
 
     def _get_transform_at_s(self, s, q):
         """
-        计算圆弧上任意位置 s 处的变换矩阵
+        核心函数：计算沿 PCC 曲线 s 处的齐次变换矩阵 T (4x4)。
-        q: [theta, phi, length]
+        
+        参数:
+            s: 沿曲线的弧长位置 (0 <= s <= L0)
+            q: 构型空间坐标 [theta, phi, s_total]
+               theta: 弯曲角度 (决定曲率 k = theta / s_total)
+               phi:   弯曲方向 (绕 Z 轴的偏转角)
+               s_total: 当前段的实际弧长 (通常等于 L0，除非考虑伸缩)
         """
         theta, phi, s_total = q
+        T = np.eye(4)  # 初始化单位矩阵
         
-        T = np.eye(4)
+        # --- 奇异点处理 ---
-        
+        # 当 theta 接近 0 时，机械臂处于直立状态。
-        # 奇异点处理：直线状态
+        # 此时曲率 k -> 0，半径 R -> 无穷大，直接套用 PCC 公式会导致除以零错误。
+        # 因此，直立状态下直接简化为沿 Z 轴的平移。
         if abs(theta) < 1e-6:
             T[0, 3] = 0
             T[1, 3] = 0
-            T[2, 3] = s
+            T[2, 3] = s  # 纯粹沿 Z 轴向上延伸 s 长度
         else:
-            # 常曲率公式
+            # --- PCC 变换公式 ---
-            k = theta / s_total  # 曲率
+            # 1. 曲率 k = theta / s_total
-            # 注意：这里的 s 是当前点在弧上的长度
+            # 2. 变换逻辑：先旋转 phi 到弯曲平面 -> 在平面内弯曲 theta -> 旋转 -phi 回去 (等效推导结果如下)
-            # 当 s > s_total 时（拉伸），我们假设曲率均匀分布在整个 s_total 上
-            # 这里简化处理，直接用 s 计算几何
             
-            ct = np.cos(theta * (s / s_total))
+            k = theta / s_total
-            st = np.sin(theta * (s / s_total))
-            cp = np.cos(phi)
-            sp = np.sin(phi)
             
+            # 预计算三角函数，减少重复计算
+            ct = np.cos(theta * (s / s_total))  # 当前位置的切线角度余弦
+            st = np.sin(theta * (s / s_total))  # 当前位置的切线角度正弦
+            cp = np.cos(phi)                    # 弯曲平面的方位角余弦
+            sp = np.sin(phi)                    # 弯曲平面的方位角正弦
+            
+            # 旋转矩阵 R (3x3)
+            # 描述了当前截面相对于底部的旋转姿态
             R = np.array([
                 [cp*cp*(ct-1)+1, cp*sp*(ct-1), cp*st],
                 [cp*sp*(ct-1),   sp*sp*(ct-1)+1, sp*st],
                 [-cp*st,         -sp*st,         ct]
             ])
             
+            # 位置向量 p (3x1)
+            # 描述了当前截面中心在空间中的坐标 (x, y, z)
             p = np.array([
-                (cp * (1 - ct)) / k,
+                (cp * (1 - ct)) / k,  # x 坐标：在弯曲方向上的投影
-                (sp * (1 - ct)) / k,
+                (sp * (1 - ct)) / k,  # y 坐标
-                st / k
+                st / k                # z 坐标：高度
             ])
             
             T[:3, :3] = R
@@ -50,73 +68,96 @@ class PCCSection:
         return T
 
     def forward_kinematics(self, q):
-        """返回所有 Disk 的变换矩阵 (用于 TF)"""
+        """
+        计算该段内所有 Disk 相对于该段底部的变换矩阵。
+        用于放置可视化模型 (Cylinder)。
+        """
         theta, phi, s_total = q
         transforms = []
+        # 遍历该段内的每一个 Disk (索引从1开始，不包括底座)
         for i in range(1, self.n_disks + 1):
+            # 计算第 i 个 disk 在曲线上的弧长位置
             s = (s_total / self.n_disks) * i
+            # 调用核心公式计算变换
             transforms.append(self._get_transform_at_s(s, q))
         return transforms
 
     def get_curve_points(self, q, num_points=10):
         """
-        返回用于画线的密集点集 (相对坐标)
+        生成用于可视化的骨架曲线点集 (Marker LineStrip)。
-        :param num_points: 这一段生成的点数 (越多越平滑)
+        比 forward_kinematics 采样更密集，使线条看起来更平滑。
         """
         theta, phi, s_total = q
         points = []
-        # 生成从 0 到 L 的密集点
+        # 在 0 到 s_total 之间均匀生成 num_points 个点
         s_values = np.linspace(0, s_total, num_points)
-        
         for s in s_values:
             T = self._get_transform_at_s(s, q)
-            points.append(T[:3, 3]) # 只取位置 xyz
+            points.append(T[:3, 3]) # 只提取位置 (x, y, z)
-            
         return points
 
 class SoftArmKinematics:
-    def __init__(self):
+    """
-        # 3段 PCC
+    SoftArmKinematics 类：多段 PCC 机械臂的高层管理器。
-        self.sections = [
+    负责将多个 PCCSection 串联起来，计算全局坐标。
-            PCCSection(length=0.24, disk_num=3),
+    """
-            PCCSection(length=0.24, disk_num=3),
+    def __init__(self, num_sections=3, section_length=0.24, disks_per_section=3, disk_radius=0.033):
-            PCCSection(length=0.24, disk_num=3)
+        """
-        ]
+        初始化时接收动态参数，支持任意段数和长度的配置。
+        """
+        self.num_sections = num_sections
+        self.sections = []
+        
+        # 根据参数动态生成 Section 对象列表
+        for _ in range(num_sections):
+            self.sections.append(PCCSection(
+                length=section_length,
+                disk_num=disks_per_section,
+                disk_radius=disk_radius
+            ))
 
     def forward(self, joint_configs):
         """
+        计算整个机械臂的全局运动学。
+        
+        参数:
+            joint_configs: 列表，包含每一段的配置 [(theta1, phi1, L1), (theta2, phi2, L2), ...]
+            
         返回:
-        1. transforms: 用于发布 TF (Base -> Disk)
+            all_transforms: 所有 Disk 相对于世界坐标系 (Base) 的变换矩阵列表。
-        2. path_points: 用于发布 Marker (平滑曲线)
+            all_path_points: 整个机械臂骨架曲线的全局坐标点列表。
         """
         all_transforms = []
         all_path_points = []
         
-        T_current_base = np.eye(4) # 当前段基座
+        # T_current_base: 当前段的基坐标系。
+        # 初始为单位矩阵 (机械臂根部在世界原点)
+        T_current_base = np.eye(4)
+        all_path_points.append(T_current_base[:3, 3]) # 添加原点
         
-        # 初始点 (0,0,0)
+        # 安全检查：防止输入的配置数量少于定义的段数 (例如只给了1组数据但有3段)
-        all_path_points.append(T_current_base[:3, 3])
+        limit = min(len(joint_configs), len(self.sections))
         
-        for i, config in enumerate(joint_configs):
+        for i in range(limit):
-            # 1. 计算 TF (Disks)
+            config = joint_configs[i]
-            section_transforms = self.sections[i].forward_kinematics(config)
             
-            # 2. 计算 曲线点 (Marker)
+            # 1. 计算当前段的局部变换 (相对于当前段的底部)
-            # 获取当前段内的局部密集点
+            section_transforms = self.sections[i].forward_kinematics(config)
             local_points = self.sections[i].get_curve_points(config, num_points=15)
             
-            # 将局部点转换到全局坐标系
+            # 2. 坐标系转换：局部 -> 全局
+            # 公式：P_global = T_current_base * P_local
             for p_local in local_points:
-                # 点: p_global = R * p_local + t
                 p_global = T_current_base[:3, :3] @ p_local + T_current_base[:3, 3]
                 all_path_points.append(p_global)
             
-            # 处理 TF 级联
             for T_local in section_transforms:
+                # 矩阵乘法级联变换
                 T_global = T_current_base @ T_local
                 all_transforms.append(T_global)
             
-            # 更新下一段基座
+            # 3. 更新基坐标系
+            # 下一段的底部 = 当前段的末端 (即当前段 transform 列表中的最后一个)
             if section_transforms:
                 T_current_base = all_transforms[-1]