initial: upload source code to src branch

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 资源文件

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
+    ])

soft_arm_sim/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>soft_arm_sim</name>
+  <version>0.0.0</version>
+  <description>Soft Robot Arm Simulation with 3 PCC sections</description>
+  <maintainer email="user@todo.todo">mio</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>xacro</depend>
+  <depend>rviz2</depend>
+
+  <!-- Python 库依赖 (虽然 colcon 不一定自动安装 pip 库，但写在这里是好习惯) -->
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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

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',
+        ],
+    },
+)

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()

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()

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

soft_arm_sim/urdf/soft_arm.urdf.xacro

@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="soft_arm">
+
+    <xacro:property name="disk_radius" value="0.04" />
+    <xacro:property name="disk_height" value="0.005" />
+    
+    <!-- Base Link -->
+    <link name="base_link">
+        <visual>
+            <geometry>
+                <box size="0.1 0.1 0.02"/>
+            </geometry>
+            <material name="grey">
+                <color rgba="0.5 0.5 0.5 1"/>
+            </material>
+        </visual>
+    </link>
+
+    <!-- Macro for a Disk -->
+    <xacro:macro name="pcc_disk" params="name parent color">
+        <link name="${name}">
+            <visual>
+                <geometry>
+                    <cylinder radius="${disk_radius}" length="${disk_height}"/>
+                </geometry>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <material name="${color}_mat">
+                    <color rgba="${color} 1"/>
+                </material>
+            </visual>
+        </link>
+
+        <!-- ！！！核心修改在这里！！！ -->
+        <!-- 必须添加 Joint，否则 robot_state_publisher 会崩溃 -->
+        <!-- type="floating" 表示这个关节是浮动的，位置由外部 TF 决定 -->
+        <joint name="${name}_joint" type="floating">
+            <parent link="${parent}"/>
+            <child link="${name}"/>
+        </joint>
+    </xacro:macro>
+
+    <!-- Section 1 -->
+    <xacro:pcc_disk name="sec1_disk1" parent="base_link" color="1 0 0"/>
+    <xacro:pcc_disk name="sec1_disk2" parent="base_link" color="1 0 0"/>
+    <xacro:pcc_disk name="sec1_disk3" parent="base_link" color="1 0 0"/>
+    
+    <!-- Section 2 -->
+    <xacro:pcc_disk name="sec2_disk1" parent="base_link" color="0 1 0"/>
+    <xacro:pcc_disk name="sec2_disk2" parent="base_link" color="0 1 0"/>
+    <xacro:pcc_disk name="sec2_disk3" parent="base_link" color="0 1 0"/>
+
+    <!-- Section 3 -->
+    <xacro:pcc_disk name="sec3_disk1" parent="base_link" color="0 0 1"/>
+    <xacro:pcc_disk name="sec3_disk2" parent="base_link" color="0 0 1"/>
+    <xacro:pcc_disk name="sec3_disk3" parent="base_link" color="0 0 1"/>
+
+</robot>