add dual arm motion control

2026-01-11 14:28:13 +08:00
parent 29dbef1cd6
commit 6c02271f49
67 changed files with 3152 additions and 262 deletions
--- a/src/dual_arm_description/CMakeLists.txt
+++ b/src/dual_arm_description/CMakeLists.txt
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.10)
+project(dual_arm_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(xacro REQUIRED)
+find_package(urdf REQUIRED)
+
+# Install directories
+install(DIRECTORY launch meshes urdf
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()
+
--- a/src/dual_arm_description/LICENSE
+++ b/src/dual_arm_description/LICENSE
@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
--- a/src/dual_arm_description/config/joint_names_arms.yaml
+++ b/src/dual_arm_description/config/joint_names_arms.yaml
@@ -0,0 +1 @@
+controller_joint_names: ['', 'left_arm_joint_1', 'left_arm_joint_2', 'left_arm_joint_3', 'left_arm_joint_4', 'left_arm_joint_5', 'left_arm_joint_6', 'right_arm_joint_1', 'right_arm_joint_2', 'right_arm_joint_3', 'right_arm_joint_4', 'right_arm_joint_5', 'right_arm_joint_6', ]
--- a/src/dual_arm_description/launch/display.launch.py
+++ b/src/dual_arm_description/launch/display.launch.py
@@ -0,0 +1,51 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+    # 找到包的路径
+    package_share_dir = FindPackageShare(package='dual_arm_description').find('dual_arm_description')
+    
+    # 声明启动参数
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    urdf_path = os.path.join(package_share_dir, 'urdf', 'dual_arm.urdf')
+    
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'
+        ),
+        
+        # 启动joint_state_publisher_gui
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='joint_state_publisher_gui',
+            output='screen'
+        ),
+        
+        # 启动robot_state_publisher
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{
+                'use_sim_time': use_sim_time,
+                'robot_description': open(urdf_path).read()
+            }]
+        ),
+        
+        # 启动RViz2
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time}]
+        )
+    ])
--- a/src/dual_arm_description/launch/gazebo.launch
+++ b/src/dual_arm_description/launch/gazebo.launch
@@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find arms)/urdf/arms.urdf -urdf -model arms"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>
--- a/src/dual_arm_description/meshes/base_link.STL
+++ b/src/dual_arm_description/meshes/base_link.STL
--- a/src/dual_arm_description/meshes/left_arm_link_1.STL
+++ b/src/dual_arm_description/meshes/left_arm_link_1.STL
--- a/src/dual_arm_description/meshes/left_arm_link_2.STL
+++ b/src/dual_arm_description/meshes/left_arm_link_2.STL
--- a/src/dual_arm_description/meshes/left_arm_link_3.STL
+++ b/src/dual_arm_description/meshes/left_arm_link_3.STL
--- a/src/dual_arm_description/meshes/left_arm_link_4.STL
+++ b/src/dual_arm_description/meshes/left_arm_link_4.STL
--- a/src/dual_arm_description/meshes/left_arm_link_5.STL
+++ b/src/dual_arm_description/meshes/left_arm_link_5.STL
--- a/src/dual_arm_description/meshes/left_arm_link_6.STL
+++ b/src/dual_arm_description/meshes/left_arm_link_6.STL
--- a/src/dual_arm_description/meshes/right_arm_link_1.STL
+++ b/src/dual_arm_description/meshes/right_arm_link_1.STL
--- a/src/dual_arm_description/meshes/right_arm_link_2.STL
+++ b/src/dual_arm_description/meshes/right_arm_link_2.STL
--- a/src/dual_arm_description/meshes/right_arm_link_3.STL
+++ b/src/dual_arm_description/meshes/right_arm_link_3.STL
--- a/src/dual_arm_description/meshes/right_arm_link_4.STL
+++ b/src/dual_arm_description/meshes/right_arm_link_4.STL
--- a/src/dual_arm_description/meshes/right_arm_link_5.STL
+++ b/src/dual_arm_description/meshes/right_arm_link_5.STL
--- a/src/dual_arm_description/meshes/right_arm_link_6.STL
+++ b/src/dual_arm_description/meshes/right_arm_link_6.STL
--- a/src/dual_arm_description/package.xml
+++ b/src/dual_arm_description/package.xml
@@ -0,0 +1,20 @@
+<?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>dual_arm_description</name>
+  <version>1.0.0</version>
+  <description>hivecore dual arm robot description package</description>
+  <maintainer email="Ray@hivecore.cn">Ray Lv</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>xacro</buildtool_depend>
+  
+  <depend>urdf</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>rviz2</depend>
+  
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/dual_arm_description/urdf/dual_arm.csv
+++ b/src/dual_arm_description/urdf/dual_arm.csv
@@ -0,0 +1,14 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,0.00298057124503031,0.000515801514565793,0.0143479775720482,0,0,0,2.38613737991974,0.0300134006922132,-0.000102411277041058,0.00180787987056327,0.0140254000198246,1.62272105684866E-05,0.0277791757738099,0,0,0,0,0,0,package://arms/meshes/base_link.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/base_link.STL,,外壳2-1;右臂-1/大臂法兰1-1;左臂-1/大臂法兰1-1;后盖2-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+left_arm_link_1,-0.00121014292843515,-0.00607985090076274,2.30717195142055E-06,0,0,0,1.01438144565655,0.00177944959424869,-7.74281812735368E-06,1.41839525869718E-08,0.00147694157046928,2.16095534381193E-07,0.00159565402432639,0,0,0,0,0,0,package://arms/meshes/left_arm_link_1.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_1.STL,,左臂-1/大臂法兰2-1;左臂-1/eRob90H××I-XX-18EX_V6_MC2E.DX.XX-2;左臂-1/肩部轴承法兰1-1,left_arm_1,A_left_arm_1,left_arm_joint_1,revolute,0,0.2645,0,0,0,0,base_link,0,-1,0,30,1,-3.14,3.14,,,,,,,,
+left_arm_link_2,0.0473073990020662,0.000240467230209984,-0.124896766185207,0,0,0,3.44815930076977,0.0205425911387401,1.08497424985243E-07,1.58751090790631E-05,0.0221557550613541,3.11670312449268E-05,0.00629782892914115,0,0,0,0,0,0,package://arms/meshes/left_arm_link_2.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_2.STL,,左臂-1/肩部周转法兰3-镜像-1;左臂-1/肩部周转法兰4-1;左臂-1/肩部线缆遮盖-1;左臂-1/肩关节端盖-1;左臂-1/eRob80H××I-XX-18EX_V6_MC2E.DX.XX-1;左臂-1/肘关节盖1-1,left_arm_2,A_left_arm_2,left_arm_joint_2,revolute,0,0,0,0,0,0,left_arm_link_1,1,0,0,30,1,-3.14,3.14,,,,,,,,
+left_arm_link_3,-0.0244216591365906,1.57026866504095E-05,-0.0644017249622112,0,0,0,0.872590806848245,0.00194830010675522,2.54189746127937E-07,0.000582315993376883,0.00211386368849458,-3.77171097146813E-07,0.00113934274062373,0,0,0,0,0,0,package://arms/meshes/left_arm_link_3.STL,1,1,1,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_3.STL,,左臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-1;左臂-1/肘部展折法兰2-1;左臂-1/肘关节盖2-1,left_arm_3,A_left_arm_3,left_arm_joint_3,revolute,0,0,-0.256,0,0,0,left_arm_link_2,-1,0,0,30,1,-3.14,3.14,,,,,,,,
+left_arm_link_4,0.0298883078018623,3.2586123593914E-07,-0.0634039016642955,0,0,0,0.554170739785688,0.000791642003681768,1.82737472933534E-09,0.000163616251225181,0.000796515292662639,-5.09665937788319E-09,0.000529269504583201,0,0,0,0,0,0,package://arms/meshes/left_arm_link_4.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_4.STL,,左臂-1/肘部旋转法兰2-1;左臂-1/腕关节盖2-3,left_arm_4,A_left_arm_4,left_arm_joint_4,revolute,0,0,-0.14,0,0,0,left_arm_link_3,0,0,1,30,1,-3.14,3.14,,,,,,,,
+left_arm_link_5,0.000905144911766077,4.98476898669331E-06,-0.0452958049045105,0,0,0,0.878688377705966,0.00139023048760559,3.08179807647102E-08,-4.43955406467986E-05,0.00135925380280776,-3.78717391068107E-07,0.000904056015468609,0,0,0,0,0,0,package://arms/meshes/left_arm_link_5.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_5.STL,,左臂-1/腕部防护盖-1;左臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-3;左臂-1/腕关节盖1-1;左臂-1/腕部翻转法兰2-1;左臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-2,left_arm_5,A_left_arm_5,left_arm_joint_5,revolute,0,0,-0.08,0,0,0,left_arm_link_4,-1,0,0,30,1,-3.14,3.14,,,,,,,,
+left_arm_link_6,-0.000101887716633362,0.015114071240723,-0.0652860140985222,0,0,0,0.999964109118212,0.000777334341902839,-2.05506130408708E-07,-3.61838630971151E-07,0.00110371590937852,2.93487288693614E-06,0.000974801992678963,0,0,0,0,0,0,package://arms/meshes/left_arm_link_6.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_6.STL,,左臂-1/手臂相机法兰-1;左臂-1/M12堵头-1;左臂-1/奥比支架-1;左臂-1/奥比支架后盖-1;左臂-1/Gemini 335L_20240318-1-1;左臂-1/摄像头前挡板-1;左臂-1/钧舵RG75-300-1,left_arm_6,A_left_arm_6,left_arm_joint_6,revolute,0,0,-0.125,0,0,0,left_arm_link_5,0,0,-1,30,1,-3.14,3.14,,,,,,,,
+right_arm_link_1,-0.00121014259703951,0.00608867124860621,-1.29848221117196E-05,0,0,0,1.01438145710903,0.00177944960401512,8.0553144995043E-06,-3.5960361781722E-07,0.00147672077346284,-1.54892249181004E-07,0.00159587483827316,0,0,0,0,0,0,package://arms/meshes/right_arm_link_1.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_1.STL,,右臂-1/大臂法兰2-1;右臂-1/eRob90H××I-XX-18EX_V6_MC2E.DX.XX-2;右臂-1/肩部轴承法兰1-1,right_arm_1,A_right_arm_1,right_arm_joint_1,revolute,0,-0.264499999954925,0,0,0,0,base_link,0,-1,0,30,1,-3.14,3.14,,,,,,,,
+right_arm_link_2,-0.00050247965429728,-0.00021546614906065,-0.12491520176963,0,0,0,2.06685139029607,0.0108037096742892,-4.71664207475744E-06,-0.0018060013988364,0.0114661489578501,-1.52948974000445E-05,0.0033764723014604,0,0,0,0,0,0,package://arms/meshes/right_arm_link_2.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_2.STL,,右臂-1/肩部周转法兰3-1;右臂-1/肩部线缆遮盖-1;右臂-1/肩部周转法兰4-1;右臂-1/肩关节端盖-1;右臂-1/eRob80H××I-XX-18EX_V6_MC2E.DX.XX-1;右臂-1/肘关节盖1-1,right_arm_2,A_right_arm_2,right_arm_joint_2,revolute,0,0,0,0,0,0,right_arm_link_1,-1,0,0,30,1,-3.14,3.14,,,,,,,,
+right_arm_link_3,-0.0244230615717252,1.69165931350745E-05,-0.0644017249622173,0,0,0,0.87259080684837,0.00194827035859605,3.14401107436852E-07,0.000582351277048671,0.00211389343665385,-4.11685281008291E-07,0.00113934274062382,0,0,0,0,0,0,package://arms/meshes/right_arm_link_3.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_3.STL,,右臂-1/肘关节盖2-2;右臂-1/肘部展折法兰2-1;右臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-1,right_arm_3,A_right_arm_3,right_arm_joint_3,revolute,0,0,-0.256,0,0,0,right_arm_link_2,-1,0,0,30,1,-3.14,3.14,,,,,,,,
+right_arm_link_4,0.0298883078018621,3.25861235550562E-07,-0.0634039016642952,0,0,0,0.554170739785691,0.000791642003681773,1.82737472901328E-09,0.000163616251225182,0.000796515292662645,-5.09665937775888E-09,0.000529269504583203,0,0,0,0,0,0,package://arms/meshes/right_arm_link_4.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_4.STL,,右臂-1/肘部旋转法兰2-1;右臂-1/腕关节盖2-1,right_arm_4,A_right_arm_4,right_arm_joint_4,revolute,0,0,-0.14,0,0,0,right_arm_link_3,0,0,1,30,1,-3.14,3.14,,,,,,,,
+right_arm_link_5,0.000901888305542433,1.25876753304666E-05,-0.0452702924877855,0,0,0,0.878688258547313,0.0013901168763223,2.17076373482529E-07,-4.36500837199152E-05,0.00135937763962618,-4.32056596179085E-07,0.000904045240570538,0,0,0,0,0,0,package://arms/meshes/right_arm_link_5.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_5.STL,,右臂-1/腕部防护盖-1;右臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-2;右臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-3;右臂-1/腕部翻转法兰2-1;右臂-1/腕关节盖1-1,right_arm_5,A_right_arm_5,right_arm_joint_5,revolute,0,0,-0.0799999999999815,0,0,0,right_arm_link_4,-1,0,0,30,1,-3.14,3.14,,,,,,,,
+right_arm_link_6,-0.000111720946994739,-0.015114065193529,-0.0652860148982759,0,0,0,0.999964129684861,0.00077733434761381,-2.05512635353002E-07,-4.5647112991376E-07,0.00110371592448829,-2.93548237473146E-06,0.000974802009102366,0,0,0,0,0,0,package://arms/meshes/right_arm_link_6.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_6.STL,,右臂-1/奥比支架后盖-1;右臂-1/奥比支架-1;右臂-1/Gemini 335L_20240318-1-1;右臂-1/摄像头前挡板-1;右臂-1/手臂相机法兰-1;右臂-1/M12堵头-1;右臂-1/钧舵RG75-300-1,right_arm_6,A_right_arm_6,right_arm_joint_6,revolute,0,0,-0.125,0,0,0,right_arm_link_5,0,0,-1,30,1,-3.14,3.14,,,,,,,,
--- a/src/dual_arm_description/urdf/dual_arm.urdf
+++ b/src/dual_arm_description/urdf/dual_arm.urdf
@@ -0,0 +1,744 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="dual_arm">
+
+  <link
+    name="base_link">
+    <!-- <inertial>
+      <origin
+        xyz="0.00298057124503031 0.000515801514565793 0.0143479775720482"
+        rpy="0 0 0" />
+      <mass
+        value="2.38613737991974" />
+      <inertia
+        ixx="0.01841"
+        ixy="0.000143"
+        ixz="0.000002"
+        iyy="0.050619"
+        iyz="-0.001358"
+        izz="0.052099" />
+    </inertial> -->
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="left_arm_link_1">
+    <inertial>
+      <origin
+        xyz="-0.00121014292843515 -0.00607985090076274 2.30717195142055E-06"
+        rpy="0 0 0" />
+      <mass
+        value="1.01438144565655" />
+      <inertia
+        ixx="0.001976"
+        ixy="-0.000032"
+        ixz="-0.000001"
+        iyy="0.001842"
+        iyz="0.000001"
+        izz="0.002157" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_arm_joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0.2645 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="left_arm_link_1" />
+    <axis
+      xyz="0 -1 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="left_arm_link_2">
+    <inertial>
+      <origin
+        xyz="0.0473073990020662 0.000240467230209984 -0.124896766185207"
+        rpy="0 0 0" />
+      <mass
+        value="3.44815930076977" />
+      <inertia
+        ixx="0.045011"
+        ixy="0.000018"
+        ixz="0.000032"
+        iyy="0.029756"
+        iyz="-0.001086"
+        izz="0.01994" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_arm_joint_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="left_arm_link_1" />
+    <child
+      link="left_arm_link_2" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="left_arm_link_3">
+    <inertial>
+      <origin
+        xyz="-0.0244216591365906 1.57026866504095E-05 -0.0644017249622112"
+        rpy="0 0 0" />
+      <mass
+        value="0.872590806848245" />
+      <inertia
+        ixx="0.002933"
+        ixy="-0.000001"
+        ixz="0.000001"
+        iyy="0.00252"
+        iyz="-0.00095"
+        izz="0.001386" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_arm_joint_3"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.256"
+      rpy="0 0 0" />
+    <parent
+      link="left_arm_link_2" />
+    <child
+      link="left_arm_link_3" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="left_arm_link_4">
+    <inertial>
+      <origin
+        xyz="0.0298883078018623 3.2586123593914E-07 -0.0634039016642955"
+        rpy="0 0 0" />
+      <mass
+        value="0.554170739785688" />
+      <inertia
+        ixx="0.001529"
+        ixy="-0.000001"
+        ixz="0.000001"
+        iyy="0.001395"
+        iyz="0.000066"
+        izz="0.000659" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_arm_joint_4"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.14"
+      rpy="0 0 0" />
+    <parent
+      link="left_arm_link_3" />
+    <child
+      link="left_arm_link_4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="left_arm_link_5">
+    <inertial>
+      <origin
+        xyz="0.000905144911766077 4.98476898669331E-06 -0.0452958049045105"
+        rpy="0 0 0" />
+      <mass
+        value="0.878688377705965" />
+      <inertia
+        ixx="0.002476"
+        ixy="-0.000001"
+        ixz="0.000001"
+        iyy="0.002441"
+        iyz="0.000035"
+        izz="0.000970" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_arm_joint_5"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.08"
+      rpy="0 0 0" />
+    <parent
+      link="left_arm_link_4" />
+    <child
+      link="left_arm_link_5" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="left_arm_link_6">
+    <inertial>
+      <origin
+        xyz="-0.000101887716633362 0.015114071240723 -0.0652860140985222"
+        rpy="0 0 0" />
+      <mass
+        value="1.908709" />
+      <inertia
+        ixx="0.006191"
+        ixy="0.000018"
+        ixz="-0.001104"
+        iyy="0.006720"
+        iyz="0.000001"
+        izz="0.002820" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/left_arm_link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_arm_joint_6"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.125"
+      rpy="0 0 0" />
+    <parent
+      link="left_arm_link_5" />
+    <child
+      link="left_arm_link_6" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="right_arm_link_1">
+    <inertial>
+      <origin
+        xyz="-0.00121014259703951 0.00608867124860621 -1.29848221117196E-05"
+        rpy="0 0 0" />
+      <mass
+        value="1.01438145710903" />
+      <inertia
+        ixx="0.001976"
+        ixy="0.000032"
+        ixz="-0.000001"
+        iyy="0.001842"
+        iyz="0.000001"
+        izz="0.002157" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_arm_joint_1"
+    type="revolute">
+    <origin
+      xyz="0 -0.264499999954925 0"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="right_arm_link_1" />
+    <axis
+      xyz="0 -1 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="right_arm_link_2">
+    <inertial>
+      <origin
+        xyz="-0.00050247965429728 -0.00021546614906065 -0.12491520176963"
+        rpy="0 0 0" />
+      <mass
+        value="3.44815930076977" />
+      <inertia
+        ixx="0.045011"
+        ixy="0.000018"
+        ixz="0.000032"
+        iyy="0.029756"
+        iyz="-0.001086"
+        izz="0.01994" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_arm_joint_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="right_arm_link_1" />
+    <child
+      link="right_arm_link_2" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="right_arm_link_3">
+    <inertial>
+      <origin
+        xyz="-0.0244230615717252 1.69165931350745E-05 -0.0644017249622173"
+        rpy="0 0 0" />
+      <mass
+        value="0.87259080684837" />
+      <inertia
+        ixx="0.002933"
+        ixy="-0.000001"
+        ixz="0.000001"
+        iyy="0.00252"
+        iyz="-0.00095"
+        izz="0.001386" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_arm_joint_3"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.256"
+      rpy="0 0 0" />
+    <parent
+      link="right_arm_link_2" />
+    <child
+      link="right_arm_link_3" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="right_arm_link_4">
+    <inertial>
+      <origin
+        xyz="0.0298883078018621 3.25861235550562E-07 -0.0634039016642952"
+        rpy="0 0 0" />
+      <mass
+        value="0.554170739785691" />
+      <inertia
+        ixx="0.001529"
+        ixy="-0.000001"
+        ixz="0.000001"
+        iyy="0.001395"
+        iyz="0.000066"
+        izz="0.000659" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_arm_joint_4"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.14"
+      rpy="0 0 0" />
+    <parent
+      link="right_arm_link_3" />
+    <child
+      link="right_arm_link_4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="right_arm_link_5">
+    <inertial>
+      <origin
+        xyz="0.000901888305542433 1.25876753304666E-05 -0.0452702924877855"
+        rpy="0 0 0" />
+      <mass
+        value="0.878688258547313" />
+      <inertia
+        ixx="0.002476"
+        ixy="-0.000001"
+        ixz="0.000001"
+        iyy="0.002441"
+        iyz="0.000035"
+        izz="0.000970" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_arm_joint_5"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.0799999999999815"
+      rpy="0 0 0" />
+    <parent
+      link="right_arm_link_4" />
+    <child
+      link="right_arm_link_5" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+  <link
+    name="right_arm_link_6">
+    <inertial>
+      <origin
+        xyz="-0.000111720946994739 -0.015114065193529 -0.0652860148982759"
+        rpy="0 0 0" />
+      <mass
+        value="1.908709" />
+      <inertia
+        ixx="0.006191"
+        ixy="0.000018"
+        ixz="-0.001104"
+        iyy="0.006720"
+        iyz="0.000001"
+        izz="0.002820" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://dual_arm_description/meshes/right_arm_link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_arm_joint_6"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.125"
+      rpy="0 0 0" />
+    <parent
+      link="right_arm_link_5" />
+    <child
+      link="right_arm_link_6" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-3.14"
+      upper="3.14"
+      effort="30.0"
+      velocity="1.0" />
+  </joint>
+</robot>
--- a/src/dual_arm_moveit_config/.setup_assistant
+++ b/src/dual_arm_moveit_config/.setup_assistant
@@ -0,0 +1,25 @@
+moveit_setup_assistant_config:
+  urdf:
+    package: dual_arm_description
+    relative_path: urdf/dual_arm.urdf
+  srdf:
+    relative_path: config/dual_arm.srdf
+  package_settings:
+    author_name: ray
+    author_email: ray@hivecore.cn
+    generated_timestamp: 1764123395
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
+  modified_urdf:
+    xacros:
+      - control_xacro
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
--- a/src/dual_arm_moveit_config/CMakeLists.txt
+++ b/src/dual_arm_moveit_config/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.22)
+project(dual_arm_moveit_config)
+
+find_package(ament_cmake REQUIRED)
+
+ament_package()
+
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
+  install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+    PATTERN "setup_assistant.launch" EXCLUDE)
+endif()
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
--- a/src/dual_arm_moveit_config/config/dual_arm.ros2_control.xacro
+++ b/src/dual_arm_moveit_config/config/dual_arm.ros2_control.xacro
@@ -0,0 +1,98 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="dual_arm_ros2_control" params="name initial_positions_file">
+        <xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
+
+        <ros2_control name="${name}" type="system">
+            <hardware>
+                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
+                <plugin>mock_components/GenericSystem</plugin>
+            </hardware>
+            <joint name="left_arm_joint_1">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['left_arm_joint_1']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="left_arm_joint_2">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['left_arm_joint_2']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="left_arm_joint_3">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['left_arm_joint_3']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="left_arm_joint_4">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['left_arm_joint_4']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="left_arm_joint_5">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['left_arm_joint_5']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="left_arm_joint_6">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['left_arm_joint_6']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="right_arm_joint_1">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['right_arm_joint_1']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="right_arm_joint_2">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['right_arm_joint_2']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="right_arm_joint_3">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['right_arm_joint_3']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="right_arm_joint_4">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['right_arm_joint_4']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="right_arm_joint_5">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['right_arm_joint_5']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="right_arm_joint_6">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['right_arm_joint_6']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+
+        </ros2_control>
+    </xacro:macro>
+</robot>
--- a/src/dual_arm_moveit_config/config/dual_arm.srdf
+++ b/src/dual_arm_moveit_config/config/dual_arm.srdf
@@ -0,0 +1,134 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="dual_arm">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="arm_left">
+        <chain base_link="base_link" tip_link="left_arm_link_6"/>
+    </group>
+    <group name="arm_right">
+        <chain base_link="base_link" tip_link="right_arm_link_6"/>
+    </group>
+    <group name="dual_arm">
+        <group name="arm_left"/>
+        <group name="arm_right"/>
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="arm_left_home_pose" group="arm_left">
+        <joint name="left_arm_joint_1" value="0"/>
+        <joint name="left_arm_joint_2" value="0"/>
+        <joint name="left_arm_joint_3" value="0"/>
+        <joint name="left_arm_joint_4" value="0"/>
+        <joint name="left_arm_joint_5" value="0"/>
+        <joint name="left_arm_joint_6" value="0"/>
+    </group_state>
+    <group_state name="arm_right_home_pose" group="arm_right">
+        <joint name="right_arm_joint_1" value="0"/>
+        <joint name="right_arm_joint_2" value="0"/>
+        <joint name="right_arm_joint_3" value="0"/>
+        <joint name="right_arm_joint_4" value="0"/>
+        <joint name="right_arm_joint_5" value="0"/>
+        <joint name="right_arm_joint_6" value="0"/>
+    </group_state>
+    <group_state name="dual_arm_home_pose" group="dual_arm">
+        <joint name="left_arm_joint_1" value="0"/>
+        <joint name="left_arm_joint_2" value="0"/>
+        <joint name="left_arm_joint_3" value="0"/>
+        <joint name="left_arm_joint_4" value="0"/>
+        <joint name="left_arm_joint_5" value="0"/>
+        <joint name="left_arm_joint_6" value="0"/>
+        <joint name="right_arm_joint_1" value="0"/>
+        <joint name="right_arm_joint_2" value="0"/>
+        <joint name="right_arm_joint_3" value="0"/>
+        <joint name="right_arm_joint_4" value="0"/>
+        <joint name="right_arm_joint_5" value="0"/>
+        <joint name="right_arm_joint_6" value="0"/>
+    </group_state>
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="base_joint" type="fixed" parent_frame="world_frame" child_link="base_link"/>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="base_link" link2="left_arm_link_1" reason="User"/>
+    <disable_collisions link1="base_link" link2="left_arm_link_2" reason="User"/>
+    <disable_collisions link1="base_link" link2="left_arm_link_3" reason="User"/>
+    <disable_collisions link1="base_link" link2="left_arm_link_4" reason="User"/>
+    <disable_collisions link1="base_link" link2="left_arm_link_5" reason="User"/>
+    <disable_collisions link1="base_link" link2="left_arm_link_6" reason="User"/>
+    <disable_collisions link1="base_link" link2="right_arm_link_1" reason="User"/>
+    <disable_collisions link1="base_link" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="base_link" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="base_link" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="base_link" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="base_link" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="left_arm_link_2" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="left_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="left_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="left_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="left_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="right_arm_link_1" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_1" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="left_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="left_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="left_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="left_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="right_arm_link_1" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_2" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="left_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="left_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="left_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="right_arm_link_1" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_3" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="left_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="left_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="right_arm_link_1" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_4" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_5" link2="left_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_5" link2="right_arm_link_1" reason="User"/>
+    <disable_collisions link1="left_arm_link_5" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="left_arm_link_5" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_5" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_5" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_5" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="left_arm_link_6" link2="right_arm_link_1" reason="User"/>
+    <disable_collisions link1="left_arm_link_6" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="left_arm_link_6" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="left_arm_link_6" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="left_arm_link_6" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="left_arm_link_6" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="right_arm_link_1" link2="right_arm_link_2" reason="User"/>
+    <disable_collisions link1="right_arm_link_1" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="right_arm_link_1" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="right_arm_link_1" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="right_arm_link_1" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="right_arm_link_2" link2="right_arm_link_3" reason="User"/>
+    <disable_collisions link1="right_arm_link_2" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="right_arm_link_2" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="right_arm_link_2" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="right_arm_link_3" link2="right_arm_link_4" reason="User"/>
+    <disable_collisions link1="right_arm_link_3" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="right_arm_link_3" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="right_arm_link_4" link2="right_arm_link_5" reason="User"/>
+    <disable_collisions link1="right_arm_link_4" link2="right_arm_link_6" reason="User"/>
+    <disable_collisions link1="right_arm_link_5" link2="right_arm_link_6" reason="User"/>
+</robot>
--- a/src/dual_arm_moveit_config/config/dual_arm.urdf.xacro
+++ b/src/dual_arm_moveit_config/config/dual_arm.urdf.xacro
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="dual_arm">
+    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
+
+    <!-- Import dual_arm urdf file -->
+    <xacro:include filename="$(find dual_arm_description)/urdf/dual_arm.urdf" />
+
+    <!-- Import control_xacro -->
+    <xacro:include filename="dual_arm.ros2_control.xacro" />
+
+
+    <xacro:dual_arm_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
+
+</robot>
--- a/src/dual_arm_moveit_config/config/initial_positions.yaml
+++ b/src/dual_arm_moveit_config/config/initial_positions.yaml
@@ -0,0 +1,15 @@
+# Default initial positions for dual_arm's ros2_control fake system
+
+initial_positions:
+  left_arm_joint_1: 0
+  left_arm_joint_2: 0
+  left_arm_joint_3: 0
+  left_arm_joint_4: 0
+  left_arm_joint_5: 0
+  left_arm_joint_6: 0
+  right_arm_joint_1: 0
+  right_arm_joint_2: 0
+  right_arm_joint_3: 0
+  right_arm_joint_4: 0
+  right_arm_joint_5: 0
+  right_arm_joint_6: 0
--- a/src/dual_arm_moveit_config/config/joint_limits.yaml
+++ b/src/dual_arm_moveit_config/config/joint_limits.yaml
@@ -0,0 +1,70 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  left_arm_joint_1:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  left_arm_joint_2:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  left_arm_joint_3:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  left_arm_joint_4:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  left_arm_joint_5:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  left_arm_joint_6:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  right_arm_joint_1:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  right_arm_joint_2:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  right_arm_joint_3:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  right_arm_joint_4:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  right_arm_joint_5:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
+  right_arm_joint_6:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: false
+    max_acceleration: 0
--- a/src/dual_arm_moveit_config/config/kinematics.yaml
+++ b/src/dual_arm_moveit_config/config/kinematics.yaml
@@ -0,0 +1,8 @@
+arm_left:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_timeout: 0.0050000000000000001
+arm_right:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_timeout: 0.0050000000000000001
--- a/src/dual_arm_moveit_config/config/moveit.rviz
+++ b/src/dual_arm_moveit_config/config/moveit.rviz
@@ -0,0 +1,51 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MotionPlanning1
+  - Class: rviz_common/Help
+    Name: Help
+  - Class: rviz_common/Views
+    Name: Views
+Visualization Manager:
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Value: true
+    - Class: moveit_rviz_plugin/MotionPlanning
+      Name: MotionPlanning
+      Planned Path:
+        Loop Animation: true
+        State Display Time: 0.05 s
+        Trajectory Topic: display_planned_path
+      Planning Scene Topic: monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+      Scene Robot:
+        Robot Alpha: 0.5
+      Value: true
+  Global Options:
+    Fixed Frame: base_link
+  Tools:
+    - Class: rviz_default_plugins/Interact
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.0
+      Focal Point:
+        X: -0.1
+        Y: 0.25
+        Z: 0.30
+      Name: Current View
+      Pitch: 0.5
+      Target Frame: base_link
+      Yaw: -0.623
+Window Geometry:
+  Height: 975
+  QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Width: 1200
--- a/src/dual_arm_moveit_config/config/moveit_controllers.yaml
+++ b/src/dual_arm_moveit_config/config/moveit_controllers.yaml
@@ -0,0 +1,47 @@
+# MoveIt uses this configuration for controller management
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - arm_left_controller
+    - arm_right_controller
+    # - dual_arm_controller
+
+  arm_left_controller:
+    type: FollowJointTrajectory
+    joints:
+      - left_arm_joint_1
+      - left_arm_joint_2
+      - left_arm_joint_3
+      - left_arm_joint_4
+      - left_arm_joint_5
+      - left_arm_joint_6
+    action_ns: follow_joint_trajectory
+    default: true
+  arm_right_controller:
+    type: FollowJointTrajectory
+    joints:
+      - right_arm_joint_1
+      - right_arm_joint_2
+      - right_arm_joint_3
+      - right_arm_joint_4
+      - right_arm_joint_5
+      - right_arm_joint_6
+    action_ns: follow_joint_trajectory
+    default: false
+  # dual_arm_controller:
+  #   type: FollowJointTrajectory
+  #   joints:
+  #     - left_arm_joint_1
+  #     - left_arm_joint_2
+  #     - left_arm_joint_3
+  #     - left_arm_joint_4
+  #     - left_arm_joint_5
+  #     - left_arm_joint_6
+  #     - right_arm_joint_1
+  #     - right_arm_joint_2
+  #     - right_arm_joint_3
+  #     - right_arm_joint_4
+  #     - right_arm_joint_5
+  #     - right_arm_joint_6
--- a/src/dual_arm_moveit_config/config/pilz_cartesian_limits.yaml
+++ b/src/dual_arm_moveit_config/config/pilz_cartesian_limits.yaml
@@ -0,0 +1,6 @@
+# Limits for the Pilz planner
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57
--- a/src/dual_arm_moveit_config/config/ros2_controllers.yaml
+++ b/src/dual_arm_moveit_config/config/ros2_controllers.yaml
@@ -0,0 +1,68 @@
+# This config file is used by ros2_control
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    arm_left_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+    arm_right_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+    # dual_arm_controller:
+    #   type: joint_trajectory_controller/JointTrajectoryController
+
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+arm_left_controller:
+  ros__parameters:
+    joints:
+      - left_arm_joint_1
+      - left_arm_joint_2
+      - left_arm_joint_3
+      - left_arm_joint_4
+      - left_arm_joint_5
+      - left_arm_joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+arm_right_controller:
+  ros__parameters:
+    joints:
+      - right_arm_joint_1
+      - right_arm_joint_2
+      - right_arm_joint_3
+      - right_arm_joint_4
+      - right_arm_joint_5
+      - right_arm_joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+# dual_arm_controller:
+#   ros__parameters:
+#     joints:
+#       - left_arm_joint_1
+#       - left_arm_joint_2
+#       - left_arm_joint_3
+#       - left_arm_joint_4
+#       - left_arm_joint_5
+#       - left_arm_joint_6
+#       - right_arm_joint_1
+#       - right_arm_joint_2
+#       - right_arm_joint_3
+#       - right_arm_joint_4
+#       - right_arm_joint_5
+#       - right_arm_joint_6
+#     command_interfaces:
+#       - position
+#     state_interfaces:
+#       - position
+#       - velocity
--- a/src/dual_arm_moveit_config/config/sensors_3d.yaml
+++ b/src/dual_arm_moveit_config/config/sensors_3d.yaml
@@ -0,0 +1,26 @@
+sensors:
+  []
+  # - default_sensor
+  # - kinect_depthimage
+# default_sensor:
+#   far_clipping_plane_distance: 5.0
+#   filtered_cloud_topic: filtered_cloud
+#   image_topic: /head_mount_kinect/depth_registered/image_raw
+#   max_update_rate: 1.0
+#   near_clipping_plane_distance: 0.3
+#   padding_offset: 0.03
+#   padding_scale: 4.0
+#   queue_size: 5
+#   sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+#   shadow_threshold: 0.2
+# kinect_depthimage:
+#   far_clipping_plane_distance: 5.0
+#   filtered_cloud_topic: filtered_cloud
+#   image_topic: /head_mount_kinect/depth_registered/image_raw
+#   max_update_rate: 1.0
+#   near_clipping_plane_distance: 0.3
+#   padding_offset: 0.03
+#   padding_scale: 4.0
+#   queue_size: 5
+#   sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+#   shadow_threshold: 0.2
--- a/src/dual_arm_moveit_config/launch/demo.launch.py
+++ b/src/dual_arm_moveit_config/launch/demo.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_demo_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_demo_launch(moveit_config)
--- a/src/dual_arm_moveit_config/launch/move_group.launch.py
+++ b/src/dual_arm_moveit_config/launch/move_group.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_move_group_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_move_group_launch(moveit_config)
--- a/src/dual_arm_moveit_config/launch/move_group_with_rviz.launch.py
+++ b/src/dual_arm_moveit_config/launch/move_group_with_rviz.launch.py
@@ -0,0 +1,31 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch
+from launch import LaunchDescription
+
+
+def generate_launch_description():
+    """
+    Launch file for MoveIt move_group node with RViz visualization.
+    
+    This launch file starts both the move_group node and RViz for visualization
+    and debugging. Use this when you need to visualize robot motion planning
+    and execution in RViz.
+    
+    Usage:
+        ros2 launch dual_arm_moveit_config move_group_with_rviz.launch.py
+    """
+    # Build MoveIt configuration
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    
+    # Generate move_group launch description
+    move_group_launch_description = generate_move_group_launch(moveit_config)
+    
+    # Generate RViz launch description  
+    rviz_launch_description = generate_moveit_rviz_launch(moveit_config)
+    
+    # Combine both launch descriptions
+    return LaunchDescription([
+        move_group_launch_description,
+        rviz_launch_description
+    ])
+
--- a/src/dual_arm_moveit_config/launch/moveit_rviz.launch.py
+++ b/src/dual_arm_moveit_config/launch/moveit_rviz.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_moveit_rviz_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_moveit_rviz_launch(moveit_config)
--- a/src/dual_arm_moveit_config/launch/rsp.launch.py
+++ b/src/dual_arm_moveit_config/launch/rsp.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_rsp_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_rsp_launch(moveit_config)
--- a/src/dual_arm_moveit_config/launch/setup_assistant.launch.py
+++ b/src/dual_arm_moveit_config/launch/setup_assistant.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_setup_assistant_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_setup_assistant_launch(moveit_config)
--- a/src/dual_arm_moveit_config/launch/spawn_controllers.launch.py
+++ b/src/dual_arm_moveit_config/launch/spawn_controllers.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_spawn_controllers_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_spawn_controllers_launch(moveit_config)
--- a/src/dual_arm_moveit_config/launch/static_virtual_joint_tfs.launch.py
+++ b/src/dual_arm_moveit_config/launch/static_virtual_joint_tfs.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_static_virtual_joint_tfs_launch(moveit_config)
--- a/src/dual_arm_moveit_config/launch/warehouse_db.launch.py
+++ b/src/dual_arm_moveit_config/launch/warehouse_db.launch.py
@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_warehouse_db_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
+    return generate_warehouse_db_launch(moveit_config)
--- a/src/dual_arm_moveit_config/package.xml
+++ b/src/dual_arm_moveit_config/package.xml
@@ -0,0 +1,52 @@
+<?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>dual_arm_moveit_config</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the dual_arm with the MoveIt Motion Planning Framework
+  </description>
+  <maintainer email="ray@hivecore.cn">ray</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit2</url>
+
+  <author email="ray@hivecore.cn">ray</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_kinematics</exec_depend>
+  <exec_depend>moveit_planners</exec_depend>
+  <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+       We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <exec_depend>joint_trajectory_controller</exec_depend> -->
+  <!-- <exec_depend>gazebo_ros_control</exec_depend> -->
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>dual_arm_description</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>moveit_ros_warehouse</exec_depend>
+  <exec_depend>moveit_setup_assistant</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rviz_common</exec_depend>
+  <exec_depend>rviz_default_plugins</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>warehouse_ros_mongo</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+
+  <export>
+      <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/robot_control/CMakeLists.txt
+++ b/src/robot_control/CMakeLists.txt
@@ -20,6 +20,9 @@ find_package(control_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(action_msgs REQUIRED) 
 find_package(interfaces REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_msgs REQUIRED)

 # 头文件目录
 include_directories(
@@ -33,6 +36,7 @@ set(SOURCES
  src/core/robot_control_node.cpp
  src/core/robot_control_manager.cpp
  src/core/controller_factory.cpp
+  src/core/remote_controller.cpp
  src/actions/action_manager.cpp
  src/controllers/control_base.cpp
  src/controllers/leg_control.cpp
@@ -58,12 +62,29 @@ ament_target_dependencies(robot_control_node
  control_msgs
  rclcpp_action
  interfaces
+  moveit_core
+  moveit_ros_planning_interface
+  moveit_msgs
 )

+# 构建 right_arm_joint_test 可执行文件
+add_executable(right_arm_joint_test
+  src/utils/right_arm_joint_test.cpp
+)
+
+# 链接ROS依赖
+ament_target_dependencies(right_arm_joint_test
+  rclcpp
+  rclcpp_action
+  moveit_core
+  moveit_ros_planning_interface
+  control_msgs
+)

 # 安装可执行文件和launch目录
 install(TARGETS
  robot_control_node
+  right_arm_joint_test
  DESTINATION lib/${PROJECT_NAME})

 install(DIRECTORY launch
--- a/src/robot_control/include/actions/action_manager.hpp
+++ b/src/robot_control/include/actions/action_manager.hpp
@@ -16,6 +16,7 @@
 #include <thread>
 #include <atomic>
 #include <functional>
+#include <mutex>
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"

@@ -23,6 +24,7 @@
 #include "interfaces/action/move_leg.hpp"
 #include "interfaces/action/move_waist.hpp"
 #include "interfaces/action/move_wheel.hpp"
+#include "interfaces/action/dual_arm.hpp"
 #include "interfaces/msg/motor_cmd.hpp"
 #include "interfaces/srv/motor_param.hpp"

@@ -38,6 +40,9 @@ using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
 using MoveWheel = interfaces::action::MoveWheel;
 using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;

+using DualArm = interfaces::action::DualArm;
+using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;
+
 using MotorCmd = interfaces::msg::MotorCmd;
 using MotorParam = interfaces::srv::MotorParam;

@@ -55,6 +60,7 @@ namespace robot_control
     * - MoveLeg: 腿部运动动作
     * - MoveWaist: 腰部运动动作
     * - MoveWheel: 轮子运动动作
+     * - DualArm: 双臂运动动作
     */
    class ActionManager
    {
@@ -109,6 +115,12 @@ namespace robot_control
         * @return true 正在执行，false 未执行
         */
        bool is_move_wheel_executing() const { return move_wheel_executing_.load(); }
+        
+        /**
+         * @brief 检查DualArm是否正在执行
+         * @return true 正在执行，false 未执行
+         */
+        bool is_dual_arm_executing() const { return dual_arm_executing_.load(); }

        // ==================== 执行状态设置接口 ====================
        
@@ -135,6 +147,12 @@ namespace robot_control
         * @param value true 设置为执行中，false 设置为未执行
         */
        void set_move_wheel_executing(bool value) { move_wheel_executing_.store(value); }
+        
+        /**
+         * @brief 设置DualArm执行状态
+         * @param value true 设置为执行中，false 设置为未执行
+         */
+        void set_dual_arm_executing(bool value) { dual_arm_executing_.store(value); }

    private:
        // ==================== MoveHome Action处理函数 ====================
@@ -199,6 +217,16 @@ namespace robot_control
        void handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
        void move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);

+        // ==================== DualArm Action处理函数 ====================
+        
+        rclcpp_action::GoalResponse handle_dual_arm_goal(
+            const rclcpp_action::GoalUUID& uuid,
+            std::shared_ptr<const DualArm::Goal> goal);
+        rclcpp_action::CancelResponse handle_dual_arm_cancel(
+            const std::shared_ptr<GoalHandleDualArm> goal_handle);
+        void handle_dual_arm_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle);
+        void dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> goal_handle);
+
        // ==================== 成员变量 ====================
        
        rclcpp::Node* node_;                              ///< ROS 2节点指针
@@ -212,11 +240,13 @@ namespace robot_control
        rclcpp_action::Server<MoveLeg>::SharedPtr move_leg_action_server_;     ///< MoveLeg动作服务器
        rclcpp_action::Server<MoveWaist>::SharedPtr move_waist_action_server_; ///< MoveWaist动作服务器
        rclcpp_action::Server<MoveWheel>::SharedPtr move_wheel_action_server_; ///< MoveWheel动作服务器
+        rclcpp_action::Server<DualArm>::SharedPtr dual_arm_action_server_;     ///< DualArm动作服务器

        // Action执行状态标志（使用atomic确保线程安全）
        std::atomic<bool> move_home_executing_;   ///< MoveHome执行状态
        std::atomic<bool> move_leg_executing_;    ///< MoveLeg执行状态
        std::atomic<bool> move_waist_executing_;  ///< MoveWaist执行状态
        std::atomic<bool> move_wheel_executing_;  ///< MoveWheel执行状态
+        std::atomic<bool> dual_arm_executing_;    ///< DualArm执行状态
    };
 }
