Added plotting code

.circleci/config.yml

@@ -1,8 +1,8 @@
 jobs:
   build-and-test:
-    working_directory: /catkin_ws/src
+    working_directory: /ros2_ws/src
     docker:
-      - image: osrf/ros:melodic-desktop-full
+      - image: osrf/ros:jazzy-desktop-full
     steps:
       - checkout
       - run:
@@ -11,30 +11,30 @@ jobs:
       - run:
           name: "Call setup script and build"
           command: |
-            source /opt/ros/melodic/setup.bash
+            source /opt/ros/jazzy/setup.bash
             chmod +x setup.sh
             ./setup.sh
-            cd /catkin_ws
-            catkin build -j4 -l4
+            cd /ros2_ws
+            colcon build -j4 -l4
       - run:
           name: "Load ROS params in the background"
           command: |
-            source /opt/ros/melodic/setup.bash
-            source /catkin_ws/devel/setup.bash
-            roslaunch quad_utils load_global_params.launch load_robot_params:=true
+            source /opt/ros/jazzy/setup.bash
+            source /ros2_ws/install/setup.bash
+            ros2 launch quad_utils load_global_params.launch load_robot_params:=true
           background: true
       - run:
           name: "Run tests"
           command: |
-            cd /catkin_ws
-            source /opt/ros/melodic/setup.bash
-            source /catkin_ws/devel/setup.bash
-            catkin run_tests -j2 -l2 && catkin_test_results
+            cd /ros2_ws
+            source /opt/ros/jazzy/setup.bash
+            source /ros2_ws/install/setup.bash
+            colcon run_tests -j2 -l2 && catkin_test_results
       - run:
           name: "Run soft linting checks"
           command: ./scripts/lint_soft.sh
 workflows:
   version: 2
-  melodic-build-and-test:
+  jazzy-build-and-test:
     jobs:
       - build-and-test

.devcontainer/Dockerfile

