commit d799e49f642667d4f0d94f90c261683afea32e7c Author: mio Date: Wed Jan 14 15:39:02 2026 +0800 initial: upload source code to src branch diff --git a/README.md b/README.md new file mode 100644 index 0000000..df15a80 --- /dev/null +++ b/README.md @@ -0,0 +1,24 @@ +# Soft Arm Sim - ROS 2 柔性机械臂仿真平台 + +这是一个基于 ROS 2 (Humble) 开发的柔性机械臂（Soft Manipulator）仿真程序。 +项目实现了基于常曲率（PCC）假设的运动学模型，包含可视化仿真、运动控制接口，并预留了深度学习与硬件控制的扩展接口。 + +## 🤖 机器人参数 +- **结构**：3 段 PCC (Piecewise Constant Curvature) 串联 +- **每段配置**： + - 长度：240mm (3 disks × 80mm间隔) + - 磁盘直径：80mm + - 绳索孔距：33mm +- **自由度**：配置空间 (Configuration Space) 控制，每段由 `theta` (弯曲角) 和 `phi` (偏转角) 定义。 + +## 📂 文件结构 +```text +src/soft_arm_sim/ +├── soft_arm_sim/ # Python 核心源码 +│ ├── base/ # ROS 2 仿真节点 (TF发布, Marker绘制) +│ ├── control/ # 控制算法 (测试控制器, PID, 甚至 RL) +│ ├── model/ # 纯数学模型 (PCC运动学, 无ROS依赖) +│ └── deeplearning/ # 强化学习环境 (Gym) +├── launch/ # 启动脚本 +├── urdf/ # 机器人视觉模型描述 (Xacro) +└── resource/ # ROS 资源文件 \ No newline at end of file diff --git a/soft_arm_sim/launch/simulate.launch.py b/soft_arm_sim/launch/simulate.launch.py new file mode 100644 index 0000000..3b11191 --- /dev/null +++ b/soft_arm_sim/launch/simulate.launch.py @@ -0,0 +1,43 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import Command +from launch_ros.actions import Node + +def generate_launch_description(): + pkg_name = 'soft_arm_sim' + share_dir = get_package_share_directory(pkg_name) + + # 获取 xacro 文件路径 + xacro_file = os.path.join(share_dir, 'urdf', 'soft_arm.urdf.xacro') + + # 1. Robot State Publisher + # 使用 Command 进行转换，这样更稳定，且能在终端看到详细报错 + robot_description = Command(['xacro ', xacro_file]) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{'robot_description': robot_description}] + ) + + # 2. 仿真节点 + node_simulator = Node( + package=pkg_name, + executable='simulator', + output='screen' + ) + + # 3. Rviz2 + node_rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + ) + + return LaunchDescription([ + node_robot_state_publisher, + node_simulator, + node_rviz + ]) \ No newline at end of file diff --git a/soft_arm_sim/package.xml b/soft_arm_sim/package.xml new file mode 100644 index 0000000..be20101 --- /dev/null +++ b/soft_arm_sim/package.xml @@ -0,0 +1,31 @@ + + + + soft_arm_sim + 0.0.0 + Soft Robot Arm Simulation with 3 PCC sections + mio + TODO: License declaration + + rclpy + std_msgs + geometry_msgs + visualization_msgs + tf2_ros + robot_state_publisher + xacro + rviz2 + + + python3-numpy + python3-scipy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/soft_arm_sim/resource/soft_arm_sim b/soft_arm_sim/resource/soft_arm_sim new file mode 100644 index 0000000..e69de29 diff --git a/soft_arm_sim/setup.cfg b/soft_arm_sim/setup.cfg new file mode 100644 index 0000000..d2338b7 --- /dev/null +++ b/soft_arm_sim/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/soft_arm_sim + +[install] +install_scripts=$base/lib/soft_arm_sim \ No newline at end of file diff --git a/soft_arm_sim/setup.py b/soft_arm_sim/setup.py new file mode 100644 index 0000000..493c8f9 --- /dev/null +++ b/soft_arm_sim/setup.py @@ -0,0 +1,44 @@ +from setuptools import setup, find_packages +import os +from glob import glob + +package_name = 'soft_arm_sim' + +setup( + name=package_name, + version='0.0.0', + # 自动查找所有包含 __init__.py 的子包 + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + + # 1. 安装 launch 文件 + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + + # 2. 安装 urdf 文件 (非常重要，否则 robot_state_publisher 找不到模型) + (os.path.join('share', package_name, 'urdf'), glob('urdf/*')), + + # 3. 配置文件安装 + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='mio', + maintainer_email='user@todo.todo', + description='Soft Robot Arm Simulation', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + # 格式: '可执行文件名 = 包名.路径.文件名:函数名' + + # 仿真核心节点 (对应 base/simulation_node.py 中的 main 函数) + 'simulator = soft_arm_sim.base.simulation_node:main', + + # 测试控制器 (对应 control/test_controller.py 中的 main 函数) + 'sine_controller = soft_arm_sim.control.test_controller:main', + ], + }, +) \ No newline at end of file diff --git a/soft_arm_sim/soft_arm_sim/__init__.py b/soft_arm_sim/soft_arm_sim/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/soft_arm_sim/soft_arm_sim/__pycache__/__init__.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..52c5ac6 Binary files /dev/null and b/soft_arm_sim/soft_arm_sim/__pycache__/__init__.cpython-310.pyc differ diff --git a/soft_arm_sim/soft_arm_sim/base/__init__.py b/soft_arm_sim/soft_arm_sim/base/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/soft_arm_sim/soft_arm_sim/base/__pycache__/__init__.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/base/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..3572a5f Binary files /dev/null and b/soft_arm_sim/soft_arm_sim/base/__pycache__/__init__.cpython-310.pyc differ 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 new file mode 100644 index 0000000..a61a9f5 Binary files /dev/null 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 new file mode 100644 index 0000000..cc7af97 --- /dev/null +++ b/soft_arm_sim/soft_arm_sim/base/simulation_node.py @@ -0,0 +1,167 @@ +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 std_msgs.msg import Float64MultiArray +from builtin_interfaces.msg import Time +import tf2_ros +import numpy as np +from scipy.spatial.transform import Rotation +from soft_arm_sim.model.pcc_kinematics import SoftArmKinematics + +class SoftArmSimulator(Node): + def __init__(self): + super().__init__('soft_arm_simulator') + + self.kinematics = SoftArmKinematics() + + # TF 广播器 (依然保留，为了保证 base_link 存在，且供其他节点使用) + self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) + + # QoS 设置 + marker_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.VOLATILE + ) + + # 注意：这里改成了 MarkerArray + self.marker_pub = self.create_publisher(MarkerArray, 'soft_arm_visual', marker_qos) + + 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) + ] + + # 30Hz + self.timer = self.create_timer(0.033, self.update_loop) + + 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) + + def update_loop(self): + 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" + ] + + for i, T in enumerate(transforms): + t = TransformStamped() + t.header.stamp = real_time # 真实时间，保证 base_link 不丢 + 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] + + r = Rotation.from_matrix(T[:3, :3]) + q = r.as_quat() + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + self.tf_broadcaster.sendTransform(t) + + # 2. 发布全套 Marker (Line + Disks) + self.publish_all_visuals(transforms, curve_points) + + def publish_all_visuals(self, transforms, curve_points): + marker_array = MarkerArray() + + # 通用设置 + 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.id = i + 1 # ID 从 1 开始 + 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.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.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] + disk.pose.orientation.y = q[1] + disk.pose.orientation.z = q[2] + disk.pose.orientation.w = q[3] + + disk.lifetime.sec = 0 + disk.lifetime.nanosec = 0 + + marker_array.markers.append(disk) + + # --- 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.action = Marker.ADD + line.scale.x = 0.008 + line.color.a = 1.0 + 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]) + line.points.append(p) + + marker_array.markers.append(line) + + # 发布 + self.marker_pub.publish(marker_array) + +def main(args=None): + rclpy.init(args=args) + node = SoftArmSimulator() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() \ No newline at end of file diff --git a/soft_arm_sim/soft_arm_sim/control/__init__.py b/soft_arm_sim/soft_arm_sim/control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/soft_arm_sim/soft_arm_sim/control/__pycache__/__init__.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/control/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..fa9e934 Binary files /dev/null and b/soft_arm_sim/soft_arm_sim/control/__pycache__/__init__.cpython-310.pyc differ diff --git a/soft_arm_sim/soft_arm_sim/control/__pycache__/test_controller.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/control/__pycache__/test_controller.cpython-310.pyc new file mode 100644 index 0000000..8874e6d Binary files /dev/null and b/soft_arm_sim/soft_arm_sim/control/__pycache__/test_controller.cpython-310.pyc differ diff --git a/soft_arm_sim/soft_arm_sim/control/test_controller.py b/soft_arm_sim/soft_arm_sim/control/test_controller.py new file mode 100644 index 0000000..88dc7ae --- /dev/null +++ b/soft_arm_sim/soft_arm_sim/control/test_controller.py @@ -0,0 +1,35 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray +import numpy as np + +class SineWaveController(Node): + def __init__(self): + super().__init__('sine_controller') + self.pub = self.create_publisher(Float64MultiArray, 'soft_arm/command', 10) + self.timer = self.create_timer(0.05, self.timer_callback) + self.t = 0.0 + + def timer_callback(self): + msg = Float64MultiArray() + # 让三个段做不同相位的摆动 + # [theta1, phi1, theta2, phi2, theta3, phi3] + theta1 = 0.5 * np.sin(self.t) + 0.5 # 弯曲程度 + phi1 = self.t # 旋转方向 + + theta2 = 0.5 * np.sin(self.t + 1.0) + 0.5 + phi2 = self.t + 1.0 + + theta3 = 0.5 * np.sin(self.t + 2.0) + 0.5 + phi3 = self.t + 2.0 + + msg.data = [theta1, phi1, theta2, phi2, theta3, phi3] + self.pub.publish(msg) + self.t += 0.05 + +def main(args=None): + rclpy.init(args=args) + node = SineWaveController() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() \ No newline at end of file diff --git a/soft_arm_sim/soft_arm_sim/model/__init__.py b/soft_arm_sim/soft_arm_sim/model/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/soft_arm_sim/soft_arm_sim/model/__pycache__/__init__.cpython-310.pyc b/soft_arm_sim/soft_arm_sim/model/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..c6bc038 Binary files /dev/null and b/soft_arm_sim/soft_arm_sim/model/__pycache__/__init__.cpython-310.pyc differ 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 new file mode 100644 index 0000000..1f4f702 Binary files /dev/null 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 new file mode 100644 index 0000000..93e4c34 --- /dev/null +++ b/soft_arm_sim/soft_arm_sim/model/pcc_kinematics.py @@ -0,0 +1,123 @@ +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 + + def _get_transform_at_s(self, s, q): + """ + 计算圆弧上任意位置 s 处的变换矩阵 + q: [theta, phi, length] + """ + theta, phi, s_total = q + + T = np.eye(4) + + # 奇异点处理：直线状态 + if abs(theta) < 1e-6: + T[0, 3] = 0 + T[1, 3] = 0 + T[2, 3] = s + else: + # 常曲率公式 + k = theta / s_total # 曲率 + # 注意：这里的 s 是当前点在弧上的长度 + # 当 s > s_total 时（拉伸），我们假设曲率均匀分布在整个 s_total 上 + # 这里简化处理，直接用 s 计算几何 + + ct = np.cos(theta * (s / s_total)) + st = np.sin(theta * (s / s_total)) + cp = np.cos(phi) + sp = np.sin(phi) + + 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 = np.array([ + (cp * (1 - ct)) / k, + (sp * (1 - ct)) / k, + st / k + ]) + + T[:3, :3] = R + T[:3, 3] = p + + return T + + def forward_kinematics(self, q): + """返回所有 Disk 的变换矩阵 (用于 TF)""" + theta, phi, s_total = q + transforms = [] + for i in range(1, self.n_disks + 1): + 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: 这一段生成的点数 (越多越平滑) + """ + theta, phi, s_total = q + points = [] + # 生成从 0 到 L 的密集点 + 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 + + 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) + ] + + def forward(self, joint_configs): + """ + 返回: + 1. transforms: 用于发布 TF (Base -> Disk) + 2. path_points: 用于发布 Marker (平滑曲线) + """ + all_transforms = [] + all_path_points = [] + + T_current_base = np.eye(4) # 当前段基座 + + # 初始点 (0,0,0) + all_path_points.append(T_current_base[:3, 3]) + + for i, config in enumerate(joint_configs): + # 1. 计算 TF (Disks) + section_transforms = self.sections[i].forward_kinematics(config) + + # 2. 计算 曲线点 (Marker) + # 获取当前段内的局部密集点 + local_points = self.sections[i].get_curve_points(config, num_points=15) + + # 将局部点转换到全局坐标系 + 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) + + # 更新下一段基座 + if section_transforms: + T_current_base = all_transforms[-1] + + return all_transforms, all_path_points \ No newline at end of file diff --git a/soft_arm_sim/urdf/soft_arm.urdf.xacro b/soft_arm_sim/urdf/soft_arm.urdf.xacro new file mode 100644 index 0000000..fe07a90 --- /dev/null +++ b/soft_arm_sim/urdf/soft_arm.urdf.xacro @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file