feat: new mujoco simulator

RoboJackets · Jan 28, 2025 · b358cbe · b358cbe
1 parent 0a6bf08
commit b358cbe
Show file tree

Hide file tree

Showing 28 changed files with 7,262 additions and 850 deletions.
diff --git a/urc_arm/CMakeLists.txt b/urc_arm/CMakeLists.txt
@@ -10,48 +10,39 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
-find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
-find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(eigen3_cmake_module REQUIRED)
-find_package(Eigen3)
-find_package(pinocchio REQUIRED)
-find_package(realtime_tools REQUIRED)
-
-
-include_directories(
-  include
-)
+find_package(geometry_msgs REQUIRED)
+find_package(glfw3 REQUIRED)
 
-# Library creation
-add_library(${PROJECT_NAME} SHARED
-  src/arm_rt.cpp
+# Build and install simulator
+add_executable(
+  arm_mjsim
+  src/arm_mjsim.cpp
 )
+ament_target_dependencies(
+  arm_mjsim
 
-set(dependencies
   rclcpp
-  rclcpp_components
+  sensor_msgs
   std_msgs
-  std_srvs
   geometry_msgs
-  sensor_msgs
-  realtime_tools
-  pinocchio
-  Eigen3
+  std_srvs
 )
-
-ament_target_dependencies(${PROJECT_NAME}
-  ${dependencies}
+target_link_libraries(
+  arm_mjsim
+  ${CMAKE_SOURCE_DIR}/lib/mujoco/libmujoco.so.3.2.2
+  glfw
 )
-# target_link_libraries(${PROJECT_NAME} pinocchio)
-
-# Node Registration
-rclcpp_components_register_node(
-  ${PROJECT_NAME}
-  PLUGIN "urc_arm::ArmRT"
-  EXECUTABLE arm_rt_pinocchio
+target_include_directories(
+  arm_mjsim
+  PUBLIC
+  ${CMAKE_SOURCE_DIR}/lib/mujoco
+)
+install(
+  TARGETS arm_mjsim
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 # Install launch files.
@@ -60,14 +51,6 @@ install(
   DESTINATION share/${PROJECT_NAME}/
 )
 
-# Install library
-install(TARGETS
-  ${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the copyright linker

diff --git a/urc_arm/config/controller_config.yaml b/urc_arm/config/controller_config.yaml
@@ -1,11 +1,41 @@
-arm_mj_sim:
+arm_mjsim:
   ros__parameters:
-    model_dir: "/home/keseterg/Documents/RoboJackets/urc_ws/src/urc-software/urc_arm_models/urdf/walli_arm_v2_block_mjcf/walli_arm_v2_block.xml"
+    model_path: "/home/keseterg/Documents/RoboJackets/urc_ws/src/urc-software/urc_arm_models/urdf/walli_arm_v2_block_mjcf/walli_arm_v2_block.xml"
+    fps: 100.0
+    policy_rate_hz: 250.0
 
-arm_qp_controller:
+qp_ctrl:
   ros__parameters:
-    control_config:
-      control_topic: arm_position_controller/commands
+    robot_config:
+      model_path: "/home/keseterg/Documents/RoboJackets/urc_ws/src/urc-software/urc_arm_models/urdf/walli_arm_v2_block_mjcf/robot.urdf"
+      end_effector_frame_name: "ee_rod"
+    joints:
+      - base_yaw_joint
+      - upper_arm_joint
+      - fore_arm_joint
+      - ee_roll_joint
+      - ee_pitch_joint
+    gains:
+      base_yaw_joint:
+        kp: 10.0
+        kd: 5.0
+        torque_limit: [-116.82, 116.82]
+      upper_arm_joint:
+        kp: 50.0
+        kd: 10.0
+        torque_limit: [-116.82, 116.82]
+      fore_arm_joint:
+        kp: 15.0
+        kd: 1.0
+        torque_limit: [-116.82, 116.82]
+      ee_roll_joint:
+        kp: 0.7
+        kd: 0.001
+        torque_limit: [-116.82, 116.82]
+      ee_pitch_joint:
+        kp: 0.7
+        kd: 0.001
+        torque_limit: [-116.82, 116.82]
 
 controller_manager:
   ros__parameters:
@@ -23,5 +53,5 @@ arm_position_controller:
       - base_yaw_joint
       - upper_arm_joint
       - fore_arm_joint
-      - ee_roll_jont
+      - ee_roll_joint
       - ee_pitch_joint
diff --git a/urc_arm/include/urc_arm/arm_rt.hpp b/urc_arm/include/urc_arm/arm_rt.hpp
diff --git a/urc_arm/launch/mjsim.launch.py b/urc_arm/launch/mjsim.launch.py
@@ -4,60 +4,65 @@
 from launch import LaunchDescription
 from launch.actions import (
     IncludeLaunchDescription,
+    SetEnvironmentVariable
 )
-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")
+arm_models_dir = get_package_share_directory("urc_arm_models")
+robot_description_dir = os.path.join(
+    arm_models_dir, "urdf", "walli_arm_v2_block_mjcf", "robot.urdf"
+)
+with open(robot_description_dir, 'r+') as urdf_file:
+    robot_description = urdf_file.read()
 controller_config_file_dir = os.path.join(
     urc_arm_dir, "config", "controller_config.yaml"
 )
 
 
 def generate_launch_description():
-    # mujoco simulator
-    mjsim = Node(
+    qp_controller = Node(
         package="urc_arm_py",
-        executable="arm_sim_mj",
+        executable="arm_qp_control",
+        name="qp_ctrl",
         parameters=[controller_config_file_dir],
+        output="screen",
     )
-    control_node = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
+
+    mjsim = Node(
+        package="urc_arm",
+        executable="arm_mjsim",
         parameters=[controller_config_file_dir],
-        output="both",
-        remappings=[("~/robot_description", "/robot_description")],
+        remappings=[
+            ("~/joint_states", "/joint_states"),
+            ("~/torque_ctrl", "qp_ctrl/torque_ctrl")
+        ]
     )
     rsp_node = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         respawn=True,
         output="screen",
         parameters=[
-            moveit_config.robot_description,
             {
+                "robot_description": robot_description,
                 "publish_frequency": 100.0,
             },
-        ],
-        remappings=[("/joint_states", "/arm_joint_states")],
+        ]
     )
-    spawn_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["arm_position_controller"],
-        output="screen",
-    )
-    qp_controller = Node(
-        package="urc_arm_py",
-        executable="arm_qp_control",
-        parameters=[controller_config_file_dir],
-        output="screen",
+
+    foxglove_bridge = Node(
+        package="foxglove_bridge",
+        executable="foxglove_bridge",
+        output="log"
     )
 
     return LaunchDescription(
-        [rsp_node, control_node, mjsim, spawn_controller, qp_controller]
+        [
+            rsp_node,
+            mjsim,
+            qp_controller,
+            # foxglove_bridge
+        ]
     )
diff --git a/urc_arm/lib/mujoco/libmujoco.so.3.2.2 b/urc_arm/lib/mujoco/libmujoco.so.3.2.2