--- a/src/robot_control/include/controllers/arm_control.hpp
+++ b/src/robot_control/include/controllers/arm_control.hpp
@@ -8,6 +8,27 @@
 #pragma once

 #include "controllers/control_base.hpp"
+#include <memory>
+#include <string>
+#include <vector>
+#include "geometry_msgs/msg/pose.hpp"
+
+// 前向声明
+namespace rclcpp {
+    class Node;
+}
+
+namespace moveit {
+    namespace planning_interface {
+        class MoveGroupInterface;
+    }
+}
+
+namespace moveit {
+    namespace planning_interface {
+        class MoveGroupInterface;
+    }
+}

 namespace robot_control
 {
@@ -16,7 +37,7 @@ namespace robot_control
     * @brief 手臂控制器类
     * 
     * 继承自ControlBase，提供手臂特定的控制功能。
-     * 当前为占位实现，可根据需要扩展功能。
+     * 使用MoveIt进行轨迹规划。
     */
    class ArmControl : public ControlBase
    {
@@ -48,5 +69,95 @@ namespace robot_control
         * @brief 析构函数
         */
        ~ArmControl();
+
+        /**
+         * @brief 初始化MoveIt（需要在ROS节点创建后调用）
+         * @param node ROS 2节点指针
+         * @param move_group_name MoveGroup名称（如 "arm_left", "arm_right"）
+         * @return true 初始化成功，false 初始化失败
+         */
+        bool InitializeMoveIt(rclcpp::Node* node, const std::string& move_group_name);
+
+        /**
+         * @brief 规划关节空间运动（MOVEJ）
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param target_joints 目标关节角度（弧度）
+         * @param velocity_scaling 速度缩放因子 [0,1]
+         * @param acceleration_scaling 加速度缩放因子 [0,1]
+         * @param trajectory_points 输出：规划好的轨迹点（关节角度，弧度）
+         * @return true 规划成功，false 规划失败
+         */
+        bool PlanJointMotion(
+            uint8_t arm_id,
+            const std::vector<double>& target_joints,
+            double velocity_scaling,
+            double acceleration_scaling,
+            std::vector<std::vector<double>>& trajectory_points);
+
+        /**
+         * @brief 规划笛卡尔空间运动（MOVEL）
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param target_pose 目标位姿
+         * @param velocity_scaling 速度缩放因子 [0,1]
+         * @param acceleration_scaling 加速度缩放因子 [0,1]
+         * @param trajectory_points 输出：规划好的轨迹点（关节角度，弧度）
+         * @return true 规划成功，false 规划失败
+         */
+        bool PlanCartesianMotion(
+            uint8_t arm_id,
+            const geometry_msgs::msg::Pose& target_pose,
+            double velocity_scaling,
+            double acceleration_scaling,
+            std::vector<std::vector<double>>& trajectory_points);
+
+        /**
+         * @brief 检查MoveIt是否已初始化
+         * @return true 已初始化，false 未初始化
+         */
+        bool IsMoveItInitialized() const { return move_group_ != nullptr; }
+
+        /**
+         * @brief 存储规划好的轨迹
+         * @param trajectory_points 轨迹点（关节角度，弧度）
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         */
+        void StoreTrajectory(const std::vector<std::vector<double>>& trajectory_points, uint8_t arm_id);
+
+        /**
+         * @brief 获取当前插补点位（根据时间周期获取下一个点位）
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param dt 时间步长（秒）
+         * @param joint_positions 输出：当前插补点位（关节角度，弧度）
+         * @return true 还有更多点位，false 轨迹执行完成
+         */
+        bool GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions);
+
+        /**
+         * @brief 检查是否有正在执行的轨迹
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @return true 有轨迹在执行，false 没有轨迹
+         */
+        bool HasActiveTrajectory(uint8_t arm_id) const;
+
+        /**
+         * @brief 清除轨迹
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         */
+        void ClearTrajectory(uint8_t arm_id);
+
+    private:
+        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;  ///< MoveIt MoveGroup接口
+        std::string move_group_name_;  ///< MoveGroup名称
+        rclcpp::Node* node_;  ///< ROS节点指针（用于MoveIt初始化）
+
+        // 轨迹执行相关
+        struct TrajectoryState {
+            std::vector<std::vector<double>> trajectory_points;  ///< 轨迹点（关节角度，弧度）
+            size_t current_index;  ///< 当前轨迹点索引
+            double elapsed_time;   ///< 已执行时间
+            bool is_active;        ///< 是否激活
+        };
+        TrajectoryState left_arm_trajectory_;   ///< 左臂轨迹状态
+        TrajectoryState right_arm_trajectory_;  ///< 右臂轨迹状态
    };
 }
