feat: testing mujoco simulation

urc_arm/CMakeLists.txt

@@ -56,7 +56,7 @@ rclcpp_components_register_node(
 
 # Install launch files.
 install(
-  DIRECTORY launch
+  DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}/
 )
 
@@ -68,14 +68,6 @@ install(TARGETS
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-# # Install Python modules
-# ament_python_install_package(${PROJECT_NAME})
-# # Install Python executables
-# install(PROGRAMS
-#   scripts/armjoy.py
-#   DESTINATION lib/${PROJECT_NAME}
-# )
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the copyright linker

urc_arm/launch/mjsim.launch.py

@@ -0,0 +1,59 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    IncludeLaunchDescription,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+moveit_config = MoveItConfigsBuilder(
+    "walli_arm_v2_block", package_name="walli_arm_v2_block_moveit_config"
+).to_moveit_configs()
+urc_arm_dir = get_package_share_directory("urc_arm")
+controller_config_file_dir = os.path.join(
+    urc_arm_dir, "config", "controller_config.yaml"
+)
+
+
+def generate_launch_description():
+    # mujoco simulator
+    mjsim = Node(package="urc_arm_py", executable="arm_sim_mj")
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[controller_config_file_dir],
+        output="both",
+        remappings=[("~/robot_description", "/robot_description")],
+    )
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        respawn=True,
+        output="screen",
+        parameters=[
+            moveit_config.robot_description,
+            {
+                "publish_frequency": 100.0,
+            },
+        ],
+        remappings=[("/joint_states", "/arm_joint_states")],
+    )
+
+    # spawn_controller = IncludeLaunchDescription(
+    #     # PythonLaunchDescriptionSource(
+    #     #     str(moveit_config.package_path / "launch/spawn_controllers.launch.py")
+    #     # ),
+    # )
+
+    return LaunchDescription(
+        [
+            rsp_node,
+            control_node,
+            mjsim,
+            # spawn_controller,
+        ]
+    )

urc_arm/launch/sim.launch.py

@@ -88,7 +88,7 @@ def generate_launch_description():
             # ),
             robot_state_publisher_launch,
             # movegroup_launch,
-            gazebo,
-            spawn_robot,
+            # gazebo,
+            # spawn_robot,
         ]
     )

urc_arm/package.xml

@@ -22,5 +22,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
+
   </export>
 </package>

urc_arm_models/urdf/walli_arm_v2_block_mjcf/MUJOCO_LOG.TXT

@@ -0,0 +1,6 @@
+Tue Nov  5 20:22:54 2024
+WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 4.3740.
+
+Tue Nov  5 20:24:40 2024
+WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 3.2000.
+

urc_arm_models/urdf/walli_arm_v2_block_mjcf/base_rotator.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "e8a82d08862cc1d66b265cd2",
+    "documentMicroversion": "a0df853c8107576c8e1d2fb6",
+    "elementId": "a50fbd2124f27847731afecb",
+    "fullConfiguration": "default",
+    "id": "MaZd8I+5bDUKrAWY2",
+    "isStandardContent": false,
+    "name": "base_rotator <1>",
+    "partId": "JID",
+    "suppressed": false,
+    "type": "Part"
+}

urc_arm_models/urdf/walli_arm_v2_block_mjcf/config.json

@@ -0,0 +1,10 @@
+{
+    "robotName": "walli_arm_v2_block",
+    "documentId": "e8a82d08862cc1d66b265cd2",
+    "outputFormat": "urdf",
+    "jointMaxEffort": 60,
+    "jointMaxVelocity": 20,
+    "mergeSTLs": "all",
+    "simplifySTLs" : "all",
+    "packageName": "urc_arm_models/urdf/walli_arm_v2_block"
+}

urc_arm_models/urdf/walli_arm_v2_block_mjcf/ee_rod.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "e8a82d08862cc1d66b265cd2",
+    "documentMicroversion": "a0df853c8107576c8e1d2fb6",
+    "elementId": "a50fbd2124f27847731afecb",
+    "fullConfiguration": "default",
+    "id": "MJYtYPlTYdHn2KkRS",
+    "isStandardContent": false,
+    "name": "ee_rod <1>",
+    "partId": "JrD",
+    "suppressed": false,
+    "type": "Part"
+}

urc_arm_models/urdf/walli_arm_v2_block_mjcf/ee_rotary.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "e8a82d08862cc1d66b265cd2",
+    "documentMicroversion": "a0df853c8107576c8e1d2fb6",
+    "elementId": "a50fbd2124f27847731afecb",
+    "fullConfiguration": "default",
+    "id": "M/k6Rrdl9yvCZjzqq",
+    "isStandardContent": false,
+    "name": "ee_rotary <1>",
+    "partId": "JfD",
+    "suppressed": false,
+    "type": "Part"
+}

urc_arm_models/urdf/walli_arm_v2_block_mjcf/fore_arm.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "e8a82d08862cc1d66b265cd2",
+    "documentMicroversion": "a0df853c8107576c8e1d2fb6",
+    "elementId": "a50fbd2124f27847731afecb",
+    "fullConfiguration": "default",
+    "id": "MPoTeGwxZWv83Aq1m",
+    "isStandardContent": false,
+    "name": "fore_arm <1>",
+    "partId": "JaD",
+    "suppressed": false,
+    "type": "Part"
+}