gramaziokohler · ZacZhangzhuo · Jun 4, 2023 · Jun 4, 2023 · Jun 11, 2023
diff --git a/.setup_assistant b/.setup_assistant
@@ -1,11 +1,26 @@
 moveit_setup_assistant_config:
-  URDF:
-    package: abb_crb15000_support
-    relative_path: urdf/crb15000_5_95.xacro
-    xacro_args: ""
-  SRDF:
-    relative_path: config/abb_crb15000_5_95.srdf
-  CONFIG:
-    author_name: Gonzalo Casas
-    author_email: [email protected]
-    generated_timestamp: 1648849764
+  urdf:
+    package: abb_irb4600_40_255
+    relative_path: urdf/abb_irb4600_40_255.urdf
+    xacro_args: "--inorder "
+  srdf:
+    relative_path: config/abb_irb4600_40_255.srdf
+  package_settings:
+    author_name: Zhu Zhang
+    author_email: [email protected]
+    generated_timestamp: 1686503172
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
+  modified_urdf:
+    xacros:
+      - control_xacro
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
@@ -0,0 +1,184 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/zac/ws_moveit2/install/pilz_industrial_motion_planner_testutils/include/**",
+        "/home/zac/ws_moveit2/install/pilz_industrial_motion_planner/include/**",
+        "/home/zac/ws_moveit2/install/moveit_visual_tools/include/**",
+        "/home/zac/ws_moveit2/install/moveit_task_constructor_visualization/include/**",
+        "/home/zac/ws_moveit2/install/moveit_task_constructor_demo/include/**",
+        "/home/zac/ws_moveit2/install/moveit_task_constructor_core/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_control_interface/include/**",
+        "/home/zac/ws_moveit2/install/moveit_simple_controller_manager/include/**",
+        "/home/zac/ws_moveit2/install/moveit_setup_assistant/include/**",
+        "/home/zac/ws_moveit2/install/moveit_setup_srdf_plugins/include/**",
+        "/home/zac/ws_moveit2/install/moveit_setup_core_plugins/include/**",
+        "/home/zac/ws_moveit2/install/moveit_setup_controllers/include/**",
+        "/home/zac/ws_moveit2/install/moveit_setup_app_plugins/include/**",
+        "/home/zac/ws_moveit2/install/moveit_setup_framework/include/**",
+        "/home/zac/ws_moveit2/install/moveit_servo/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_visualization/include/**",
+        "/home/zac/ws_moveit2/install/moveit_hybrid_planning/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_planning_interface/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_benchmarks/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_warehouse/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_robot_interaction/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_perception/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_move_group/include/**",
+        "/home/zac/ws_moveit2/install/moveit_planners_ompl/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_planning/include/**",
+        "/home/zac/ws_moveit2/install/moveit_ros_occupancy_map_monitor/include/**",
+        "/home/zac/ws_moveit2/install/moveit_kinematics/include/**",
+        "/home/zac/ws_moveit2/install/chomp_motion_planner/include/**",
+        "/home/zac/ws_moveit2/install/moveit_core/include/**",
+        "/home/zac/ws_moveit2/install/srdfdom/include/**",
+        "/home/zac/ws_moveit2/install/rviz_marker_tools/include/**",
+        "/home/zac/ws_moveit2/install/rosparam_shortcuts/include/**",
+        "/home/zac/ws_moveit2/install/moveit_task_constructor_msgs/include/**",
+        "/opt/ros/humble/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_compression_zstd/include/**",
+        "/home/zac/ros2_humble/install/mcap_vendor/include/**",
+        "/home/zac/ros2_humble/install/rviz_visual_testing_framework/include/**",
+        "/home/zac/ros2_humble/install/rviz_default_plugins/include/**",
+        "/home/zac/ros2_humble/install/rviz_common/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_transport/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_compression/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_cpp/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_storage_default_plugins/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_storage/include/**",
+        "/home/zac/ros2_humble/install/camera_info_manager/include/**",
+        "/home/zac/ros2_humble/install/camera_calibration_parsers/include/**",
+        "/home/zac/ros2_humble/install/interactive_markers/include/**",
+        "/home/zac/ros2_humble/install/visualization_msgs/include/**",
+        "/home/zac/ros2_humble/install/robot_state_publisher/include/**",
+        "/home/zac/ros2_humble/install/kdl_parser/include/**",
+        "/home/zac/ros2_humble/install/urdf/include/**",
+        "/home/zac/ros2_humble/install/urdf_parser_plugin/include/**",
+        "/home/zac/ros2_humble/install/turtlesim/include/**",
+        "/home/zac/ros2_humble/install/tf2_sensor_msgs/include/**",
+        "/home/zac/ros2_humble/install/tf2_kdl/include/**",
+        "/home/zac/ros2_humble/install/tf2_geometry_msgs/include/**",
+        "/home/zac/ros2_humble/install/tf2_eigen/include/**",
+        "/home/zac/ros2_humble/install/tf2_bullet/include/**",
+        "/home/zac/ros2_humble/install/tf2_ros/include/**",
+        "/home/zac/ros2_humble/install/tf2_msgs/include/**",
+        "/home/zac/ros2_humble/install/test_msgs/include/**",
+        "/home/zac/ros2_humble/install/ros2cli_test_interfaces/include/**",
+        "/home/zac/ros2_humble/install/image_transport/include/**",
+        "/home/zac/ros2_humble/install/message_filters/include/**",
+        "/home/zac/ros2_humble/install/laser_geometry/include/**",
+        "/home/zac/ros2_humble/install/rclcpp_action/include/**",
+        "/home/zac/ros2_humble/install/rcl_action/include/**",
+        "/home/zac/ros2_humble/install/example_interfaces/include/**",
+        "/home/zac/ros2_humble/install/action_tutorials_interfaces/include/**",
+        "/home/zac/ros2_humble/install/action_msgs/include/**",
+        "/home/zac/ros2_humble/install/unique_identifier_msgs/include/**",
+        "/home/zac/ros2_humble/install/trajectory_msgs/include/**",
+        "/home/zac/ros2_humble/install/tlsf_cpp/include/**",
+        "/home/zac/ros2_humble/install/rqt_gui_cpp/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_test_common/include/**",
+        "/home/zac/ros2_humble/install/rclcpp_lifecycle/include/**",
+        "/home/zac/ros2_humble/install/logging_demo/include/**",
+        "/home/zac/ros2_humble/install/image_tools/include/**",
+        "/home/zac/ros2_humble/install/rclcpp_components/include/**",
+        "/home/zac/ros2_humble/install/intra_process_demo/include/**",
+        "/home/zac/ros2_humble/install/examples_rclcpp_cbg_executor/include/**",
+        "/home/zac/ros2_humble/install/rclcpp/include/**",
+        "/home/zac/ros2_humble/install/rcl_lifecycle/include/**",
+        "/home/zac/ros2_humble/install/libstatistics_collector/include/**",
+        "/home/zac/ros2_humble/install/rcl/include/**",
+        "/home/zac/ros2_humble/install/rmw_fastrtps_dynamic_cpp/include/**",
+        "/home/zac/ros2_humble/install/rmw_fastrtps_cpp/include/**",
+        "/home/zac/ros2_humble/install/rmw_fastrtps_shared_cpp/include/**",
+        "/home/zac/ros2_humble/install/tracetools/include/**",
+        "/home/zac/ros2_humble/install/tlsf/include/**",
+        "/home/zac/ros2_humble/install/qt_gui_cpp/include/**",
+        "/home/zac/ros2_humble/install/pluginlib/include/**",
+        "/home/zac/ros2_humble/install/tf2_eigen_kdl/include/**",
+        "/home/zac/ros2_humble/install/tf2/include/**",
+        "/home/zac/ros2_humble/install/stereo_msgs/include/**",
+        "/home/zac/ros2_humble/install/std_srvs/include/**",
+        "/home/zac/ros2_humble/install/shape_msgs/include/**",
+        "/home/zac/ros2_humble/install/map_msgs/include/**",
+        "/home/zac/ros2_humble/install/sensor_msgs/include/**",
+        "/home/zac/ros2_humble/install/nav_msgs/include/**",
+        "/home/zac/ros2_humble/install/diagnostic_msgs/include/**",
+        "/home/zac/ros2_humble/install/geometry_msgs/include/**",
+        "/home/zac/ros2_humble/install/actionlib_msgs/include/**",
+        "/home/zac/ros2_humble/install/std_msgs/include/**",
+        "/home/zac/ros2_humble/install/statistics_msgs/include/**",
+        "/home/zac/ros2_humble/install/shared_queues_vendor/include/**",
+        "/home/zac/ros2_humble/install/rviz_rendering/include/**",
+        "/home/zac/ros2_humble/install/rttest/include/**",
+        "/home/zac/ros2_humble/install/rmw_connextdds_common/include/**",
+        "/home/zac/ros2_humble/install/rosgraph_msgs/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_interfaces/include/**",
+        "/home/zac/ros2_humble/install/rmw_dds_common/include/**",
+        "/home/zac/ros2_humble/install/composition_interfaces/include/**",
+        "/home/zac/ros2_humble/install/rcl_interfaces/include/**",
+        "/home/zac/ros2_humble/install/pendulum_msgs/include/**",
+        "/home/zac/ros2_humble/install/lifecycle_msgs/include/**",
+        "/home/zac/ros2_humble/install/builtin_interfaces/include/**",
+        "/home/zac/ros2_humble/install/rosidl_typesupport_cpp/include/**",
+        "/home/zac/ros2_humble/install/rosidl_typesupport_introspection_cpp/include/**",
+        "/home/zac/ros2_humble/install/rosidl_typesupport_c/include/**",
+        "/home/zac/ros2_humble/install/rosidl_typesupport_introspection_c/include/**",
+        "/home/zac/ros2_humble/install/rosidl_typesupport_fastrtps_c/include/**",
+        "/home/zac/ros2_humble/install/rosidl_typesupport_fastrtps_cpp/include/**",
+        "/home/zac/ros2_humble/install/rosidl_runtime_cpp/include/**",
+        "/home/zac/ros2_humble/install/rcl_yaml_param_parser/include/**",
+        "/home/zac/ros2_humble/install/rmw/include/**",
+        "/home/zac/ros2_humble/install/rosidl_runtime_c/include/**",
+        "/home/zac/ros2_humble/install/rosidl_typesupport_interface/include/**",
+        "/home/zac/ros2_humble/install/rosbag2_storage_mcap_testdata/include/**",
+        "/home/zac/ros2_humble/install/resource_retriever/include/**",
+        "/home/zac/ros2_humble/install/class_loader/include/**",
+        "/home/zac/ros2_humble/install/rcpputils/include/**",
+        "/home/zac/ros2_humble/install/rcl_logging_interface/include/**",
+        "/home/zac/ros2_humble/install/rcutils/include/**",
+        "/home/zac/ros2_humble/install/performance_test_fixture/include/**",
+        "/home/zac/ros2_humble/install/mimick_vendor/include/**",
+        "/home/zac/ros2_humble/install/libyaml_vendor/include/**",
+        "/home/zac/ros2_humble/install/keyboard_handler/include/**",
+        "/home/zac/ros2_humble/install/ament_index_cpp/include/**",
+        "/home/zac/ws_moveit2/src/hello_moveit/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_planners/chomp/chomp_interface/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_planners/chomp/chomp_motion_planner/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_planners/pilz_industrial_motion_planner/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_planners/pilz_industrial_motion_planner_testutils/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_plugins/moveit_ros_control_interface/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_plugins/moveit_simple_controller_manager/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_ros/benchmarks/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_ros/move_group/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_ros/moveit_servo/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_ros/occupancy_map_monitor/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_ros/robot_interaction/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_ros/warehouse/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_app_plugins/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_assistant/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_controllers/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_core_plugins/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_framework/include/**",
+        "/home/zac/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/include/**",
+        "/home/zac/ws_moveit2/src/moveit_task_constructor/core/include/**",
+        "/home/zac/ws_moveit2/src/moveit_task_constructor/demo/include/**",
+        "/home/zac/ws_moveit2/src/moveit_task_constructor/rviz_marker_tools/include/**",
+        "/home/zac/ws_moveit2/src/moveit_visual_tools/include/**",
+        "/home/zac/ws_moveit2/src/rosparam_shortcuts/include/**",
+        "/home/zac/ws_moveit2/src/srdfdom/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,10 +1,11 @@
-cmake_minimum_required(VERSION 3.1.3)
+cmake_minimum_required(VERSION 3.22)
 project(abb_crb15000_5_95_moveit_config)
 
-find_package(catkin REQUIRED)
+find_package(ament_cmake REQUIRED)
 
-catkin_package()
+ament_package()
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
   PATTERN "setup_assistant.launch" EXCLUDE)
-install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
diff --git a/README.md b/README.md
@@ -1 +1,3 @@
-# abb_crb15000_5_95_moveit_config
+# abb_crb15000_5_95_moveit_config
+
+ROS2 package.
diff --git a/config/abb_irb4600_40_255.ros2_control.xacro b/config/abb_irb4600_40_255.ros2_control.xacro
@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="abb_irb4600_40_255_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="joint_1">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_1']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_2">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_2']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_3">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_3']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_4">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_4']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_5">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_5']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_6">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_6']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+
+        </ros2_control>
+    </xacro:macro>
+</robot>
diff --git a/config/abb_crb15000_5_95.srdf → config/abb_irb4600_40_255.srdf b/config/abb_crb15000_5_95.srdf → config/abb_irb4600_40_255.srdf
@@ -3,7 +3,7 @@
     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="abb_crb15000_5_95">
+<robot name="abb_irb4600_40_255">
     <!--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-->
@@ -12,16 +12,26 @@
     <group name="manipulator">
         <chain base_link="base_link" tip_link="tool0"/>
     </group>
+    <group name="manipulator_lma">
+        <chain base_link="base_link" tip_link="tool0"/>
+    </group>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="flange" parent_link="tool0" group="manipulator"/>
     <!--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="virtual_joint" type="fixed" parent_frame="world" child_link="base_link"/>
+    <virtual_joint name="fixed_base" type="fixed" parent_frame="world" 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="link_1" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="link_3" reason="Never"/>
     <disable_collisions link1="link_1" link2="link_2" reason="Adjacent"/>
     <disable_collisions link1="link_1" link2="link_3" reason="Never"/>
     <disable_collisions link1="link_2" link2="link_3" reason="Adjacent"/>
+    <disable_collisions link1="link_2" link2="link_4" reason="Never"/>
+    <disable_collisions link1="link_2" link2="link_5" reason="Never"/>
+    <disable_collisions link1="link_2" link2="link_6" reason="Never"/>
     <disable_collisions link1="link_3" link2="link_4" reason="Adjacent"/>
     <disable_collisions link1="link_3" link2="link_5" reason="Never"/>
     <disable_collisions link1="link_3" link2="link_6" reason="Never"/>
     <disable_collisions link1="link_4" link2="link_5" reason="Adjacent"/>
+    <disable_collisions link1="link_4" link2="link_6" reason="Default"/>
     <disable_collisions link1="link_5" link2="link_6" reason="Adjacent"/>
 </robot>
diff --git a/config/abb_irb4600_40_255.urdf.xacro b/config/abb_irb4600_40_255.urdf.xacro
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="abb_irb4600_40_255">
+    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
+
+    <!-- Import abb_irb4600_40_255 urdf file -->
+    <xacro:include filename="$(find abb_irb4600_40_255)/urdf/abb_irb4600_40_255.urdf" />
+
+    <!-- Import control_xacro -->
+    <xacro:include filename="abb_irb4600_40_255.ros2_control.xacro" />
+
+
+    <xacro:abb_irb4600_40_255_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
+
+</robot>
diff --git a/config/cartesian_limits.yaml b/config/cartesian_limits.yaml