--- a/src/robot_control/include/controllers/control_base.hpp
+++ b/src/robot_control/include/controllers/control_base.hpp
@@ -134,6 +134,12 @@ namespace robot_control
        std::vector<double> joint_home_positions_;  ///< 回零位置
        std::vector<double> joint_min_limits_;      ///< 最小限位
        std::vector<double> joint_max_limits_;      ///< 最大限位
+        
+        // 通用参数（所有控制器共享）
+        std::vector<double> lengthParameters_;      ///< 长度参数
+        std::vector<int> joint_directions_;         ///< 关节方向
+        std::vector<double> joint_zero_positions_;  ///< 零点位置
+        bool is_target_set_;                        ///< 目标是否已设置

        TrapezoidalTrajectory* trapezoidalTrajectory_;  ///< 梯形轨迹规划器

--- a/src/robot_control/include/controllers/leg_control.hpp
+++ b/src/robot_control/include/controllers/leg_control.hpp
@@ -56,7 +56,10 @@ namespace robot_control
         */
        bool MoveUp(std::vector<double>& joint_positions, double dt);

-    private:
+        // Internal methods - made public for RobotControlManager access
        bool SetMoveLegParametersInternal(double moveLegDistance);
+
+    private:
+        double calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree);
    };
 }
--- a/src/robot_control/include/controllers/waist_control.hpp
+++ b/src/robot_control/include/controllers/waist_control.hpp
@@ -57,10 +57,11 @@ namespace robot_control
         */
        bool MoveWaist(std::vector<double>& joint_positions, double dt);