@@ -0,0 +1,129 @@
+# Legged Controls Dockerfile
+FROM nvcr.io/nvidia/cuda:12.8.1-cudnn-devel-ubuntu24.04
+
+ARG USERNAME=USERNAME
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+ARG ONNX_VERSION=1.17.0
+ARG ONNXRUNTIME_VERSION=1.22.2
+ARG CMAKE_VERSION=4.0.0
+ARG NUM_JOBS=16
+
+# Delete user if it exists in container (e.g Ubuntu Noble: ubuntu)
+RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi
+
+# Create the user
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    #
+    # [Optional] Add sudo support. Omit if you don't need to install software after connecting.
+    && apt-get update \
+    && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME
+RUN apt-get update && apt-get upgrade -y
+RUN apt-get install -y python3-pip
+ENV SHELL /bin/bash
+
+ENV NVIDIA_VISIBLE_DEVICES \
+    ${NVIDIA_VISIBLE_DEVICES:-all}
+ENV NVIDIA_DRIVER_CAPABILITIES \
+    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install package dependencies
+RUN apt-get update && apt-get install -y --no-install-recommends \
+        build-essential \
+        software-properties-common \
+        autoconf \
+        automake \
+        libtool \
+        pkg-config \
+        ca-certificates \
+        locales \
+        locales-all \
+        python3-full \
+        wget \
+        sudo \
+        vim \
+        tmux \
+        curl \
+        less \
+        htop \
+        git && \
+    apt-get clean
+
+# System locale
+# Important for UTF-8
+ENV LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 LANGUAGE=en_US.UTF-8
+
+## Install CMake 4.0
+RUN cd /tmp && \
+    wget https://github.com/Kitware/CMake/releases/download/v${CMAKE_VERSION}/cmake-${CMAKE_VERSION}-linux-x86_64.sh && \
+    bash cmake-${CMAKE_VERSION}-linux-x86_64.sh --prefix=/usr/local --exclude-subdir --skip-license && \
+    rm -rf /tmp/*
+
+# Install ONNX Runtime (Policy Inference)
+# RUN pip install numpy==2.2.4 psutil==7.0.0 pytest==8.3.5 onnx==${ONNX_VERSION}
+RUN cd /tmp && \
+    git clone --recursive --branch v${ONNXRUNTIME_VERSION} https://github.com/Microsoft/onnxruntime
+RUN cd /tmp && \
+    cd onnxruntime && \
+    ./build.sh \
+        --allow_running_as_root \
+        --cuda_home /usr/local/cuda \
+        --cudnn_home /usr/lib/x86_64-linux-gnu/ \
+        --use_cuda \
+        # --use_tensorrt \
+        # --tensorrt_home /usr/lib/x86_64-linux-gnu/ \
+        --config RelWithDebInfo \
+        --build_shared_lib \
+        # Somehow --build_wheel cannot be used in Docker build because NumPy cannot be found.
+        # However, if this command is run in a Docker container, it works.
+        # --build_wheel \
+        --skip_tests \
+        --parallel ${NUM_JOBS} && \
+    cd build/Linux/RelWithDebInfo && \
+    make install && \
+    # pip install dist/* && \
+    rm -rf /tmp/*
+ENV LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH 
+
+# Install ROS2 Jazzy
+SHELL ["/bin/bash", "-lc"]
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    software-properties-common && \
+    add-apt-repository universe && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
+      -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    sh -c 'echo "deb [signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
+      http://packages.ros.org/ros2/ubuntu noble main" > \
+      /etc/apt/sources.list.d/ros2.list' && \
+    apt-get update && apt-get install -y --no-install-recommends \
+      ros-jazzy-desktop \
+      python3-rosdep python3-colcon-common-extensions \
+      && rm -rf /var/lib/apt/lists/*
+
+# Initialize rosdep (optional if you build workspaces later)
+RUN rosdep init || true && rosdep update
+RUN echo "source /opt/ros/jazzy/setup.bash" >> /etc/bash.bashrc
+
+# Install Livox SDK
+RUN which cmake && cmake --version
+RUN apt-get install -y cmake
+WORKDIR /root/
+RUN git clone https://github.com/liyufan/Livox-SDK2.git
+WORKDIR /root/Livox-SDK2
+RUN mkdir build
+WORKDIR /root/Livox-SDK2/build
+RUN cmake -DCMAKE_POLICY_VERSION_MINIMUM=3.5 .. && make -j2 && make install
+
+# Install Python
+WORKDIR /root/ros2_ws
+
+# Load ROS environment at each run
+COPY ./ros_entrypoint.sh /
+RUN chmod 755 /ros_entrypoint.sh
+ENTRYPOINT ["/ros_entrypoint.sh"]
+
+CMD ["bash"]

.devcontainer/devcontainer.json

@@ -0,0 +1,52 @@
+{
+  "name": "ROS 2 Development Container",
+  "privileged": true,
+  "remoteUser": "root",
+
+  "build": {
+    "dockerfile": "Dockerfile",
+    "args": { "USERNAME": "rml" }
+  },
+
+  "workspaceFolder": "/root/ros2_ws/src/",
+  "workspaceMount": "source=${localWorkspaceFolder}/..,target=/root/ros2_ws/src/,type=bind",
+
+  "customizations": {
+    "vscode": {
+      "extensions": [
+        "ms-vscode.cpptools",
+        "ms-vscode.cpptools-themes",
+        "ms-vscode.cmake-tools",
+        "twxs.cmake",
+        "donjayamanne.python-extension-pack",
+        "eamodio.gitlens",
+        "ms-iot.vscode-ros"
+      ]
+    }
+  },
+
+  "containerEnv": {
+    "DISPLAY": "${localEnv:DISPLAY}",
+    "ROS_AUTOMATIC_DISCOVERY_RANGE": "LOCALHOST",
+    "ROS_DOMAIN_ID": "42",
+    "__GLX_VENDOR_LIBRARY_NAME": "nvidia"
+  },
+
+  "runArgs": [
+    "--gpus", "all",
+    "-e", "NVIDIA_VISIBLE_DEVICES=all",
+    "-e", "NVIDIA_DRIVER_CAPABILITIES=all",
+    "--net=host",
+    "--ipc=host",
+    "--shm-size=4g",
+    "-v", "/tmp/.X11-unix:/tmp/.X11-unix:ro",
+    "-v", "/dev/dri:/dev/dri",
+    "-e", "XDG_RUNTIME_DIR=/run/user/${localUserId}",
+    "-e", "WAYLAND_DISPLAY=${localEnv:WAYLAND_DISPLAY}",
+    "-v", "/run/user/${localUserId}:/run/user/${localUserId}",
+    "--entrypoint", "/bin/bash"             // <-- override your image ENTRYPOINT for VS Code bootstrap
+  ],
+
+  "overrideCommand": true,                  // let VS Code control the startup command
+  "postStartCommand": "bash -lc 'source /opt/ros/jazzy/setup.bash && echo \"source /opt/ros/jazzy/setup.bash\" >> ~/.bashrc'"
+}

.devcontainer/ros_entrypoint.sh

@@ -0,0 +1,14 @@
+#!/bin/bash
+
+set -e
+
+# Ros build
+source "/opt/ros/jazzy/setup.bash"
+
+echo "==============ROS2 Docker Env Ready================"
+
+if [ -n "$VSCODE_REMOTE_CONTAINERS" ]; then
+  exec "$@"
+fi
+cd "${WS_DIR:-$HOME/ros2_ws}"
+exec "$@"

.devcontainer/run.sh

@@ -0,0 +1,33 @@
+#!/bin/bash
+# Author : Taeyoung Kim (https://github.com/Taeyoung96)
+
+# Set the project directory (PROJECT_DIR) as the parent directory of the current working directory
+PROJECT_DIR=$(dirname "$PWD")
+
+# Move to the parent folder of the project directory
+cd "$PROJECT_DIR"
+
+# Print the current working directory to verify the change
+echo "Current working directory: $PROJECT_DIR"
+
+# Check if arguments are provided for the image name and tag
+if [ "$#" -ne 2 ]; then
+  echo "Usage: $0 <container_name> <image_name:tag>"
+  exit 1
+fi
+CONTAINER_NAME="$1"
+IMAGE_NAME="$2"
+
+docker run --privileged -it \
+            --gpus='all,"capabilities=compute,utility,graphics,display"' \
+           -e NVIDIA_DRIVER_CAPABILITIES=all \
+           -e NVIDIA_VISIBLE_DEVICES=all \
+           --volume="$PROJECT_DIR:/root/ros2_ws/src/quad-sdk" \
+           --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw \
+           --net=host \
+           --ipc=host \
+           --shm-size=4gb \
+           --name="$CONTAINER_NAME" \
+           --env="DISPLAY=$DISPLAY" \
+           --rm \
+           "$IMAGE_NAME" /bin/bash

.gitignore

@@ -6,3 +6,5 @@ docs/
 external/ipopt/*
 !external/ipopt/.gitkeep
 *.csv
+robot_driver/include/robot_driver/models/
+matlab_msg_gen/

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "external/rbdl-orb"]
+	path = external/rbdl-orb
+	url = https://github.com/ORB-HD/rbdl-orb
+[submodule "external/unitree_sdk2"]
+	path = external/unitree_sdk2
+	url = https://github.com/unitreerobotics/unitree_sdk2.git

body_force_estimator/CMakeLists.txt

@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.8)
+project(body_force_estimator)
+
+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(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(quad_msgs REQUIRED)
+find_package(quad_utils REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_library(${PROJECT_NAME}
+  src/body_force_estimator.cpp
+  src/body_force_estimator_dynamics.cpp
+)
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  std_msgs
+  quad_msgs
+  quad_utils
+)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+add_executable(${PROJECT_NAME}_node src/body_force_estimator_node.cpp)
+target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME})
+ament_target_dependencies(${PROJECT_NAME}_node
+  rclcpp
+  std_msgs
+  quad_msgs
+  quad_utils
+)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY config/
+  DESTINATION share/${PROJECT_NAME}/config
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(PROGRAMS
+  scripts/leg_flail.py
+  scripts/path_following.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_dependencies(rclcpp std_msgs quad_msgs quad_utils)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()