添加了config.yaml，所有参数都从config文件中加载，优化了文件结构
This commit is contained in:
mio
parent
f634d6a421
commit
dc5d46dcae
14
soft_arm_sim/config/config.yaml
Normal file
14
soft_arm_sim/config/config.yaml
Normal file
@ -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)
|
||||
@ -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,
|
||||
|
||||
Binary file not shown.
@ -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()
|
||||
Binary file not shown.
@ -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]
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user