-    private:
+        // Internal methods - made public for RobotControlManager access
        bool SetMoveWaistParametersInternal(double movePitchAngle, double moveYawAngle);

-        bool is_target_set_;           ///< 目标是否已设置
+    private:
+        // 成员变量（特定于腰部控制器）
        bool is_cmd_send_finished_;    ///< 命令是否已发送完成
        int time_out_count_;           ///< 超时计数
        static constexpr int TIME_OUT_COUNT = 500;  ///< 超时计数阈值
--- a/src/robot_control/include/controllers/wheel_control.hpp
+++ b/src/robot_control/include/controllers/wheel_control.hpp
@@ -62,8 +62,13 @@ namespace robot_control
         */
        double GetWheelRatioInternal();

-    private:
+        // Internal methods - made public for RobotControlManager access
        bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
        bool SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle);
+
+    private:
+        // 成员变量（特定于轮子控制器）
+        double lastMoveDistance_;                   ///< 上次移动距离
+        double moveRatio_;                          ///< 移动比例
    };
 }
--- a/src/robot_control/include/core/remote_controller.hpp
+++ b/src/robot_control/include/core/remote_controller.hpp
@@ -0,0 +1,163 @@
+/**
+ * @file remote_controller.hpp
+ * @brief 遥控器控制器 - 处理遥控器指令
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该类负责：
+ * - 处理遥控器命令（gamepad消息）
+ * - 单关节点动控制（jog）
+ * - 各部位运动控制（leg, waist, wheel等）
+ */
+
+#pragma once
+
+#include <string>
+#include <functional>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "core/common_enum.hpp"
+
+namespace robot_control
+{
+    // 前向声明
+    class RobotControlManager;
+
+    /**
+     * @class RemoteController
+     * @brief 遥控器控制器类
+     * 
+     * 负责处理遥控器命令，分为两大类功能：
+     * 1. 单关节点动（jog）- 精确控制单个关节的运动
+     * 2. 运动控制 - 控制各个部位的运动（leg, waist, wheel等）
+     */
+    class RemoteController
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param node ROS 2节点指针（用于日志）
+         * @param robot_control_manager 机器人控制管理器引用
+         */
+        RemoteController(
+            rclcpp::Node* node,
+            RobotControlManager& robot_control_manager);
+
+        /**
+         * @brief 析构函数
+         */
+        ~RemoteController();
+
+        /**
+         * @brief 处理遥控器命令
+         * @param msg 遥控器命令消息
+         */
+        void ProcessCommand(const std_msgs::msg::String::SharedPtr msg);
+
+        // ==================== Jog 控制状态查询 ====================
+        
+        /**
+         * @brief 检查是否处于点动模式
+         * @return true 点动模式开启，false 点动模式关闭
+         */
+        bool IsJogMode() const { return isJogMode_; }
+
+        /**
+         * @brief 获取当前点动的关节索引
+         * @return 关节索引
+         */
+        size_t GetJogIndex() const { return jogIndex_; }
+
+        /**
+         * @brief 获取当前点动方向
+         * @return 方向（-1, 0, 1）
+         */
+        int GetJogDirection() const { return jogDirection_; }
+
+        /**
+         * @brief 设置点动模式状态（用于外部控制）
+         * @param enabled true 启用点动模式，false 禁用点动模式
+         */
+        void SetJogMode(bool enabled) { isJogMode_ = enabled; }
+
+        /**
+         * @brief 检查是否有停止请求
+         * @return true 有停止请求，false 无停止请求
+         */
+        bool HasStopRequest() const { return isStopping_; }
+
+        /**
+         * @brief 清除停止请求标志
+         */
+        void ClearStopRequest() { isStopping_ = false; }
+
+    private:
+        // ==================== 核心组件引用 ====================
+        
+        rclcpp::Node* node_;                          ///< ROS 2节点指针（用于日志）
+        RobotControlManager& robotControlManager_;    ///< 机器人控制管理器引用
+
+        // ==================== Jog 控制状态 ====================
+        
+        bool isJogMode_;      ///< 是否处于点动模式
+        int jogDirection_;    ///< 点动方向（-1, 0, 1）
+        size_t jogIndex_;     ///< 当前点动的关节索引
+
+        // ==================== 运动控制状态 ====================
+        
+        bool isStopping_;     ///< 是否有停止请求
+
+        // ==================== 命令去重 ====================
+        
+        std::string lastCommand_;  ///< 上次处理的命令（用于去重）
+
+        // ==================== Jog 控制方法 ====================
+        
+        /**
+         * @brief 处理点动模式开关命令
+         * @param command 命令字符串
+         */
+        void HandleJogModeToggle(const std::string& command);
+
+        /**
+         * @brief 处理关节切换命令
+         * @param command 命令字符串
+         */
+        void HandleJogAxisSwitch(const std::string& command);
+
+        /**
+         * @brief 处理点动方向命令
+         * @param command 命令字符串
+         */
+        void HandleJogDirection(const std::string& command);
+
+        // ==================== 运动控制方法 ====================
+        
+        /**
+         * @brief 处理紧急停止命令
+         */
+        void HandleEmergencyStop();
+
+        /**
+         * @brief 处理运动部位控制命令（预留接口，用于未来扩展）
+         * @param command 命令字符串
+         */
+        void HandleMovementControl(const std::string& command);
+
+        // ==================== 辅助方法 ====================
+        
+        /**
+         * @brief 检查命令是否为重复命令
+         * @param command 命令字符串
+         * @return true 重复命令，false 新命令
+         */
+        bool IsDuplicateCommand(const std::string& command);
+
+        /**
+         * @brief 更新最后处理的命令
+         * @param command 命令字符串
+         */
+        void UpdateLastCommand(const std::string& command);
+    };
+}
+
--- a/src/robot_control/include/core/robot_control_manager.hpp
+++ b/src/robot_control/include/core/robot_control_manager.hpp
@@ -33,9 +33,11 @@

 #include "interfaces/msg/motor_pos.hpp"
 #include "interfaces/msg/imu_msg.hpp"
