diff --git a/soft_arm_sim/config/config.yaml b/soft_arm_sim/config/config.yaml new file mode 100644 index 0000000..4e0d722 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/soft_arm_sim/launch/simulate.launch.py b/soft_arm_sim/launch/simulate.launch.py index 3b11191..ef49fdd 100644 --- a/soft_arm_sim/launch/simulate.launch.py +++ b/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 - # 使用 Command 进行转换，这样更稳定，且能在终端看到详细报错 + # 2. Robot State Publisher 节点 + # 作用：发布静态 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', - output='screen' + executable='simulator', # setup.py 中 entry_points 定义的可执行文件名 + 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, diff --git a/soft_arm_sim/soft_arm_sim/base/__pycache__/simulation_node.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/base/__pycache__/simulation_node.cpython-310.pyc index a61a9f5..690b092 100644 Binary files a/soft_arm_sim/soft_arm_sim/base/__pycache__/simulation_node.cpython-310.pyc and b/soft_arm_sim/soft_arm_sim/base/__pycache__/simulation_node.cpython-310.pyc differ diff --git a/soft_arm_sim/soft_arm_sim/base/simulation_node.py b/soft_arm_sim/soft_arm_sim/base/simulation_node.py index cc7af97..f587e6c 100644 --- a/soft_arm_sim/soft_arm_sim/base/simulation_node.py +++ b/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 + # 发布者：发送 MarkerArray (包含所有 disk 和 骨架线) 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 = [ - (0.0, 0.0, 0.24), - (0.0, 0.0, 0.24), - (0.0, 0.0, 0.24) - ] + # --- 4. 动态初始化 current_config --- + # current_config 存储当前的机械臂状态。 + # 格式：[(theta, phi, length), (theta, phi, length), ...] + # 初始状态全为 0 (直立) + self.current_config = [] + for _ in range(self.num_sections): + self.current_config.append((0.0, 0.0, self.section_length)) - # 30Hz - self.timer = self.create_timer(0.033, self.update_loop) + # 创建定时器：以固定频率 (sim_rate) 运行 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) - self.current_config[1] = (data[2], data[3], 0.24) - self.current_config[2] = (data[4], data[5], 0.24) + # 简单校验：确保数据长度足够覆盖所有段。 + # 因为每段需要 2 个控制量 (弯曲角 theta, 偏转角 phi) + if len(data) >= 2 * self.num_sections: + 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 (这是为了系统健全性，不用于显示) - disk_names = [ - "sec1_disk1", "sec1_disk2", "sec1_disk3", - "sec2_disk1", "sec2_disk2", "sec2_disk3", - "sec3_disk1", "sec3_disk2", "sec3_disk3" - ] + # 2. 动态生成 disk 名字 + # 这种生成方式 (secX_diskY) 必须与 URDF 或后续的控制逻辑对应。 + disk_names = [] + for s in range(1, self.num_sections + 1): + 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.frame_id = "base_link" - t.child_frame_id = disk_names[i] + t.header.stamp = real_time + t.header.frame_id = "base_link" # 所有变换都是相对于世界/基座的 + t.child_frame_id = disk_names[i] # 子坐标系名字 - t.transform.translation.x = T[0, 3] - t.transform.translation.y = T[1, 3] - t.transform.translation.z = T[2, 3] + # 填充位置 + t.transform.translation.x = transforms[i][0, 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，表示“最新” - # 通用设置 - zero_time = Time() # 强制零时间，立刻渲染 - - # --- A. 创建 Disk Markers (替代 RobotModel) --- + # --- A. 构建 Disk Markers (圆柱体) --- for i, T in enumerate(transforms): disk = Marker() disk.header.stamp = zero_time disk.header.frame_id = "base_link" - disk.ns = "disks" - disk.id = i + 1 # ID 从 1 开始 - disk.type = Marker.CYLINDER - disk.action = Marker.ADD + disk.ns = "disks" # 命名空间 + disk.id = i + 1 # 唯一 ID + disk.type = Marker.CYLINDER # 形状：圆柱 + disk.action = Marker.ADD # 动作：添加/修改 - # 尺寸 (80mm 直径, 5mm 厚) - disk.scale.x = 0.08 - disk.scale.y = 0.08 - disk.scale.z = 0.005 + # 设置尺寸 (来自参数) + disk.scale.x = self.visual_disk_r * 2 # 直径 = 半径 * 2 + disk.scale.y = self.visual_disk_r * 2 + disk.scale.z = self.visual_disk_h - # 颜色 (根据段区分) - 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 + disk.color.a = 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() \ No newline at end of file diff --git a/soft_arm_sim/soft_arm_sim/model/__pycache__/pcc_kinematics.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/model/__pycache__/pcc_kinematics.cpython-310.pyc index 1f4f702..688308d 100644 Binary files a/soft_arm_sim/soft_arm_sim/model/__pycache__/pcc_kinematics.cpython-310.pyc and b/soft_arm_sim/soft_arm_sim/model/__pycache__/pcc_kinematics.cpython-310.pyc differ diff --git a/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py b/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py index 93e4c34..ca3f60d 100644 --- a/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py +++ b/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 - self.n_disks = disk_num - self.d_per_segment = length / disk_num + """ + PCCSection 类：表示柔性臂的单个独立分段 (Segment)。 + 每个分段由若干个 Disk (圆盘) 组成，且假设该段内部曲率恒定。 + """ + 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 处的变换矩阵 - q: [theta, phi, length] + 核心函数：计算沿 PCC 曲线 s 处的齐次变换矩阵 T (4x4)。 + + 参数: + 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: - # 常曲率公式 - k = theta / s_total # 曲率 - # 注意：这里的 s 是当前点在弧上的长度 - # 当 s > s_total 时（拉伸），我们假设曲率均匀分布在整个 s_total 上 - # 这里简化处理，直接用 s 计算几何 + # --- PCC 变换公式 --- + # 1. 曲率 k = theta / s_total + # 2. 变换逻辑：先旋转 phi 到弯曲平面 -> 在平面内弯曲 theta -> 旋转 -phi 回去 (等效推导结果如下) - ct = np.cos(theta * (s / s_total)) - st = np.sin(theta * (s / s_total)) - cp = np.cos(phi) - sp = np.sin(phi) + k = theta / s_total + # 预计算三角函数，减少重复计算 + 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, - (sp * (1 - ct)) / k, - st / k + (cp * (1 - ct)) / k, # x 坐标：在弯曲方向上的投影 + (sp * (1 - ct)) / k, # y 坐标 + 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): """ - 返回用于画线的密集点集 (相对坐标) - :param num_points: 这一段生成的点数 (越多越平滑) + 生成用于可视化的骨架曲线点集 (Marker LineStrip)。 + 比 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 - self.sections = [ - PCCSection(length=0.24, disk_num=3), - PCCSection(length=0.24, disk_num=3), - PCCSection(length=0.24, disk_num=3) - ] + """ + SoftArmKinematics 类：多段 PCC 机械臂的高层管理器。 + 负责将多个 PCCSection 串联起来，计算全局坐标。 + """ + def __init__(self, num_sections=3, section_length=0.24, disks_per_section=3, disk_radius=0.033): + """ + 初始化时接收动态参数，支持任意段数和长度的配置。 + """ + 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) - 2. path_points: 用于发布 Marker (平滑曲线) + all_transforms: 所有 Disk 相对于世界坐标系 (Base) 的变换矩阵列表。 + 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) - all_path_points.append(T_current_base[:3, 3]) + # 安全检查：防止输入的配置数量少于定义的段数 (例如只给了1组数据但有3段) + limit = min(len(joint_configs), len(self.sections)) - for i, config in enumerate(joint_configs): - # 1. 计算 TF (Disks) - section_transforms = self.sections[i].forward_kinematics(config) + for i in range(limit): + config = joint_configs[i] - # 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]