initial commit

.gitignore

@@ -0,0 +1,5 @@
+build/
+cmake-build-debug/
+data/*
+!data/.gitkeep
+.idea/

CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 3.10)
+
+project(m20_sdk_deploy)
+
+# 设置C++标准
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# 默认构建类型
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+# 警告忽略和编译选项
+add_definitions(-w)
+add_compile_options(-fPIC)
+
+# 配置选项
+set(BUILD_PLATFORM "x86" CACHE STRING "Target platform (x86 or arm)")
+
+if(BUILD_PLATFORM STREQUAL "arm")
+  set(CMAKE_C_COMPILER "aarch64-linux-gnu-gcc")
+  set(CMAKE_CXX_COMPILER "aarch64-linux-gnu-g++") 
+endif()
+
+# 查找依赖包
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(drdds REQUIRED)
+
+set(THIRD_PARTY ${CMAKE_CURRENT_SOURCE_DIR}/third_party)
+
+include_directories(
+  include/types
+  include/utils
+  interface/robot
+  interface/user_command
+  interface/network
+  state_machine
+  run_policy
+  ${THIRD_PARTY}/eigen/
+  ${THIRD_PARTY}/gamepad/include
+  ${THIRD_PARTY}/onnxruntime/${BUILD_PLATFORM}/include
+)
+
+# 子目录
+add_subdirectory(interface)
+
+# Keep the default RUNPATH behavior so a correctly sourced ROS environment can
+# override stale build-machine library paths when necessary.
+set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+
+set(CMAKE_INSTALL_RPATH 
+  ${THIRD_PARTY}/onnxruntime/${BUILD_PLATFORM}/lib/
+  ../lib/onnxruntime/
+)
+
+file(GLOB_RECURSE STATE_MACHINE_SRC "state_machine/*.c*")
+
+add_executable(rl_deploy main.cpp ${STATE_MACHINE_SRC})
+
+add_library(onnxruntime SHARED IMPORTED)
+set_target_properties(onnxruntime PROPERTIES IMPORTED_LOCATION "${THIRD_PARTY}/onnxruntime//${BUILD_PLATFORM}/lib/libonnxruntime.so")
+
+ament_target_dependencies(rl_deploy
+  rclcpp
+  drdds
+)
+if(BUILD_PLATFORM STREQUAL "arm")
+  target_link_libraries(rl_deploy
+    interface
+    onnxruntime
+    -lpthread
+    -lm
+    -lrt
+    -ldl
+    -lstdc++
+  )
+else()
+  target_link_libraries(rl_deploy
+    interface
+    onnxruntime
+    -lpthread
+    -lm
+    -lrt
+    -ldl
+    -lstdc++
+  )
+endif()
+
+
+install(TARGETS rl_deploy
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

M20_description/fire_ant_mjcf/mjcf/fire_ant.xml

@@ -0,0 +1,140 @@
+<mujoco model="fire_ant">
+  <compiler angle="radian" autolimits="true" balanceinertia="true"/>
+  <option timestep="0.001" gravity="0 0 -9.81" integrator="implicitfast">
+    <flag multiccd="enable"/>
+  </option>
+
+  <default>
+    <geom density="0" condim="3" solref="0.005 1" solimp="0.95 0.99 0.001"
+          friction="0.6 0.005 0.0001" contype="1" conaffinity="1"/>
+    <joint damping="0" armature="0" frictionloss="0"/>
+    <default class="wheel">
+      <!-- Wheel-ground friction is intentionally independent from the other links. -->
+      <geom type="cylinder" size="0.096 0.0415" quat="0.70710678 0.70710678 0 0"
+            friction="1.0 0.005 0.0001" rgba="0.08 0.08 0.08 1"/>
+      <joint axis="0 -1 0" armature="0.00243216" frictionloss="1.0"/>
+    </default>
+  </default>
+
+  <asset>
+    <texture name="floor_tex" type="2d" builtin="checker" rgb1="0.22 0.27 0.31"
+             rgb2="0.12 0.15 0.18" width="512" height="512"/>
+    <material name="floor_mat" texture="floor_tex" texrepeat="2 2" texuniform="true"/>
+  </asset>
+
+  <worldbody>
+    <light directional="true" pos="-2 -1 4" dir="0.5 0.2 -1"/>
+    <geom name="floor" type="plane" size="30 8 0.1" material="floor_mat"
+          friction="1.0 0.005 0.0001" rgba="0.55 0.6 0.65 1"/>
+
+    <body name="base_link" pos="0 0 0.306">
+      <freejoint name="floating_base"/>
+      <inertial pos="0.00355106 0.00165076 -0.00346462" mass="15.882"
+                diaginertia="0.0880787 0.533816 0.563915"/>
+      <site name="base_site" pos="0 0 0" size="0.005"/>
+      <site name="imu_site" pos="0 0 0" size="0.005"/>
+      <geom name="base_collision" type="box" size="0.30 0.16 0.06" rgba="0.35 0.43 0.50 1"/>
+      <geom name="base_left_rail" type="cylinder" size="0.055 0.30" pos="0 0.13 0"
+            quat="0.70710678 0 0.70710678 0" rgba="0.22 0.28 0.34 1"/>
+      <geom name="base_right_rail" type="cylinder" size="0.055 0.30" pos="0 -0.13 0"
+            quat="0.70710678 0 0.70710678 0" rgba="0.22 0.28 0.34 1"/>
+
+      <body name="fl_knee" pos="0.30 0.14 0">
+        <inertial pos="0.00341917 0.00269124 -0.18126550" mass="0.612"
+                  diaginertia="0.005202 0.005202 0.0007344"/>
+        <joint name="fl_knee_joint" axis="0 -1 0" range="-2.792 2.809"
+               actuatorfrcrange="-75 75"/>
+        <geom name="fl_knee_collision" type="box" size="0.025 0.012 0.10"
+              pos="0.012 -0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
+        <geom name="fl_motor_collision" type="cylinder" size="0.04 0.045"
+              pos="0 0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
+        <body name="fl_wheel" pos="0 0.11 -0.21">
+          <inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
+          <joint name="fl_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
+          <geom name="fl_wheel_collision" class="wheel"/>
+        </body>
+      </body>
+
+      <body name="fr_knee" pos="0.30 -0.14 0">
+        <inertial pos="0.00354945 -0.00247884 -0.18008924" mass="0.612"
+                  diaginertia="0.005202 0.005202 0.0007344"/>
+        <joint name="fr_knee_joint" axis="0 -1 0" range="-2.792 2.809"
+               actuatorfrcrange="-75 75"/>
+        <geom name="fr_knee_collision" type="box" size="0.025 0.012 0.10"
+              pos="0.012 0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
+        <geom name="fr_motor_collision" type="cylinder" size="0.04 0.045"
+              pos="0 -0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
+        <body name="fr_wheel" pos="0 -0.11 -0.21">
+          <inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
+          <joint name="fr_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
+          <geom name="fr_wheel_collision" class="wheel"/>
+        </body>
+      </body>
+
+      <body name="hl_knee" pos="-0.30 0.14 0">
+        <inertial pos="-0.00354945 0.00247885 -0.18008926" mass="0.612"
+                  diaginertia="0.005202 0.005202 0.0007344"/>
+        <joint name="hl_knee_joint" axis="0 -1 0" range="-2.809 2.792"
+               actuatorfrcrange="-75 75"/>
+        <geom name="hl_knee_collision" type="box" size="0.025 0.012 0.10"
+              pos="-0.012 0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
+        <geom name="hl_motor_collision" type="cylinder" size="0.04 0.045"
+              pos="0 0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
+        <body name="hl_wheel" pos="0 0.11 -0.21">
+          <inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
+          <joint name="hl_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
+          <geom name="hl_wheel_collision" class="wheel"/>
+        </body>
+      </body>
+
+      <body name="hr_knee" pos="-0.30 -0.14 0">
+        <inertial pos="-0.00341053 -0.00270202 -0.18128749" mass="0.612"
+                  diaginertia="0.005202 0.005202 0.0007344"/>
+        <joint name="hr_knee_joint" axis="0 -1 0" range="-2.809 2.792"
+               actuatorfrcrange="-75 75"/>
+        <geom name="hr_knee_collision" type="box" size="0.025 0.012 0.10"
+              pos="-0.012 0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
+        <geom name="hr_motor_collision" type="cylinder" size="0.04 0.045"
+              pos="0 -0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
+        <body name="hr_wheel" pos="0 -0.11 -0.21">
+          <inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
+          <joint name="hr_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
+          <geom name="hr_wheel_collision" class="wheel"/>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <!-- Robot/DDS order: FL knee/wheel, FR knee/wheel, HL knee/wheel, HR knee/wheel. -->
+  <actuator>
+    <motor name="fl_knee_motor" joint="fl_knee_joint" gear="1" ctrlrange="-75 75"/>
+    <motor name="fl_wheel_motor" joint="fl_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
+    <motor name="fr_knee_motor" joint="fr_knee_joint" gear="1" ctrlrange="-75 75"/>
+    <motor name="fr_wheel_motor" joint="fr_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
+    <motor name="hl_knee_motor" joint="hl_knee_joint" gear="1" ctrlrange="-75 75"/>
+    <motor name="hl_wheel_motor" joint="hl_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
+    <motor name="hr_knee_motor" joint="hr_knee_joint" gear="1" ctrlrange="-75 75"/>
+    <motor name="hr_wheel_motor" joint="hr_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
+  </actuator>
+
+  <contact>
+    <exclude body1="base_link" body2="fl_knee"/>
+    <exclude body1="fl_knee" body2="fl_wheel"/>
+    <exclude body1="base_link" body2="fr_knee"/>
+    <exclude body1="fr_knee" body2="fr_wheel"/>
+    <exclude body1="base_link" body2="hl_knee"/>
+    <exclude body1="hl_knee" body2="hl_wheel"/>
+    <exclude body1="base_link" body2="hr_knee"/>
+    <exclude body1="hr_knee" body2="hr_wheel"/>
+  </contact>
+
+  <sensor>
+    <framequat name="base_quat" objtype="site" objname="base_site"/>
+    <accelerometer name="base_acc" site="imu_site"/>
+    <gyro name="base_gyro" site="imu_site"/>
+  </sensor>
+
+  <keyframe>
+    <key name="home" qpos="0 0 0.306 1 0 0 0 0.5 0 0.5 0 -0.5 0 -0.5 0"/>
+  </keyframe>
+</mujoco>

M20_description/fire_ant_mjcf/mjcf/fire_ant_rough.xml

@@ -0,0 +1,18 @@
+<mujoco model="fire_ant_rough">
+  <include file="fire_ant.xml"/>
+  <worldbody>
+    <!-- A deterministic mixed obstacle course for Stage-2 sim2sim checks. -->
+    <geom name="step_08cm" type="box" pos="1.30 0 0.04" size="0.18 1.0 0.04"
+          friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.42 0.48 0.55 1"/>
+    <geom name="step_12cm" type="box" pos="1.66 0 0.06" size="0.18 1.0 0.06"
+          friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.46 0.52 0.59 1"/>
+    <geom name="step_16cm" type="box" pos="2.02 0 0.08" size="0.18 1.0 0.08"
+          friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.50 0.56 0.63 1"/>
+    <geom name="top_platform" type="box" pos="2.72 0 0.08" size="0.52 1.0 0.08"
+          friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.54 0.60 0.67 1"/>
+    <geom name="descending_step_10cm" type="box" pos="3.42 0 0.05" size="0.18 1.0 0.05"
+          friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.46 0.52 0.59 1"/>
+    <geom name="isolated_box" type="box" pos="4.30 0 0.07" size="0.16 0.55 0.07"
+          friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.73 0.42 0.20 1"/>
+  </worldbody>
+</mujoco>