+#include "interfaces/msg/arm_motion_params.hpp"

 using MotorPos = interfaces::msg::MotorPos;
 using ImuMsg = sensor_msgs::msg::Imu;
+using ArmMotionParams = interfaces::msg::ArmMotionParams;

 namespace robot_control
 {
@@ -152,6 +154,65 @@ namespace robot_control
         */
        bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
        
+        /**
+         * @brief 设置手臂关节命令（直接设置关节角度，通过topic发布）
+         * @param body_id 手臂ID（0=左臂，1=右臂）
+         * @param joint_angles 关节角度数组（度），大小为6
+         * @return true 设置成功，false 设置失败
+         */
+        bool SetArmJointCommands(int8_t body_id, const std::vector<double>& joint_angles);
+
+        /**
+         * @brief 规划手臂运动（调用ArmControl的规划方法）
+         * @param arm_params 手臂运动参数（包含MOVEJ或MOVEL）
+         * @param velocity_scaling 速度缩放因子 [0,1]
+         * @param acceleration_scaling 加速度缩放因子 [0,1]
+         * @param trajectory_points 输出：规划好的轨迹点（关节角度，弧度）
+         * @return true 规划成功，false 规划失败
+         */
+        bool PlanArmMotion(
+            const ArmMotionParams& arm_params,
+            double velocity_scaling,
+            double acceleration_scaling,
+            std::vector<std::vector<double>>& trajectory_points);
+
+        /**
+         * @brief 初始化ArmControl的MoveIt（需要在ROS节点创建后调用）
+         * @param node ROS 2节点指针
+         * @return true 初始化成功，false 初始化失败
+         */
+        bool InitializeArmMoveIt(rclcpp::Node* node);
+
+        /**
+         * @brief 存储规划好的轨迹到ArmControl
+         * @param arm_params 手臂运动参数
+         * @param trajectory_points 轨迹点（关节角度，弧度）
+         */
+        void StoreArmTrajectory(const ArmMotionParams& arm_params, const std::vector<std::vector<double>>& trajectory_points);
+
+        /**
+         * @brief 获取当前插补点位（在主循环中调用）
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @param dt 时间步长（秒）
+         * @param joint_positions 输出：当前插补点位（关节角度，弧度）
+         * @return true 还有更多点位，false 轨迹执行完成
+         */
+        bool GetArmInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions);
+
+        /**
+         * @brief 检查是否有正在执行的轨迹
+         * @param arm_id 手臂ID（0=左臂，1=右臂）
+         * @return true 有轨迹在执行，false 没有轨迹
+         */
+        bool HasActiveArmTrajectory(uint8_t arm_id) const;
+
+        /**
+         * @brief 控制手臂运动（在主循环中调用）
+         * @param dt 时间步长（秒）
+         * @return true 运动完成，false 运动进行中
+         */
+        bool MoveArm(double dt);
+        
        // ==================== 状态查询接口 ====================
        
        /**
@@ -249,6 +310,10 @@ namespace robot_control
         * @brief 更新关节命令（将各控制器的临时值合并到关节命令中）
         */
        void UpdateJointCommands();
+        
+        // Quaternion conversion utilities (private helpers)
+        void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
+        void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);

        // ==================== 成员变量 ====================
        
@@ -294,9 +359,14 @@ namespace robot_control

        // IMU相关
        std::vector<double> gyroValues_;        ///< 陀螺仪值
+        std::vector<double> gyroInitValues_;    ///< 陀螺仪初始值
        std::vector<double> gyroVelocities_;    ///< 陀螺仪速度
        std::vector<double> gyroAccelerations_; ///< 陀螺仪加速度
+        sensor_msgs::msg::Imu imuMsg_;          ///< IMU消息
        bool isGyroInited_;                     ///< 陀螺仪是否已初始化
+        
+        // 轮子状态相关
+        interfaces::msg::MotorPos motorPos_;    ///< 电机位置消息

        // 初始化相关
        std::vector<bool> jointInited_;  ///< 各关节是否已初始化
--- a/src/robot_control/include/core/robot_control_node.hpp
+++ b/src/robot_control/include/core/robot_control_node.hpp
@@ -17,10 +17,12 @@
 #include <fstream>
 #include <memory>
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "core/robot_control_manager.hpp"
+#include "core/remote_controller.hpp"

 #include "interfaces/action/move_home.hpp"
 #include "interfaces/action/move_leg.hpp"
@@ -87,7 +89,7 @@ namespace robot_control
    private:
        // ==================== ROS 2 通信接口 ====================
        
-        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ethercatSetPub_;     ///< EtherCAT设置发布器
+        rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jointTrajectoryPub_;     ///< 关节轨迹发布器
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;   ///< 手柄命令订阅器
        rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;  ///< 关节状态订阅器
        rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;                     ///< 电机命令发布器
@@ -99,6 +101,7 @@ namespace robot_control
        
        RobotControlManager robotControlManager_;              ///< 机器人控制管理器
        std::unique_ptr<ActionManager> action_manager_;        ///< 动作管理器
+        std::unique_ptr<RemoteController> remoteController_;   ///< 遥控器控制器

        // ==================== 数据记录 ====================
        
@@ -118,6 +121,7 @@ namespace robot_control
        size_t jogIndex_;    ///< 当前点动的关节索引
        double jogValue_;    ///< 点动值
        double lastSpeed_;   ///< 上次速度
+        std::string lastJoyCommand_;  ///< 上次处理的遥控器命令（用于去重）

        // ==================== 回调函数 ====================
        
--- a/src/robot_control/launch/right_arm_joint_test.launch.py
+++ b/src/robot_control/launch/right_arm_joint_test.launch.py
@@ -0,0 +1,42 @@
+"""
+Launch file for right arm joint test node
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    # 声明启动参数
+    use_action_arg = DeclareLaunchArgument(
+        'use_action',
+        default_value='true',
+        description='Whether to use FollowJointTrajectory action (true) or MoveGroupInterface::execute() (false)'
+    )
+    
+    action_name_arg = DeclareLaunchArgument(
+        'action_name',
+        default_value='/arm_right_controller/follow_joint_trajectory',
+        description='Action server name for FollowJointTrajectory'
+    )
+    
+    # 创建节点
+    right_arm_joint_test_node = Node(
+        package='robot_control',
+        executable='right_arm_joint_test',
+        name='right_arm_joint_test',
+        output='screen',
+        parameters=[{
+            'use_action': LaunchConfiguration('use_action'),
+            'action_name': LaunchConfiguration('action_name'),
+        }]
+    )
+    
+    return LaunchDescription([
+        use_action_arg,
+        action_name_arg,
+        right_arm_joint_test_node,
+    ])
+
--- a/src/robot_control/launch/robot_control.launch.py
+++ b/src/robot_control/launch/robot_control.launch.py
@@ -1,8 +1,23 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from launch.actions import TimerAction
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory

 def generate_launch_description():
+    # 获取dual_arm_moveit_config包的路径
+    dual_arm_moveit_config_path = get_package_share_directory('dual_arm_moveit_config')
+    
+    # 包含move_group launch文件（启动MoveIt move_group节点，不启动RViz）
+    # 使用 move_group_no_rviz.launch.py 以节省资源，仅启动move_group节点
+    # 如需可视化，可使用 move_group_with_rviz.launch.py
+    move_group_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            dual_arm_moveit_config_path, '/launch/move_group.launch.py'
+        ])
+    )
+
    # 创建节点启动描述
    robot_control_node = Node(
        package='robot_control',  # 功能包名称
@@ -18,6 +33,8 @@ def generate_launch_description():
    )
    
    # 组装launch描述
+    # move_group需要在robot_control_node之前启动
    return LaunchDescription([
-        start_robot_control_node
+        move_group_launch,  # 先启动move_group
+        start_robot_control_node  # 然后启动robot_control_node
    ])
--- a/src/robot_control/package.xml
+++ b/src/robot_control/package.xml
@@ -1,6 +1,7 @@
 <?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>robot_control</name>
  <version>1.0.0</version>
  <description>Robot control package</description>
  <maintainer email="Ray@email.com">Your Name</maintainer>
@@ -17,6 +18,9 @@
  <build_depend>control_msgs</build_depend>
  <build_depend>Eigen3</build_depend>  <!-- 对应CMake中的Eigen3依赖 -->
  <build_depend>interfaces</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_ros_planning_interface</build_depend>
+  <build_depend>moveit_msgs</build_depend>


  <!-- 运行时依赖（必须完整，否则类型无法识别） -->
@@ -29,6 +33,9 @@
  <exec_depend>control_msgs</exec_depend>
  <exec_depend>nav_msgs</exec_depend>
  <exec_depend>interfaces</exec_depend>
+  <exec_depend>moveit_core</exec_depend>
+  <exec_depend>moveit_ros_planning_interface</exec_depend>
+  <exec_depend>moveit_msgs</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
--- a/src/robot_control/src/actions/action_manager.cpp
+++ b/src/robot_control/src/actions/action_manager.cpp
@@ -14,6 +14,12 @@
 #include <cmath>
 #include <chrono>

+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// DualArm action - 使用 DualArm.action 中定义的常量
+
 using namespace robot_control;

 ActionManager::ActionManager(
@@ -31,6 +37,7 @@ ActionManager::ActionManager(
    , move_leg_executing_(false)
    , move_waist_executing_(false)
    , move_wheel_executing_(false)
+    , dual_arm_executing_(false)
 {
 }

@@ -77,6 +84,15 @@ void ActionManager::initialize()
        std::bind(&ActionManager::handle_move_wheel_cancel, this, _1),
        std::bind(&ActionManager::handle_move_wheel_accepted, this, _1));
    RCLCPP_INFO(node_->get_logger(), "MoveWheel action server is ready");
+
+    // 创建 DualArm Action 服务器
+    dual_arm_action_server_ = rclcpp_action::create_server<interfaces::action::DualArm>(
+        node_,
+        "DualArm",
+        std::bind(&ActionManager::handle_dual_arm_goal, this, _1, _2),
+        std::bind(&ActionManager::handle_dual_arm_cancel, this, _1),
+        std::bind(&ActionManager::handle_dual_arm_accepted, this, _1));
+    RCLCPP_INFO(node_->get_logger(), "DualArm action server is ready (using topic mode)");
 }

 // MoveHome Action 处理函数
@@ -469,3 +485,133 @@ void ActionManager::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel
    }
 }

+// DualArm Action 处理函数
+rclcpp_action::GoalResponse ActionManager::handle_dual_arm_goal(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const DualArm::Goal> goal)
+{
+    (void)uuid;
+    
+    RCLCPP_INFO(node_->get_logger(), 
+        "Received DualArm goal: arm_type=%d, velocity_scaling=%.2f, acceleration_scaling=%.2f, arm_motion_params_count=%zu",
+        goal->arm_type, goal->velocity_scaling, goal->acceleration_scaling, goal->arm_motion_params.size());
+    
+    if (robot_control_manager_.IsMoving(MovementPart::ALL))
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Robot is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+    if (dual_arm_executing_.load())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Another dual arm goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+    if (is_jog_mode_func_())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse ActionManager::handle_dual_arm_cancel(
+    const std::shared_ptr<GoalHandleDualArm> goal_handle)
+{
+    (void)goal_handle;
+    RCLCPP_INFO(node_->get_logger(), "Received request to cancel DualArm goal");
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void ActionManager::handle_dual_arm_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle)
+{
+    dual_arm_executing_.store(true);
+    using namespace std::placeholders;
+    std::thread{std::bind(&ActionManager::dual_arm_execute, this, _1), goal_handle}.detach();
+}
+
+void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> goal_handle)
+{
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<DualArm::Feedback>();
+    auto result = std::make_shared<DualArm::Result>();
+    
+    result->success = false;
+    result->message = "DualArm action failed";
+    
+    try {
+        // 使用RobotControlManager的统一规划接口
+        for (const auto& arm_param : goal->arm_motion_params) {
+            // 检查参数有效性
+            if (arm_param.motion_type == arm_param.MOVEJ && arm_param.target_joints.empty()) {
+                RCLCPP_ERROR(node_->get_logger(), "MOVEJ requested but target_joints is empty for arm_id %d", arm_param.arm_id);
+                result->message = "MOVEJ target_joints empty";
+                goal_handle->abort(result);
+                dual_arm_executing_.store(false);
+                return;
+            }
+
+            if (arm_param.motion_type == arm_param.MOVEL) {
+                // MOVEL参数验证（target_pose在消息定义中已包含）
+                // 不需要额外验证
+            }
+
+            // 调用RobotControlManager的规划接口（统一管理）
+            std::vector<std::vector<double>> trajectory_points;
+            if (robot_control_manager_.PlanArmMotion(
+                arm_param,
+                goal->velocity_scaling,
+                goal->acceleration_scaling,
+                trajectory_points)) {
+                RCLCPP_INFO(node_->get_logger(), 
+                    "Arm motion planning successful for arm_id %d, motion_type %d, trajectory points: %zu",
+                    arm_param.arm_id, arm_param.motion_type, trajectory_points.size());
+                
+                // 存储轨迹到ArmControl，在主循环中依次下发
+                robot_control_manager_.StoreArmTrajectory(arm_param, trajectory_points);
+                
+                result->success = true;
+                result->message = "DualArm planning succeeded";
+            } else {
+                RCLCPP_ERROR(node_->get_logger(), 
+                    "Arm motion planning failed for arm_id %d, motion_type %d", 
+                    arm_param.arm_id, arm_param.motion_type);
+                result->success = false;
+                result->message = "Arm motion planning failed";
+                goal_handle->abort(result);
+                dual_arm_executing_.store(false);
+                return;
+            }
+        }
+        
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(node_->get_logger(), "Exception during DualArm execution: %s", e.what());
+        result->success = false;
+        result->message = std::string("Exception: ") + e.what();
+        goal_handle->abort(result);
+        dual_arm_executing_.store(false);
+        return;
+    }
+    
+    // 检查取消请求
+    if (goal_handle->is_canceling()) {
+        result->success = false;
+        result->message = "DualArm goal canceled";
+        goal_handle->canceled(result);
+        RCLCPP_INFO(node_->get_logger(), "DualArm goal canceled");
+        dual_arm_executing_.store(false);
+        return;
+    }
+    
+    if (rclcpp::ok()) {
+        result->final_progress = 1.0;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(node_->get_logger(), "DualArm goal %s.", result->success ? "succeeded" : "failed");
+    }
+    
+    dual_arm_executing_.store(false);
+}
+
+
--- a/src/robot_control/src/controllers/arm_control.cpp
+++ b/src/robot_control/src/controllers/arm_control.cpp
@@ -1,6 +1,12 @@
 #include "controllers/arm_control.hpp"
+#include <iostream>
 #include <stdexcept>
 #include <vector>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "moveit/planning_interface/planning_interface.h"
+#include "moveit_msgs/msg/move_it_error_codes.hpp"

 namespace robot_control
 {
@@ -24,89 +30,212 @@ ArmControl::ArmControl(
    home_positions, 
    zero_positions,
    joint_directions)
+    , move_group_(nullptr)
+    , node_(nullptr)
 {
    std::cout << "ArmControl Constructor" << std::endl;
-    if (total_joints_ <= 0)
-        throw std::invalid_argument("Total joints must be positive");
-    
-    // 初始化关节状态向量
-    joint_position_.resize(total_joints_, 0.0);
-    joint_velocity_.resize(total_joints_, 0.0);
-    joint_acceleration_.resize(total_joints_, 0.0);
-    joint_torque_.resize(total_joints_, 0.0);
-    joint_position_desired_.resize(total_joints_, 0.0);
-    
-    // 初始化梯形轨迹规划器
-    trapezoidalTrajectory_ = new TrapezoidalTrajectory(10.0, 100);
-
-    is_moving_ = false;
-    is_stopping_ = false;
-    stopDurationTime_ = 0.0;
-    movingDurationTime_ = 0.0;
+    // 注意：ControlBase已经初始化了所有基本成员变量
 }

 ArmControl::~ArmControl()
 {
-    delete trapezoidalTrajectory_;
+    // MoveGroupInterface是shared_ptr，会自动释放
+    move_group_.reset();
 }

-
-bool ArmControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
+bool ArmControl::InitializeMoveIt(rclcpp::Node* node, const std::string& move_group_name)
 {
-    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
-    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
-    std::vector<double> joint_target(total_joints_, 0.0);
-    
-    // 实际应用中应在此处添加逆运动学求解
-    // joint_target = inverse_kinematics(target_pos);
-    
-    for (size_t i = 0; i < target_pos.size(); i++)
-    {
-        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
+    if (!node) {
+        std::cerr << "ArmControl::InitializeMoveIt: node is null" << std::endl;
+        return false;
    }

-    // 使用梯形轨迹规划移动到目标关节位置
-    return MoveToTargetJoint(joint_positions, joint_target, dt);
-}
+    node_ = node;
+    move_group_name_ = move_group_name;

-bool ArmControl::MoveUp(std::vector<double>& joint_positions, double dt)
-{    
-    if (!is_moving_)
-    {
-        for (size_t i = 0; i < joint_position_.size(); i++)
-        {
-            if (joint_directions_[i] == 1)
-            {
-                joint_position_desired_[i] = joint_position_[i] + 20;
-            }
-            else
-            {
-                joint_position_desired_[i] = joint_position_[i] - 20;
-            }
+    try {
+        // 创建一个shared_ptr包装node（MoveGroupInterface需要shared_ptr）
+        auto node_shared = std::shared_ptr<rclcpp::Node>(node, [](rclcpp::Node*) {
+            // 空的删除器，因为我们不拥有node的所有权
+        });
+
+        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+            node_shared, move_group_name);
+
+        if (!move_group_) {
+            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface" << std::endl;
+            return false;
        }
-    }

-    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for " 
+                  << move_group_name << std::endl;
+        return true;
+    } catch (const std::exception& e) {
+        std::cerr << "ArmControl::InitializeMoveIt: Exception: " << e.what() << std::endl;
+        return false;
+    }
 }

-bool ArmControl::MoveDown(std::vector<double>& joint_positions, double dt)
+bool ArmControl::PlanJointMotion(
+    uint8_t /* arm_id */,
+    const std::vector<double>& target_joints,
+    double velocity_scaling,
+    double acceleration_scaling,
+    std::vector<std::vector<double>>& trajectory_points)
 {
-    if (!is_moving_)
-    {
-        for (size_t i = 0; i < joint_position_.size(); i++)
-        {
-            if (joint_directions_[i] == 1)
-            {
-                joint_position_desired_[i] = joint_position_[i] - 20;
-            }
-            else
-            {
-                joint_position_desired_[i] = joint_position_[i] + 20;
-            }
-        }
+    if (!move_group_) {
+        std::cerr << "ArmControl::PlanJointMotion: MoveIt not initialized" << std::endl;
+        return false;
    }

-    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+    try {
+        // 设置速度和加速度缩放因子
+        move_group_->setMaxVelocityScalingFactor(velocity_scaling);
+        move_group_->setMaxAccelerationScalingFactor(acceleration_scaling);
+
+        // 设置目标关节值（弧度）
+        move_group_->setJointValueTarget(target_joints);
+
+        // 规划
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        moveit::core::MoveItErrorCode error_code = move_group_->plan(plan);
+
+        if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
+            std::cerr << "ArmControl::PlanJointMotion: Planning failed with error code: " 
+                      << error_code.val << std::endl;
+            return false;
+        }
+
+        // 提取轨迹点
+        trajectory_points.clear();
+        if (plan.trajectory_.joint_trajectory.points.size() > 0) {
+            for (const auto& point : plan.trajectory_.joint_trajectory.points) {
+                trajectory_points.push_back(point.positions);
+            }
+        } else {
+            std::cerr << "ArmControl::PlanJointMotion: Trajectory is empty" << std::endl;
+            return false;
+        }
+
+        std::cout << "ArmControl::PlanJointMotion: Planning successful, " 
+                  << trajectory_points.size() << " trajectory points generated" << std::endl;
+        return true;
+    } catch (const std::exception& e) {
+        std::cerr << "ArmControl::PlanJointMotion: Exception: " << e.what() << std::endl;
+        return false;
+    }
+}
+
+bool ArmControl::PlanCartesianMotion(
+    uint8_t /* arm_id */,
+    const geometry_msgs::msg::Pose& target_pose,
+    double velocity_scaling,
+    double acceleration_scaling,
+    std::vector<std::vector<double>>& trajectory_points)
+{
+    if (!move_group_) {
+        std::cerr << "ArmControl::PlanCartesianMotion: MoveIt not initialized" << std::endl;
+        return false;
+    }
+
+    try {
+        // 设置速度和加速度缩放因子
+        move_group_->setMaxVelocityScalingFactor(velocity_scaling);
+        move_group_->setMaxAccelerationScalingFactor(acceleration_scaling);
+
+        // 设置目标位姿
+        move_group_->setPoseTarget(target_pose);
+
+        // 规划
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        moveit::core::MoveItErrorCode error_code = move_group_->plan(plan);
+
+        if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
+            std::cerr << "ArmControl::PlanCartesianMotion: Planning failed with error code: " 
+                      << error_code.val << std::endl;
+            return false;
+        }
+
+        // 提取轨迹点
+        trajectory_points.clear();
+        if (plan.trajectory_.joint_trajectory.points.size() > 0) {
+            for (const auto& point : plan.trajectory_.joint_trajectory.points) {
+                trajectory_points.push_back(point.positions);
+            }
+        } else {
+            std::cerr << "ArmControl::PlanCartesianMotion: Trajectory is empty" << std::endl;
+            return false;
+        }
+
+        std::cout << "ArmControl::PlanCartesianMotion: Planning successful, " 
+                  << trajectory_points.size() << " trajectory points generated" << std::endl;
+        return true;
+    } catch (const std::exception& e) {
+        std::cerr << "ArmControl::PlanCartesianMotion: Exception: " << e.what() << std::endl;
+        return false;
+    }
+}
+
+void ArmControl::StoreTrajectory(const std::vector<std::vector<double>>& trajectory_points, uint8_t arm_id)
+{
+    TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+    
+    traj_state->trajectory_points = trajectory_points;
+    traj_state->current_index = 0;
+    traj_state->elapsed_time = 0.0;
+    traj_state->is_active = !trajectory_points.empty();
+    
+    std::cout << "ArmControl::StoreTrajectory: Stored " << trajectory_points.size() 
+              << " trajectory points for arm_id " << static_cast<int>(arm_id) << std::endl;
+}
+
+bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions)
+{
+    TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+    
+    if (!traj_state->is_active || traj_state->trajectory_points.empty()) {
+        return false;
+    }
+
+    if (traj_state->current_index >= traj_state->trajectory_points.size()) {
+        // 轨迹执行完成
+        traj_state->is_active = false;
+        return false;
+    }
+
+    // 直接返回当前索引的点位（简单实现，也可以做插值）
+    joint_positions = traj_state->trajectory_points[traj_state->current_index];
+    
+    // 更新索引和时间
+    traj_state->current_index++;
+    traj_state->elapsed_time += dt;
+    
+    // 检查是否还有更多点位
+    if (traj_state->current_index >= traj_state->trajectory_points.size()) {
+        traj_state->is_active = false;
+        return false;  // 最后一个点位，下次调用返回false
+    }
+    
+    return true;  // 还有更多点位
+}
+
+bool ArmControl::HasActiveTrajectory(uint8_t arm_id) const
+{
+    const TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+    return traj_state->is_active && !traj_state->trajectory_points.empty();
+}
+
+void ArmControl::ClearTrajectory(uint8_t arm_id)
+{
+    TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+    
+    traj_state->trajectory_points.clear();
+    traj_state->current_index = 0;
+    traj_state->elapsed_time = 0.0;
+    traj_state->is_active = false;
+    
+    std::cout << "ArmControl::ClearTrajectory: Cleared trajectory for arm_id " 
+              << static_cast<int>(arm_id) << std::endl;
 }

 }  // namespace robot_control
--- a/src/robot_control/src/controllers/control_base.cpp
+++ b/src/robot_control/src/controllers/control_base.cpp
@@ -1,6 +1,7 @@
 #include "controllers/control_base.hpp"
 #include <stdexcept>
 #include <vector>
+#include <iostream>

 namespace robot_control
 {
@@ -17,14 +18,13 @@ ControlBase::ControlBase(
    const std::vector<int>& joint_directions
 ): 
    total_joints_(total_joints),
-    lengthParameters_(lengthParameters),
-    maxSpeed_(maxSpeed),
-    maxAcc_(maxAcc),
    joint_home_positions_(home_positions),
-    joint_zero_positions_(zero_positions),
+    joint_min_limits_(minLimits),
+    joint_max_limits_(maxLimits),
+    lengthParameters_(lengthParameters),
    joint_directions_(joint_directions),
-    minLimits_(minLimits),
-    maxLimits_(maxLimits)
+    joint_zero_positions_(zero_positions),
+    is_target_set_(false)
 {
    if (total_joints_ <= 0)
        throw std::invalid_argument("Total joints must be positive");
@@ -36,21 +36,16 @@ ControlBase::ControlBase(
    joint_torque_.resize(total_joints_, 0.0);
    joint_position_desired_.resize(total_joints_, 0.0);
    joint_commands_.resize(total_joints_, 0.0);
-    joint_velocity_commands_.resize(total_joints_, 0.0);
    
-    // 初始化梯形轨迹规划器
+    // 初始化梯形轨迹规划器（使用多轴版本）
    trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);

    is_moving_ = false;
    is_stopping_ = false;
-    is_target_set_ = false;
-    is_cmd_send_finished_ = false;
    is_joint_position_initialized_ = false;

    stopDurationTime_ = 0.0;
    movingDurationTime_ = 0.0;
-
-    time_out_count_ = 0;
 }

 ControlBase::~ControlBase()
@@ -189,14 +184,17 @@ bool ControlBase::IsReached(const std::vector<double>& target_joint)
    return result;
 }

-bool ControlBase::checkJointLimits(const std::vector<double>& target_joint)
+bool ControlBase::checkJointLimits(std::vector<double>& joint_positions)
 {
-    for (size_t i = 0; i < target_joint.size(); i++)
+    for (size_t i = 0; i < joint_positions.size() && i < joint_min_limits_.size() && i < joint_max_limits_.size(); i++)
    {
-        if (target_joint[i] < minLimits_[i] || target_joint[i] > maxLimits_[i])
+        if (joint_positions[i] < joint_min_limits_[i])
        {
-            printf("Joint %zu target position %f is out of limits [%f, %f]", i + 1, target_joint[i], minLimits_[i], maxLimits_[i]);
-            return false;
+            joint_positions[i] = joint_min_limits_[i];
+        }
+        else if (joint_positions[i] > joint_max_limits_[i])
+        {
+            joint_positions[i] = joint_max_limits_[i];
        }
    }

--- a/src/robot_control/src/controllers/leg_control.cpp
+++ b/src/robot_control/src/controllers/leg_control.cpp
@@ -1,6 +1,13 @@
 #include "controllers/leg_control.hpp"
+#include "core/motion_parameters.hpp"
 #include <stdexcept>
 #include <vector>
+#include <iostream>
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif

 namespace robot_control
 {
@@ -34,23 +41,7 @@ LegControl::~LegControl()
    delete trapezoidalTrajectory_;
 }

-bool LegControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
-{
-    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
-    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
-    std::vector<double> joint_target(total_joints_, 0.0);
-    
-    // 实际应用中应在此处添加逆运动学求解
-    // joint_target = inverse_kinematics(target_pos);
-    
-    for (size_t i = 0; i < target_pos.size(); i++)
-    {
-        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
-    }
-
-    // 使用梯形轨迹规划移动到目标关节位置
-    return MoveToTargetJoint(joint_positions, joint_target, dt);
-}
+// MoveToTargetPoint removed - not declared in header

 bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
 {    
--- a/src/robot_control/src/controllers/waist_control.cpp
+++ b/src/robot_control/src/controllers/waist_control.cpp
@@ -1,4 +1,5 @@
 #include "controllers/waist_control.hpp"
+#include <iostream>
 #include <stdexcept>
 #include <vector>

@@ -34,23 +35,7 @@ WaistControl::~WaistControl()
 }


-bool WaistControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
-{
-    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
-    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
-    std::vector<double> joint_target(total_joints_, 0.0);
-    
-    // 实际应用中应在此处添加逆运动学求解
-    // joint_target = inverse_kinematics(target_pos);
-
-    for (size_t i = 0; i < target_pos.size(); i++)
-    {
-        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
-    }
-
-    // 使用梯形轨迹规划移动到目标关节位置
-    return MoveToTargetJoint(joint_positions, joint_target, dt);
-}
+// MoveToTargetPoint removed - not declared in header

 bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
 {    
--- a/src/robot_control/src/controllers/wheel_control.cpp
+++ b/src/robot_control/src/controllers/wheel_control.cpp
@@ -1,4 +1,5 @@
 #include "controllers/wheel_control.hpp"
+#include <iostream>
 #include <vector>

 namespace robot_control
@@ -23,6 +24,8 @@ WheelControl::WheelControl(
    home_positions, 
    zero_positions,
    joint_directions)
+    , lastMoveDistance_(0.0)
+    , moveRatio_(0.0)
 {
    std::cout << "WheelControl Constructor" << std::endl;
 }
@@ -57,7 +60,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub

    double beta = moveWheelAngle * 0.8 / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;

-    if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.5) && beta != 0)
+    if(moveWheelDistance > lastMoveDistance_ && (moveWheelDistance - lastMoveDistance_ > 1.5) && beta != 0)
    {
        if (beta > 0)
        {
@@ -69,7 +72,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
        }
    }

-    if(moveWheelDistance < lastMoveDistance && (moveWheelDistance - lastMoveDistance < -1.5) && beta != 0)
+    if(moveWheelDistance < lastMoveDistance_ && (moveWheelDistance - lastMoveDistance_ < -1.5) && beta != 0)
    {
        tempAdjustCount ++; 
        if (beta > 0)
@@ -146,7 +149,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub

    // std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] <<  " , " << joint_position_desired_[1] << std::endl;

-    lastMoveDistance = moveWheelDistance;
+    lastMoveDistance_ = moveWheelDistance;

    is_target_set_ = true;

@@ -220,23 +223,7 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d



-bool WheelControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
-{
-    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
-    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
-    std::vector<double> joint_target(total_joints_, 0.0);
-    
-    // 实际应用中应在此处添加逆运动学求解
-    // joint_target = inverse_kinematics(target_pos);
-    
-    for (size_t i = 0; i < target_pos.size(); i++)
-    {
-        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
-    }
-
-    // 使用梯形轨迹规划移动到目标关节位置
-    return MoveToTargetJoint(joint_positions, joint_target, dt);
-}
+// MoveToTargetPoint removed - not declared in header

 bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
 {    
@@ -257,12 +244,11 @@ bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
 }


-bool WheelControl::GoHome(std::vector<double>& joint_positions, double dt)
+// GoHome removed - not declared in header
+
+double WheelControl::GetWheelRatioInternal()
 {
-    // do nothing
-    (void) joint_positions;
-    (void) dt;
-    return true;
+    return moveRatio_;
 }

 }
--- a/src/robot_control/src/core/remote_controller.cpp
+++ b/src/robot_control/src/core/remote_controller.cpp
@@ -0,0 +1,186 @@
+/**
+ * @file remote_controller.cpp
+ * @brief 遥控器控制器实现
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#include "core/remote_controller.hpp"
+#include "core/robot_control_manager.hpp"
+#include "core/common_enum.hpp"
+
+using namespace robot_control;
+
+RemoteController::RemoteController(
+    rclcpp::Node* node,
+    RobotControlManager& robot_control_manager)
+    : node_(node)
+    , robotControlManager_(robot_control_manager)
+    , isJogMode_(false)
+    , jogDirection_(0)
+    , jogIndex_(0)
+    , isStopping_(false)
+{
+    lastCommand_.clear();
+    RCLCPP_INFO(node_->get_logger(), "RemoteController initialized");
+}
+
+RemoteController::~RemoteController()
+{
+}
+
+void RemoteController::ProcessCommand(const std_msgs::msg::String::SharedPtr msg)
+{
+    // 检查消息有效性
+    if (!msg) {
+        RCLCPP_WARN(node_->get_logger(), "Received null remote command message, ignoring");
+        return;
+    }
+
+    // 获取命令内容
+    const std::string& command = msg->data;
+    
+    // 去重：忽略重复的命令（避免快速重复触发）
+    if (IsDuplicateCommand(command)) {
+        return;
+    }
+    UpdateLastCommand(command);
+
+    // ==================== 点动模式控制 ====================
+    
+    // 点动模式开关
+    if (command == "A,1" || command == "B,1") {
+        HandleJogModeToggle(command);
+        return;
+    }
+    
+    // 关节切换（仅在点动模式下有效）
+    if (command == "RB,1" || command == "LB,1") {
+        HandleJogAxisSwitch(command);
+        return;
+    }
+    
+    // 点动方向控制（仅在点动模式下有效）
+    if (command.find("方向垂直,") == 0) {
+        HandleJogDirection(command);
+        return;
+    }
+
+    // ==================== 运动控制 ====================
+    
+    // 紧急停止
+    if (command == "X,1") {
+        HandleEmergencyStop();
+        return;
+    }
+
+    // 其他运动控制命令（预留接口）
+    HandleMovementControl(command);
+}
+
+void RemoteController::HandleJogModeToggle(const std::string& command)
+{
+    // 检查机器人是否正在运动
+    if (robotControlManager_.IsMoving(MovementPart::ALL)) {
+        RCLCPP_WARN(node_->get_logger(), "Robot is moving, cannot toggle jog mode");
+        return;
+    }
+
+    if (command == "A,1") {
+        // 开启点动模式
+        isJogMode_ = true;
+        jogDirection_ = 0;  // 重置方向
+        RCLCPP_INFO(node_->get_logger(), "Jog mode enabled");
+    }
+    else if (command == "B,1") {
+        // 关闭点动模式
+        isJogMode_ = false;
+        jogDirection_ = 0;  // 重置方向
+        RCLCPP_INFO(node_->get_logger(), "Jog mode disabled");
+    }
+}
+
+void RemoteController::HandleJogAxisSwitch(const std::string& command)
+{
+    // 仅在点动模式且当前无方向时允许切换
+    if (!isJogMode_ || jogDirection_ != 0) {
+        return;
+    }
+
+    const size_t max_joints = robotControlManager_.GetMotionParameters().total_joints_;
+
+    if (command == "RB,1") {
+        // 切换到下一个关节
+        if (jogIndex_ < max_joints - 1) {
+            jogIndex_++;
+            RCLCPP_INFO(node_->get_logger(), "Switched to jog axis: %zu", jogIndex_ + 1);
+        } else {
+            RCLCPP_WARN(node_->get_logger(), "Reached max joint (%zu), cannot switch to next axis", max_joints);
+        }
+    }
+    else if (command == "LB,1") {
+        // 切换到上一个关节
+        if (jogIndex_ > 0) {
+            jogIndex_--;
+            RCLCPP_INFO(node_->get_logger(), "Switched to jog axis: %zu", jogIndex_ + 1);
+        } else {
+            RCLCPP_WARN(node_->get_logger(), "Reached min joint (0), cannot switch to previous axis");
+        }
+    }
+}
+
+void RemoteController::HandleJogDirection(const std::string& command)
+{
+    // 仅在点动模式下有效
+    if (!isJogMode_) {
+        return;
+    }
+
+    if (command == "方向垂直,-1.0") {
+        // 负方向点动
+        jogDirection_ = -1;
+        RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: negative direction", jogIndex_ + 1);
+    }
+    else if (command == "方向垂直,1.0") {
+        // 正方向点动
+        jogDirection_ = 1;
+        RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: positive direction", jogIndex_ + 1);
+    }
+    else if (command == "方向垂直,0.0") {
+        // 停止点动
+        jogDirection_ = 0;
+        RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: stopped", jogIndex_ + 1);
+    }
+}
+
+void RemoteController::HandleEmergencyStop()
+{
+    // 紧急停止（仅在非点动模式下）
+    if (!isJogMode_) {
+        isStopping_ = true;
+        RCLCPP_WARN(node_->get_logger(), "Emergency stop requested");
+        // 注意：停止动作的执行由robot_control_node的ControlLoop处理
+        // 这里只设置停止标志
+    }
+}
+
+void RemoteController::HandleMovementControl(const std::string& command)
+{
+    // 预留接口：用于处理其他运动控制命令
+    // 例如：控制leg、waist、wheel等部位的运动
+    // 未来可以扩展支持更多遥控器命令
+    
+    // 当前实现：记录未知命令
+    RCLCPP_DEBUG(node_->get_logger(), "Unknown or unhandled movement command: '%s'", command.c_str());
+}
+
+bool RemoteController::IsDuplicateCommand(const std::string& command)
+{
+    return command == lastCommand_;
+}
+
+void RemoteController::UpdateLastCommand(const std::string& command)
+{
+    lastCommand_ = command;
+}
+
--- a/src/robot_control/src/core/robot_control_manager.cpp
+++ b/src/robot_control/src/core/robot_control_manager.cpp
@@ -125,6 +125,164 @@ bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double m
    return waist_controller_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);
 }

+bool RobotControlManager::SetArmJointCommands(int8_t body_id, const std::vector<double>& joint_angles)
+{
+    // Arm关节索引（左臂从9开始，右臂从15开始，每个臂6个关节）
+    const int LEFT_ARM_JOINT_START_INDEX = 9;
+    const int RIGHT_ARM_JOINT_START_INDEX = 15;
+    const int USED_ARM_DOF = 6;
+    
+    if (joint_angles.size() < USED_ARM_DOF) {
+        return false;
+    }
+    
+    int start_index = (body_id == 0) ? LEFT_ARM_JOINT_START_INDEX : RIGHT_ARM_JOINT_START_INDEX;
+    
+    // 确保jointCommands_数组足够大
+    if (jointCommands_.data.size() < static_cast<size_t>(start_index + USED_ARM_DOF)) {
+        jointCommands_.data.resize(start_index + USED_ARM_DOF, 0.0);
+    }
+    
+    // 设置关节命令（单位：度）
+    for (int i = 0; i < USED_ARM_DOF; ++i) {
+        jointCommands_.data[start_index + i] = joint_angles[i];
+    }
+    
+    return true;
+}
+
+bool RobotControlManager::InitializeArmMoveIt(rclcpp::Node* node)
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return false;
+    }
+
+    // 初始化左臂和右臂的MoveIt
+    // 注意：当前ArmControl只支持一个move_group，如果需要支持双臂，
+    // 可能需要两个ArmControl实例，或者修改ArmControl支持多个move_group
+    // 这里先假设只有一个arm_controller，使用"arm_left"作为默认名称
+    // TODO: 根据实际需求调整，可能需要支持"arm_left"和"arm_right"
+    
+    std::string move_group_name = "arm_left";  // 默认使用左臂
+    return arm_controller_->InitializeMoveIt(node, move_group_name);
+}
+
+bool RobotControlManager::PlanArmMotion(
+    const ArmMotionParams& arm_params,
+    double velocity_scaling,
+    double acceleration_scaling,
+    std::vector<std::vector<double>>& trajectory_points)
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return false;
+    }
+
+    if (!arm_controller_->IsMoveItInitialized()) {
+        return false;
+    }
+
+    if (arm_params.motion_type == arm_params.MOVEJ) {
+        // 关节空间运动
+        if (arm_params.target_joints.empty()) {
+            return false;
+        }
+        return arm_controller_->PlanJointMotion(
+            arm_params.arm_id,
+            arm_params.target_joints,  // 已经是弧度
+            velocity_scaling,
+            acceleration_scaling,
+            trajectory_points);
+    } else if (arm_params.motion_type == arm_params.MOVEL) {
+        // 笛卡尔空间运动
+        return arm_controller_->PlanCartesianMotion(
+            arm_params.arm_id,
+            arm_params.target_pose,
+            velocity_scaling,
+            acceleration_scaling,
+            trajectory_points);
+    }
+
+    return false;
+}
+
+void RobotControlManager::StoreArmTrajectory(
+    const ArmMotionParams& arm_params,
+    const std::vector<std::vector<double>>& trajectory_points)
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return;
+    }
+
+    arm_controller_->StoreTrajectory(trajectory_points, arm_params.arm_id);
+}
+
+bool RobotControlManager::GetArmInterpolatedPoint(
+    uint8_t arm_id,
+    double dt,
+    std::vector<double>& joint_positions)
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return false;
+    }
+
+    return arm_controller_->GetInterpolatedPoint(arm_id, dt, joint_positions);
+}
+
+bool RobotControlManager::HasActiveArmTrajectory(uint8_t arm_id) const
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return false;
+    }
+
+    return arm_controller_->HasActiveTrajectory(arm_id);
+}
+
+bool RobotControlManager::MoveArm(double dt)
+{
+    if (!arm_controller_enabled_ || !arm_controller_) {
+        return true;  // 没有arm控制器，认为已完成
+    }
+
+    // 检查是否有左臂轨迹在执行
+    bool left_arm_active = HasActiveArmTrajectory(0);
+    bool right_arm_active = HasActiveArmTrajectory(1);
+
+    if (!left_arm_active && !right_arm_active) {
+        return true;  // 没有轨迹在执行，认为已完成
+    }
+
+    // 处理左臂轨迹
+    if (left_arm_active) {
+        std::vector<double> left_joint_positions;
+        if (GetArmInterpolatedPoint(0, dt, left_joint_positions)) {
+            // 还有更多点位，转换并设置关节命令
+            std::vector<double> left_joint_degrees;
+            left_joint_degrees.reserve(left_joint_positions.size());
+            for (double rad : left_joint_positions) {
+                left_joint_degrees.push_back(rad * 180.0 / M_PI);  // 弧度转度
+            }
+            SetArmJointCommands(0, left_joint_degrees);
+        }
+    }
+
+    // 处理右臂轨迹
+    if (right_arm_active) {
+        std::vector<double> right_joint_positions;
+        if (GetArmInterpolatedPoint(1, dt, right_joint_positions)) {
+            // 还有更多点位，转换并设置关节命令
+            std::vector<double> right_joint_degrees;
+            right_joint_degrees.reserve(right_joint_positions.size());
+            for (double rad : right_joint_positions) {
+                right_joint_degrees.push_back(rad * 180.0 / M_PI);  // 弧度转度
+            }
+            SetArmJointCommands(1, right_joint_degrees);
+        }
+    }
+
+    // 检查是否还有轨迹在执行
+    return !HasActiveArmTrajectory(0) && !HasActiveArmTrajectory(1);
+}
+
 Float64MultiArray RobotControlManager::GetWheelCommands()
 {
    return wheelCommands_;
--- a/src/robot_control/src/core/robot_control_node.cpp
+++ b/src/robot_control/src/core/robot_control_node.cpp
@@ -1,6 +1,7 @@
 #include <thread>
 #include "rclcpp/rclcpp.hpp"
 #include "core/robot_control_node.hpp"
+#include "actions/action_manager.hpp"
 #include "core/motion_parameters.hpp"
 #include "core/common_enum.hpp"

@@ -14,11 +15,8 @@ namespace fs = std::filesystem;
 RobotControlNode::RobotControlNode() : Node("robot_control_node")
 {
    RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
-    isJogMode_ = false;
-    jogDirection_ = 0;
-    jogIndex_ = 0;
    lastSpeed_ = 51;
-    isStopping_ = false;
+    // 注意：isJogMode_, jogDirection_, jogIndex_, isStopping_, lastJoyCommand_ 已迁移到 RemoteController
 
    // 初始化数据文件（设置路径，确保目录存在）
    #if RECORD_FLAG
@@ -37,7 +35,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
    // 创建发布器和客户端（ActionManager 需要）
    motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
    motorParamClient_ = this->create_client<MotorParam>("/motor_param");
-    ethercatSetPub_ = this->create_publisher<std_msgs::msg::String>("/ethercat/set", 10);
+    jointTrajectoryPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/joint_trajectory", 10);
    imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
    jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
    wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
@@ -45,7 +43,10 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
    controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));

    // 初始化 ActionManager（在创建发布器和客户端之后）
-    auto is_jog_mode_func = [this]() -> bool { return isJogMode_; };
+    // 注意：is_jog_mode_func 使用一个临时的 lambda，稍后会被 RemoteController 覆盖
+    auto is_jog_mode_func = [this]() -> bool { 
+        return remoteController_ ? remoteController_->IsJogMode() : false; 
+    };
    action_manager_ = std::make_unique<ActionManager>(
        this,
        robotControlManager_,
@@ -54,6 +55,11 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
        motorParamClient_);
    action_manager_->initialize();

+    // 初始化 RemoteController（需要在 ActionManager 之前，因为 ActionManager 需要 is_jog_mode_func）
+    remoteController_ = std::make_unique<RemoteController>(
+        this,
+        robotControlManager_);
+
    lastTime_ = this->now();  // 初始化时间
    std::cout << "RobotFsm Node is created finished!" << std::endl;
 }
@@ -124,6 +130,16 @@ void RobotControlNode::ControlLoop()
        }
    }

+    // 处理arm轨迹执行（在主循环中根据时间周期依次获取插补点位并下发）
+    if (action_manager_->is_dual_arm_executing())
+    {
+        if(robotControlManager_.MoveArm(dt_sec))
+        {
+            // 轨迹执行完成
+            action_manager_->set_dual_arm_executing(false);
+        }
+    }
+
    if (robotControlManager_.RobotInitFinished())
    {
        Publish_joint_trajectory();
@@ -133,9 +149,9 @@ void RobotControlNode::ControlLoop()
 void RobotControlNode::Publish_joint_trajectory() 
 {
    std_msgs::msg::Float64MultiArray cmd_msg;
-    if (isJogMode_)
+    if (remoteController_ && remoteController_->IsJogMode())
    {
-        robotControlManager_.JogAxis(jogIndex_, jogDirection_);
+        robotControlManager_.JogAxis(remoteController_->GetJogIndex(), remoteController_->GetJogDirection());
        cmd_msg = robotControlManager_.GetJointCommands();
    }
    else
@@ -158,119 +174,45 @@ void RobotControlNode::Publish_joint_trajectory()

    #endif

-    std_msgs::msg::String positionMsg;
-    std::stringstream msg_stream; 
+    // 创建 JointTrajectory 消息
+    trajectory_msgs::msg::JointTrajectory trajectory_msg;
+    trajectory_msg.header.stamp = this->now();
+    trajectory_msg.header.frame_id = "base_link";
+    
    size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
-
+    
+    // 设置关节名称（需要根据实际配置）
+    trajectory_msg.joint_names.resize(total_joints);
    for (size_t i = 0; i < total_joints; ++i)
    {
-        double current_pos = cmd_msg.data[i];
-
-        msg_stream << i << " pos " << current_pos;
-
-        if (i != total_joints - 1)
-        {
-            msg_stream << "; ";
-        }
+        trajectory_msg.joint_names[i] = "joint_" + std::to_string(i);
    }
-    positionMsg.data = msg_stream.str();
-
-    // std::cout << "publishing joint trajectory: " << positionMsg.data << std::endl;
-
-    ethercatSetPub_->publish(positionMsg);
+    
+    // 创建轨迹点
+    trajectory_msgs::msg::JointTrajectoryPoint point;
+    point.positions.resize(total_joints);
+    point.velocities.resize(total_joints, 0.0);
+    point.accelerations.resize(total_joints, 0.0);
+    
+    for (size_t i = 0; i < total_joints; ++i)
+    {
+        point.positions[i] = cmd_msg.data[i];
+    }
+    
+    point.time_from_start.sec = 0;
+    point.time_from_start.nanosec = 0;
+    
+    trajectory_msg.points.push_back(point);
+    
+    // 发布关节轨迹
+    jointTrajectoryPub_->publish(trajectory_msg);
 }

-void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
-    if (!msg) {  // 检查消息是否有效
-        RCLCPP_WARN(this->get_logger(), "收到空的joy消息,忽略");
-        return;
-    }
-
-    // 解析消息内容
-    std::string command = msg->data;
-    static std::string last_msg="";
-    if(last_msg!=command)
-    	last_msg=command;
-    else
-    	return;
-
-    if (command == "RB,1") {
-        if (isJogMode_ && jogDirection_ == 0)
-        {
-            if (jogIndex_ < robotControlManager_.GetMotionParameters().total_joints_ - 1)
-            {
-                jogIndex_ ++;
-                std::cout << "switch to jog axis: " << jogIndex_ + 1  << std::endl;
-            }
-            else
-            {
-                std::cout << "Reach the max joint, can't switch to next axis: " << jogIndex_ + 1  << std::endl;
-            }
-        }
-    }
-    else if (command == "LB,1") {
-        if (isJogMode_ && jogDirection_ == 0)
-        {
-            if (jogIndex_ > 0)
-            {
-                jogIndex_ --;
-                std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
-            }
-            else
-            {
-                std::cout << "Reach the min joint, can't switch to previous axis: " << jogIndex_ - 1 << std::endl;
-            }
-        }
-    }
-    else if (command == "A,1") {
-        if (!robotControlManager_.IsMoving(MovementPart::ALL))
-        {
-            isJogMode_ = 1;
-            std::cout << "jog mode on" << std::endl;
-        }
-        else
-        {
-            std::cout << "robot is moving, can't switch jog mode" << std::endl;
-        }
-    }
-    else if (command == "B,1") {
-        if (!robotControlManager_.IsMoving(MovementPart::ALL))
-        {
-            isJogMode_ = 0;
-            std::cout << "jog mode OFF" << std::endl;
-        }
-        else
-        {
-            std::cout << "robot is moving, can't switch jog mode" << std::endl;
-        }
-    }
-    else if (command == "方向垂直,-1.0") {
-        if (isJogMode_)
-        {
-            jogDirection_ = -1;
-            std::cout << "jog axis in negative direction: " << jogIndex_ + 1 << std::endl;
-        }
-    }
-    else if (command == "方向垂直,1.0") {
-        if (isJogMode_)
-        {
-            jogDirection_ = 1;
-            std::cout << "jog axis in positive direction: " << jogIndex_ + 1 << std::endl;
-        }
-    }
-    else if (command == "方向垂直,0.0") {
-        if (isJogMode_)
-        {
-            jogDirection_ = 0;
-            std::cout << "jog axis stopped: " << jogIndex_ + 1 << std::endl;
-        }
-    }
-    else if (command == "X,1") {
-        if (!isJogMode_)
-        {
-            isStopping_ = true;
-            std::cout << "stop robot" << std::endl;
-        }
+void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) 
+{
+    // 委托给 RemoteController 处理
+    if (remoteController_) {
+        remoteController_->ProcessCommand(msg);
    }
 }

--- a/src/robot_control/src/utils/right_arm_joint_test.cpp
+++ b/src/robot_control/src/utils/right_arm_joint_test.cpp
@@ -0,0 +1,102 @@
+/**
+ * @file right_arm_joint_test.cpp
+ * @brief 右臂关节测试节点 - 独立可执行文件
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <memory>
+#include <vector>
+
+#define USED_ARM_DOF 6
+
+class RightArmJointTestNode : public rclcpp::Node
+{
+public:
+    RightArmJointTestNode()
+    : Node("right_arm_joint_test")
+    {
+        auto node_shared = std::shared_ptr<rclcpp::Node>(this, [](auto *) {});
+        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+            node_shared, "arm_right");
+        
+        move_group_->setMaxVelocityScalingFactor(0.3);
+        move_group_->setMaxAccelerationScalingFactor(0.3);
+        
+        RCLCPP_INFO(this->get_logger(), "Right arm joint test node initialized");
+        
+        // 启动测试（延迟1秒）
+        test_timer_ = this->create_wall_timer(
+            std::chrono::seconds(1),
+            std::bind(&RightArmJointTestNode::run_test, this));
+    }
+
+private:
+    void run_test()
+    {
+        test_timer_->cancel(); // 只运行一次
+        
+        if (!move_group_) {
+            RCLCPP_ERROR(this->get_logger(), "MoveGroup interface not initialized");
+            rclcpp::shutdown();
+            return;
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "Starting right arm joint test");
+        
+        // 固定的右臂关节初始位置（6 关节），单位：rad
+        std::vector<double> home = {0.0, 1.57, 0.0, 0.0, 0.0, 0.0};
+        double amplitude = 1.57;
+        int cycles = 10;
+        
+        std::vector<std::string> joint_names = move_group_->getJointNames();
+        
+        for (size_t j = 0; j < joint_names.size() && j < USED_ARM_DOF; ++j) {
+            RCLCPP_INFO(this->get_logger(), "Testing joint %zu: %s", j, joint_names[j].c_str());
+            
+            for (int c = 0; c < cycles; ++c) {
+                // Move up
+                auto up = home;
+                up[j] += amplitude / 2.0;
+                move_group_->setJointValueTarget(up);
+                moveit::planning_interface::MoveGroupInterface::Plan p1;
+                if (move_group_->plan(p1) == moveit::core::MoveItErrorCode::SUCCESS) {
+                    move_group_->execute(p1);
+                } else {
+                    RCLCPP_WARN(this->get_logger(), "Failed to plan UP for joint %zu", j);
+                }
+                
+                // Move down
+                auto down = home;
+                down[j] -= amplitude / 2.0;
+                move_group_->setJointValueTarget(down);
+                moveit::planning_interface::MoveGroupInterface::Plan p2;
+                if (move_group_->plan(p2) == moveit::core::MoveItErrorCode::SUCCESS) {
+                    move_group_->execute(p2);
+                } else {
+                    RCLCPP_WARN(this->get_logger(), "Failed to plan DOWN for joint %zu", j);
+                }
+            }
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "Right arm joint test finished");
+        rclcpp::shutdown();
+    }
+
+    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
+    rclcpp::TimerBase::SharedPtr test_timer_;
+};
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    
+    auto node = std::make_shared<RightArmJointTestNode>();
+    
+    rclcpp::spin(node);
+    
+    rclcpp::shutdown();
+    return 0;
+}
				`@@ -0,0 +1 @@`
				`controller_joint_names: ['', 'left_arm_joint_1', 'left_arm_joint_2', 'left_arm_joint_3', 'left_arm_joint_4', 'left_arm_joint_5', 'left_arm_joint_6', 'right_arm_joint_1', 'right_arm_joint_2', 'right_arm_joint_3', 'right_arm_joint_4', 'right_arm_joint_5', 'right_arm_joint_6', ]`