diff --git a/.codex b/.codex new file mode 100644 index 0000000..e69de29 diff --git a/.gitignore b/.gitignore index 1d74e21..9fbd995 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,4 @@ .vscode/ + +# External dependencies managed via muse_ws/muse.repos (vcstool) +muse_ws/src/point_lio_ros2/ diff --git a/Dockerfile b/Dockerfile deleted file mode 100644 index 75478a0..0000000 --- a/Dockerfile +++ /dev/null @@ -1,63 +0,0 @@ -# Base ROS image -FROM ros:noetic-ros-core - -# Set environment variables -ENV DEBIAN_FRONTEND=noninteractive -ENV ROS_DISTRO=noetic - -# Install dependencies and Pinocchio -RUN apt-get update && apt-get install -y \ - build-essential \ - cmake \ - git \ - ros-noetic-dynamic-reconfigure \ - libeigen3-dev \ - ros-$ROS_DISTRO-plotjuggler-ros \ - ros-${ROS_DISTRO}-pinocchio \ - ros-${ROS_DISTRO}-rviz \ - python3-catkin-tools \ - python3-catkin-pkg \ - python3-rospkg \ - cmake \ - python3-rosdep \ - libgl1-mesa-dev \ - libgl1-mesa-dri \ - libgl1-mesa-glx \ - libglu1-mesa \ - freeglut3-dev \ - && rm -rf /var/lib/apt/lists/* - -ENV LIBGL_ALWAYS_SOFTWARE=1 - - -# create and initialize a catkin workspace -WORKDIR /home/dev_ws -RUN mkdir -p catkin_ws/src - -# # clone kiss-icp ROS wrappers (v0.3.0) -WORKDIR /home/dev_ws/catkin_ws/src -RUN git clone --branch v0.3.0 https://github.com/PRBonn/kiss-icp.git - -# install any remaining ROS dependencies -WORKDIR /home/dev_ws/catkin_ws -RUN \ - rosdep init || true && \ - rosdep update && \ - rosdep install --from-paths src --ignore-src -r -y - -# build the workspace -RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - catkin build" - -# automatically source both ROS and your workspace on container start -SHELL ["/bin/bash", "-c"] -RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc && \ - echo "source /home/dev_ws/catkin_ws/devel/setup.bash" >> ~/.bashrc - -# create a safe runtime dir for Qt -RUN mkdir -p /tmp/runtime-root && chmod 700 /tmp/runtime-root -ENV XDG_RUNTIME_DIR=/tmp/runtime-root - -# default to bash -CMD ["bash"] - diff --git a/README.md b/README.md index 7e1cd08..41b306e 100644 --- a/README.md +++ b/README.md @@ -1,93 +1,143 @@

MUSE: A Real-Time Multi-Sensor State Estimator for Quadruped Robots

-

Ylenia Nisticò, João Carlos Virgolino Soares, Lorenzo Amatucci, Geoff Fink and Claudio Semini

+

Ylenia Nisticò, João Carlos Virgolino Soares, Lorenzo Amatucci, Geoff Fink and Claudio Semini

This paper has been accepted to IEEE Robotics and Automation Letters, and it is available at https://arxiv.org/abs/2503.12101

-

- -![muse_cropped](https://github.com/user-attachments/assets/b212edff-44a4-4e46-acb9-c48e160ae8cd) - +# :computer: Overview -# :computer: Code +MUSE provides a ROS1 state-estimator node for quadruped robots. The node loads estimator modules with `pluginlib`, reads proprioceptive robot data, and publishes attitude, contact, leg odometry, fused odometry, and TF outputs. -The `muse` package provides a ROS node and utilities for estimating the state of a quadruped robot using sensor data. It includes algorithms for state estimation, sensor fusion, and filtering. +The current ROS1 implementation is proprioceptive. The necessary online inputs are: -This first version of the code provides a proprioceptive state estimator for quadruped robots. The necessary inputs are -- **imu measurements** -- **joint states** -- **force exerted on the feet** +- `sensor_msgs/Imu` IMU measurements +- `series_elastic_actuator_msgs/SeActuatorReadings` actuator readings - -Additional code to fuse exteroceptive measurements will be available soon! -TODO list at the end of the page -

+It also requires a robot URDF and matching foot/joint frame names in the configuration. +Default configuration files are provided for ANYmal-style naming, with additional URDFs for `aliengo`, `anymal`, and `go1` in `muse_ws/src/state_estimator/urdfs`. +## :t-rex: Prerequisites -## :t-rex: Prerequisites (Don't worry! In this repo, we provide **Dockerization** to avoid dealing with the dependencies!) -* [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) -* [Pinocchio](https://github.com/stack-of-tasks/pinocchio/tree/master) -* [ROS Noetic](https://wiki.ros.org/noetic/Installation/Ubuntu) +- Conda or Mamba +- Dependencies from the provided `environment.yml` -⚠️ ATTENTION: ROS1 is deprecated. This repo will move to ROS2. There is a "still-not-stable" ROS2 version on the [`ros2`](https://github.com/iit-DLSLab/muse/tree/ros2) branch, that will become the official one at a certain point. The latest release with ROS1 is the [v1.0.0](https://github.com/iit-DLSLab/muse/releases/tag/v1.0.0). +The conda environment is named `muse` and provides the ROS1 Noetic stack, `catkin_tools`, Eigen, Pinocchio, RViz, PlotJuggler, and the ROS packages used by the workspace. +Create the environment with: +```sh +cd muse +mamba env create -f environment.yml +``` + +If the environment already exists, update it with: + +```sh +mamba env update -n muse -f environment.yml --prune +``` + +## :hammer_and_wrench: Building + +Build the ROS1 workspace with `catkin build`: + +```sh +cd muse/muse_ws +conda activate muse +catkin config --extend "$CONDA_PREFIX" --install +catkin build +source install/setup.bash +``` -## :hammer_and_wrench: Building and Running +## :rocket: Running -To install the `muse` package, follow these steps: +Launch the estimator: -1. Clone this repository and build the Docker image: - ```sh - git clone https://github.com/iit-DLSLab/muse.git - cd muse - docker build -t muse-docker . - ``` +```sh +roslaunch state_estimator state_estimator.launch +``` + +RViz is disabled by default: -2. Enter the docker and build using `catkin_make`: - ```sh - cd muse_ws - xhost +local:docker - docker run -it --rm --name muse -v "$(pwd)":/root/muse_ws -w /root/muse_ws muse-docker - catkin_make -j$(proc) install - source devel/setup.bash - ``` -3. To launch the state estimator node: - ```sh - roslaunch state_estimator state_estimator.launch - ``` -If you need to read the data from a rosbag, you need to mount the folder where you store your rosbags (`your_path_to_rosbags`), to make it visible inside the image, and then, you can attach a docker image in another terminal, for example: ```sh -docker run -it --rm --name muse \ - --net=host \ - -e DISPLAY=$DISPLAY \ - -v /tmp/.X11-unix:/tmp/.X11-unix \ - -v your_path_to_rosbags:/root/rosbags \ - -v "$(pwd)":/root/muse_ws \ - -w /root/muse_ws \ - muse-docker (terminal 1) -docker exec -it muse bash (terminal 2) -source devel/setup.bash -cd ~/rosbags (terminal 2) -rosbag play your_rosbag.bag (terminal 2) +roslaunch state_estimator state_estimator.launch rviz:=false ``` -To change the name of the topics, check the [config foder](https://github.com/iit-DLSLab/muse/tree/main/muse_ws/src/state_estimator/config). -To visualize your data, you can use [PlotJuggler](https://github.com/facontidavide/PlotJuggler?tab=readme-ov-file) which is already installed in the docker image: +For rosbag playback, use simulated time and play the bag with `/clock`: + +```sh +roslaunch state_estimator state_estimator.launch use_sim_time:=true +rosbag play --clock your_rosbag.bag +``` + +## :electric_plug: ROS Interfaces + +Default input topics: + +- `/anymal/imu` (`sensor_msgs/Imu`) +- `/anymal/state_estimator/anymal_state` (the definition of the anymal msgs is taken from the [holistic_fusion](https://github.com/leggedrobotics/holistic_fusion/tree/main/ros/graph_msf_anymal_msgs/msg) repo from ETH) + +Default estimator outputs are published in the private node namespace: + +- `/state_estimator/attitude` (`state_estimator_msgs/attitude`) +- `/state_estimator/contact_detection` (`state_estimator_msgs/ContactDetection`) +- `/state_estimator/leg_odometry` (`state_estimator_msgs/LegOdometry`) +- `/state_estimator/sensor_fusion` (`nav_msgs/Odometry`) +- TF from `world` to `base`, generated from `/state_estimator/sensor_fusion` + +The topics and frame IDs are configured in `muse_ws/src/state_estimator/config`. + +## :gear: Plugins + +The state-estimator node discovers plugins declared in `state_estimator_plugins.xml` and loads them according to `launch/pluginlist.yaml`. + +Implemented ROS1 plugins: + +- `AttitudeEstimation`: estimates attitude from IMU data. +- `ContactDetection`: estimates foot contact from actuator readings and URDF-based GRF estimation. +- `LegOdometry`: estimates base velocity from actuator readings, attitude, contact state, and Pinocchio kinematics. +- `SensorFusion`: fuses IMU, attitude, and leg odometry into `nav_msgs/Odometry`. +- `TfPublisher`: publishes TF from the fused odometry output. + +By default, both `plugin_whitelist` and `plugin_blacklist` are empty, so all declared plugins are loaded. To run only selected plugins, edit `muse_ws/src/state_estimator/launch/pluginlist.yaml`. + +### Visualization +Run PlotJuggler with: + ```sh rosrun plotjuggler plotjuggler ``` +A PlotJuggler layout is also provided at: + +```text +muse_ws/src/plotjuggler_layout.xml +``` + -:warning: In this repo we provide an example with the ANYmal B300 robot. If you want to test MUSE with another one, you only need to add the URDF of your robot in [this folder](https://github.com/iit-DLSLab/muse/tree/main/muse_ws/src/state_estimator/urdfs), and change the name of the legs in the [leg odometry plugin, line 249](https://github.com/iit-DLSLab/muse/blob/main/muse_ws/src/state_estimator/src/plugins/leg_odometry_plugin.cpp#L249): +## :robot: Robot Configuration -``` sh -std::vector feet_frame_names = {"LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"}; // Update with your actual link names +To use a different robot: + +1. Add the URDF to `muse_ws/src/state_estimator/urdfs`. +2. Update `urdf_path` in `config/contact_plugin.yaml` and `config/leg_odometry.yaml`. +3. Update `foot_frame_names` and the joint-name lists in the same config files. +4. Update `base_R_imu` and input topic names for your robot. + +The default leg order is: + +```text +LF, RF, LH, RH ``` -For real-world experiments, we recommend using this very nice [MPC](https://github.com/iit-DLSLab/Quadruped-PyMPC) to control your robot! + +Keep this order consistent across contact detection, leg odometry, messages, and downstream consumers. + + ## :scroll: TODO list -- [ ] Extend the code to include exteroception -- [x] Dockerization -- [x] Support for ROS2 (on going) + +- [x] ROS1 proprioceptive state estimation +- [x] GRF-based contact detection from actuator readings +- [x] TF publishing from fused odometry +- [ ] ROS2 support (on going) +- [ ] Exteroceptive sensor fusion (on going) ## :hugs: Contributing @@ -96,11 +146,12 @@ Contributions to this repository are welcome. ## Citing the paper If you like this work and would like to cite it (thanks): -``` + +```bibtex @ARTICLE{10933515, author={Nisticò, Ylenia and Soares, João Carlos Virgolino and Amatucci, Lorenzo and Fink, Geoff and Semini, Claudio}, - journal={IEEE Robotics and Automation Letters}, - title={MUSE: A Real-Time Multi-Sensor State Estimator for Quadruped Robots}, + journal={IEEE Robotics and Automation Letters}, + title={MUSE: A Real-Time Multi-Sensor State Estimator for Quadruped Robots}, year={2025}, volume={10}, number={5}, @@ -108,8 +159,10 @@ If you like this work and would like to cite it (thanks): keywords={Robots;Sensors;Robot sensing systems;Legged locomotion;Odometry;Cameras;Laser radar;Robot vision systems;Robot kinematics;Quadrupedal robots;State estimation;localization;sensor fusion;quadruped robots}, doi={10.1109/LRA.2025.3553047}} ``` -This repo is maintained by [Ylenia Nisticò](https://github.com/ylenianistico) - - +## Maintainer +This repo is maintained by +| Avatar | Name | +| ------- | ---- | +| | Ylenia Nisticò | diff --git a/environment.yml b/environment.yml new file mode 100644 index 0000000..2a90074 --- /dev/null +++ b/environment.yml @@ -0,0 +1,36 @@ +name: muse +channels: + - robostack-noetic + - conda-forge +dependencies: + - python=3.11 + - numpy=1.26 + - compilers + - cmake + - make + - ninja + - pkg-config + - git + - eigen=3.4 + - libboost-devel + - pinocchio + - ros-noetic-ros-base + - ros-noetic-catkin + - ros-noetic-roscpp + - ros-noetic-rosconsole + - ros-noetic-std-msgs + - ros-noetic-sensor-msgs + - ros-noetic-geometry-msgs + - ros-noetic-nav-msgs + - ros-noetic-tf2-ros + - ros-noetic-pluginlib + - ros-noetic-message-filters + - ros-noetic-message-generation + - ros-noetic-message-runtime + - ros-noetic-dynamic-reconfigure + - ros-noetic-rviz + - ros-noetic-plotjuggler-ros + - catkin_pkg + - catkin_tools + - rosdep + - rospkg diff --git a/muse_ws/.catkin_workspace b/muse_ws/.catkin_workspace deleted file mode 100644 index 52fd97e..0000000 --- a/muse_ws/.catkin_workspace +++ /dev/null @@ -1 +0,0 @@ -# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/muse_ws/.gitignore b/muse_ws/.gitignore new file mode 100644 index 0000000..da31195 --- /dev/null +++ b/muse_ws/.gitignore @@ -0,0 +1,34 @@ +# Catkin build artifacts +/build/ +/devel/ +/install/ +/logs/ +.catkin_tools/ + +# CMake +CMakeCache.txt +CMakeFiles/ +cmake_install.cmake +Makefile + +# Python +__pycache__/ +*.py[cod] +*.egg-info/ + +# ROS generated/log files +*.bag +*.bag.active +*.log +.ros/ + +# Editor/IDE +.vscode/ +.idea/ +*.swp +*.swo +*~ + +# OS +.DS_Store +Thumbs.db \ No newline at end of file diff --git a/muse_ws/src/anymal_msgs/CMakeLists.txt b/muse_ws/src/anymal_msgs/CMakeLists.txt new file mode 100644 index 0000000..8369828 --- /dev/null +++ b/muse_ws/src/anymal_msgs/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.10) +project(anymal_msgs) + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + message_generation + std_msgs +) + +add_message_files( + FILES + AnymalState.msg + Contact.msg + ExtendedJointState.msg +) + +generate_messages( + DEPENDENCIES + geometry_msgs + std_msgs +) + +catkin_package( + CATKIN_DEPENDS + geometry_msgs + message_runtime + std_msgs +) \ No newline at end of file diff --git a/muse_ws/src/anymal_msgs/msg/AnymalState.msg b/muse_ws/src/anymal_msgs/msg/AnymalState.msg new file mode 100644 index 0000000..6007746 --- /dev/null +++ b/muse_ws/src/anymal_msgs/msg/AnymalState.msg @@ -0,0 +1,18 @@ +# Anymal state including generalized coordinates, velocities, and contacts. +std_msgs/Header header + +int8 STATE_ERROR_SENSOR=-3 +int8 STATE_ERROR_ESTIMATOR=-2 +int8 STATE_ERROR_UNKNOWN=-1 +int8 STATE_OK=0 +int8 STATE_UNINITIALIZED=1 +int8 state + +# Position of the base with respect to the odom frame, and orientation from base to odom. +geometry_msgs/PoseStamped pose + +# Linear velocity and local angular velocity expressed in base frame. +geometry_msgs/TwistStamped twist +ExtendedJointState joints +Contact[] contacts +geometry_msgs/TransformStamped[] frame_transforms diff --git a/muse_ws/src/anymal_msgs/msg/Contact.msg b/muse_ws/src/anymal_msgs/msg/Contact.msg new file mode 100644 index 0000000..d9fc0b7 --- /dev/null +++ b/muse_ws/src/anymal_msgs/msg/Contact.msg @@ -0,0 +1,13 @@ +# Contact definition. +uint8 STATE_OPEN=0 +uint8 STATE_CLOSED=1 +uint8 STATE_SLIPPING=2 + +std_msgs/Header header +string name +uint8 state +geometry_msgs/Wrench wrench +geometry_msgs/Point position +geometry_msgs/Vector3 normal +float64 frictionCoefficient +float64 restitutionCoefficient diff --git a/muse_ws/src/anymal_msgs/msg/ExtendedJointState.msg b/muse_ws/src/anymal_msgs/msg/ExtendedJointState.msg new file mode 100644 index 0000000..89e6cc3 --- /dev/null +++ b/muse_ws/src/anymal_msgs/msg/ExtendedJointState.msg @@ -0,0 +1,8 @@ +# sensor_msgs/JointState with an additional field for acceleration. +std_msgs/Header header + +string[] name +float64[] position +float64[] velocity +float64[] acceleration +float64[] effort diff --git a/muse_ws/src/anymal_msgs/package.xml b/muse_ws/src/anymal_msgs/package.xml new file mode 100644 index 0000000..65051e4 --- /dev/null +++ b/muse_ws/src/anymal_msgs/package.xml @@ -0,0 +1,20 @@ + + + anymal_msgs + 0.0.0 + ANYmal message definitions + + ynistico + TODO + + catkin + + message_generation + message_runtime + + std_msgs + geometry_msgs + + + + \ No newline at end of file diff --git a/muse_ws/src/iit_commons/CMakeLists.txt b/muse_ws/src/iit_commons/CMakeLists.txt index c468c33..3433b59 100644 --- a/muse_ws/src/iit_commons/CMakeLists.txt +++ b/muse_ws/src/iit_commons/CMakeLists.txt @@ -1,5 +1,5 @@ # Project configuration -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.10) project(iit_commons) find_package(PkgConfig) @@ -96,4 +96,3 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(rotation-test ${LIB_NAME} ${catkin_LIBRARIES} pthread) endif() - diff --git a/muse_ws/src/plotjuggler_layout.xml b/muse_ws/src/plotjuggler_layout.xml new file mode 100644 index 0000000..355b4ca --- /dev/null +++ b/muse_ws/src/plotjuggler_layout.xml @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/muse_ws/src/state_estimator/CMakeLists.txt b/muse_ws/src/state_estimator/CMakeLists.txt index 316208f..066f9c0 100644 --- a/muse_ws/src/state_estimator/CMakeLists.txt +++ b/muse_ws/src/state_estimator/CMakeLists.txt @@ -1,158 +1,110 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10) project(state_estimator) -add_compile_options(-std=c++11) +add_compile_options(-std=c++17) -find_package(catkin REQUIRED +find_package(catkin REQUIRED COMPONENTS roscpp + roslib + rosconsole + dynamic_reconfigure sensor_msgs geometry_msgs nav_msgs - dynamic_reconfigure + std_msgs + tf2_ros pluginlib message_filters - message_generation state_estimator_msgs - std_msgs - rosconsole iit_commons - ) - -find_package(Boost REQUIRED COMPONENTS system program_options) +) +find_package(Boost REQUIRED COMPONENTS + chrono + program_options + system + thread +) find_package(Eigen3 REQUIRED NO_MODULE) - find_package(PkgConfig REQUIRED) - find_package(pinocchio REQUIRED) - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## Generate messages in the 'msg' folder - add_message_files( - FILES - # RollPitch.msg - # RollPitchStamped.msg - ) - -## Generate added messages and services with any dependencies listed here - generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs - nav_msgs - state_estimator - state_estimator_msgs - ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# # cfg/HeadBaseTransform.cfg -# # cfg/DynReconf1.cfg -# # cfg/DynReconf2.cfg -# ) - - -################################### -## catkin specific configuration ## -################################### catkin_package( - INCLUDE_DIRS - include - ${BOOST_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} - LIBRARIES ${PROJECT_NAME}_commons ${PROJECT_NAME}_plugins ${PROJECT_NAME}_models - CATKIN_DEPENDS pluginlib state_estimator_msgs roscpp iit_commons + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME}_plugins + CATKIN_DEPENDS + roscpp + roslib + sensor_msgs + geometry_msgs + nav_msgs + std_msgs + tf2_ros + pluginlib + message_filters + state_estimator_msgs + iit_commons ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include - /usr/include/eigen3 ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${EIGEN3_INCLUDE_DIRS} + ${PINOCCHIO_INCLUDE_DIRS} ) -include_directories(${PINOCCHIO_INCLUDE_DIRS}) - - -## Declare a C++ library add_library(${PROJECT_NAME}_plugins - src/plugins/attitude_estimation_plugin.cpp - src/plugins/contact_detection_plugin.cpp - src/plugins/leg_odometry_plugin.cpp - src/plugins/sensor_fusion_plugin.cpp - # src/Models/sensor_fusion.cpp + src/plugins/attitude_estimation_plugin.cpp + src/plugins/contact_detection_plugin.cpp + src/plugins/leg_odometry_plugin.cpp + src/plugins/sensor_fusion_plugin.cpp + src/plugins/tf_publisher_plugin.cpp ) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide - -add_executable(${PROJECT_NAME}_node src/state_estimator.cpp src/state_estimator_node.cpp) - -## Rename C++ executable without prefix -set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -message("${${PROJECT_NAME}_EXPORTED_TARGETS}") - add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) - -target_link_libraries(state_estimator_node - ${catkin_LIBRARIES} +add_dependencies(${PROJECT_NAME}_plugins + ${catkin_EXPORTED_TARGETS} ) - target_link_libraries(${PROJECT_NAME}_plugins - ${catkin_LIBRARIES} # <-- this includes message_filters now + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ${PINOCCHIO_LIBRARIES} + Eigen3::Eigen ) -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - ${PROJECT_NAME}_plugins.xml #${PROJECT_NAME}_tools.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +add_executable(${PROJECT_NAME}_node + src/state_estimator.cpp + src/state_estimator_node.cpp ) - -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" +set_target_properties(${PROJECT_NAME}_node PROPERTIES + OUTPUT_NAME node + PREFIX "" +) +add_dependencies(${PROJECT_NAME}_node + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) - +install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_plugins + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.hpp" +) - include_directories( include - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} - ) +install(DIRECTORY config launch rviz urdfs + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) -endif() +install(FILES + ${PROJECT_NAME}_plugins.xml + ${PROJECT_NAME}_tools.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/muse_ws/src/state_estimator/config/attitude_plugin.yaml b/muse_ws/src/state_estimator/config/attitude_plugin.yaml index 575c99d..9f14176 100644 --- a/muse_ws/src/state_estimator/config/attitude_plugin.yaml +++ b/muse_ws/src/state_estimator/config/attitude_plugin.yaml @@ -1,7 +1,7 @@ attitude_estimation_plugin: plugin: "AttitudeEstimationPlugin" # Subscriber and Publisher topic names - imu_topic: "/trunk_imu" # Unitree: "/trunk_imu" # ANYmal: "/sensors/imu" + imu_topic: "/anymal/imu" # Unitree: "/trunk_imu" # ANYmal: "/sensors/imu" pub_topic: "attitude" # Attitude Estimation Parameters @@ -9,9 +9,9 @@ attitude_estimation_plugin: ki: 0.02 # Rotation Matrix from IMU frame to Base frame - base_R_imu: [1.0, 0.0, 0.0, + base_R_imu: [-1.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] + 0.0, 0.0, -1.0] # Fake-North Vector in Navigation Frame: it needs to be a constant vector north_vector: [0.577, 0.577, 0.577] @@ -19,23 +19,23 @@ attitude_estimation_plugin: gravity_vector: [0.0, 0.0, 9.81] # XKF Covariance Matrices - P: [1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6] + P: [1.0e-12, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0e-12, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0e-12, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0e-12, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0e-12, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-12] - Q: [1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0e-8, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0e-8, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-8] + Q: [1.0e-15, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0e-15, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0e-15, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0e-15, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0e-15, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-15] - R: [4.0e-2, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 4.0e-2, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 4.0e-2, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 16.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 16.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 16.0] + R: [1.0e-10, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0e-10, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0e-10, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0e-10, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0e-10, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-10] diff --git a/muse_ws/src/state_estimator/config/contact_plugin.yaml b/muse_ws/src/state_estimator/config/contact_plugin.yaml index 74acda7..4c210dc 100644 --- a/muse_ws/src/state_estimator/config/contact_plugin.yaml +++ b/muse_ws/src/state_estimator/config/contact_plugin.yaml @@ -1,13 +1,31 @@ contact_detection_plugin: plugin: "ContactDetectionPlugin" - # Subscriber and Publisher topic names - # LF: Left Front leg, RF: Right Front leg, LH: Left Hind leg, RH: Right Hind leg - wrench_lf_topic: "/visual/FL_foot_contact/the_force" # Unitree: "/visual/FL_foot_contact/the_force" # ANYmal "/state_estimator/contact_force_lf_foot" - wrench_rf_topic: "/visual/FR_foot_contact/the_force" # Unitree: "/visual/FR_foot_contact/the_force" # ANYmal "/state_estimator/contact_force_rf_foot" - wrench_lh_topic: "/visual/RL_foot_contact/the_force" # Unitree: "/visual/RL_foot_contact/the_force" # ANYmal "/state_estimator/contact_force_lh_foot" - wrench_rh_topic: "/visual/RR_foot_contact/the_force" # Unitree: "/visual/RR_foot_contact/the_force" # ANYmal "/state_estimator/contact_force_rh_foot" + + # Estimate foot GRFs from state_estimator_msgs/AnymalState.joints. + anymal_state_topic: "/anymal/state_estimator/anymal_state" + urdf_path: "$(find state_estimator)/urdfs/anymal.urdf" + + # LF: Left Front leg, RF: Right Front leg, LH: Left Hind leg, RH: Right Hind leg. + foot_frame_names: ["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"] + lf_joint_names: ["LF_HAA", "LF_HFE", "LF_KFE"] + rf_joint_names: ["RF_HAA", "RF_HFE", "RF_KFE"] + lh_joint_names: ["LH_HAA", "LH_HFE", "LH_KFE"] + rh_joint_names: ["RH_HAA", "RH_HFE", "RH_KFE"] + + # Use the estimated force component along contact_normal as the GRF metric. + # Set use_force_norm=true to threshold the total force norm instead. + contact_normal: [0.0, 0.0, 1.0] + use_force_norm: false + use_absolute_normal_force: true + use_inverse_dynamics_compensation: true + force_sign: -1.0 + + use_hysteresis: true + grf_threshold_low: 40.0 + grf_threshold_high: 70.0 pub_topic: "contact_detection" - # thresholds for contact detection - grf_threshold: 30.0 \ No newline at end of file + # Threshold for contact detection. The plugin publishes bools: + # false = no contact, true = contact. + grf_threshold: 40.0 diff --git a/muse_ws/src/state_estimator/config/leg_odometry.yaml b/muse_ws/src/state_estimator/config/leg_odometry.yaml index d45921d..304616d 100644 --- a/muse_ws/src/state_estimator/config/leg_odometry.yaml +++ b/muse_ws/src/state_estimator/config/leg_odometry.yaml @@ -1,10 +1,20 @@ leg_odometry_plugin: plugin: "LegOdometryPlugin" # urdf_path: "$(find state_estimator)/urdfs/anymal.urdf" - urdf_path: "$(find state_estimator)/urdfs/go1.urdf" + urdf_path: "$(find state_estimator)/urdfs/anymal.urdf" - imu_topic: "/trunk_imu" # Unitree: "/trunk_imu" # ANYmal: "/sensors/imu" - joint_states_topic: "/go1_gazebo/joint_states" # Unitree (Go1): "/go1_gazebo/joint_states" # ANYmal: "/state_estimator/joint_states" + imu_topic: "/anymal/imu" # Unitree: "/trunk_imu" # ANYmal: "/sensors/imu" + anymal_state_topic: "/anymal/state_estimator/anymal_state" + base_R_imu: [-1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0] + base_frame_name: "base" + foot_frame_names: ["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"] + joint_names: + ["LF_HAA", "LF_HFE", "LF_KFE", + "RF_HAA", "RF_HFE", "RF_KFE", + "LH_HAA", "LH_HFE", "LH_KFE", + "RH_HAA", "RH_HFE", "RH_KFE"] contact_topic: "/state_estimator/contact_detection" attitude_topic: "/state_estimator/attitude" - pub_topic: "leg_odometry" \ No newline at end of file + pub_topic: "leg_odometry" diff --git a/muse_ws/src/state_estimator/config/sensor_fusion.yaml b/muse_ws/src/state_estimator/config/sensor_fusion.yaml index b80d88a..562ebaf 100644 --- a/muse_ws/src/state_estimator/config/sensor_fusion.yaml +++ b/muse_ws/src/state_estimator/config/sensor_fusion.yaml @@ -1,25 +1,37 @@ sensor_fusion_plugin: plugin: "SensorFusionPlugin" - imu_topic: "/trunk_imu" # Unitree: "/trunk_imu" #ANYmal: "/sensors/imu" + imu_topic: "/anymal/imu" # Unitree: "/trunk_imu" # ANYmal bag: "/anymal/imu" attitude_topic: "/state_estimator/attitude" leg_odometry_topic: "/state_estimator/leg_odometry" - pub_topic: "sensor_fusion" + pub_topic: "sensor_fusion" + frame_id: "world" + child_frame_id: "base" + use_zupt: true + z_height_correction: + enabled: true + # false: contact-anchor correction, works without assuming a fixed ground z. + # true: use base_height + ground_height as an absolute flat-ground z measurement. + assume_flat_ground: false + ground_height: 0.0 + min_stance_legs: 1 + measurement_variance: 1.0e-14 + max_innovation: 0.25 + filter_alpha: 1.0 - P: [1.0e-3, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0e-3, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0e-3, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0e-4, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-4] + P: [1.0e-8, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0e-8, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0e-8, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0e-13, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0e-13, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-12] - Q: [1.0e-9, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0e-9, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0e-9, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0e-9, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0e-9, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-9] - - R: [1.0e-11, 0.0, 0.0, - 0.0, 1.0e-11, 0.0, - 0.0, 0.0, 1.0e-11] + Q: [1.0e-8, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0e-8, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0e-8, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0e-13, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0e-13, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-12] + R: [5.0e-16, 0.0, 0.0, + 0.0, 5.0e-16, 0.0, + 0.0, 0.0, 5.0e-14] diff --git a/muse_ws/src/state_estimator/config/tf_publisher.yaml b/muse_ws/src/state_estimator/config/tf_publisher.yaml new file mode 100644 index 0000000..b351b41 --- /dev/null +++ b/muse_ws/src/state_estimator/config/tf_publisher.yaml @@ -0,0 +1,6 @@ +tf_publisher_plugin: + plugin: "TfPublisherPlugin" + odom_topic: "/state_estimator/sensor_fusion" + parent_frame: "world" + child_frame: "base" + use_odom_frame_ids: false diff --git a/muse_ws/src/state_estimator/include/state_estimator/Models/grf_contact_estimator.hpp b/muse_ws/src/state_estimator/include/state_estimator/Models/grf_contact_estimator.hpp new file mode 100644 index 0000000..8347d3d --- /dev/null +++ b/muse_ws/src/state_estimator/include/state_estimator/Models/grf_contact_estimator.hpp @@ -0,0 +1,453 @@ +#ifndef GRF_CONTACT_ESTIMATOR_HPP +#define GRF_CONTACT_ESTIMATOR_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "state_estimator/Models/joint_state_snapshot.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace state_estimator +{ + +struct GrfContactEstimate +{ + std::array stance{{false, false, false, false}}; + std::array metric{{0.0, 0.0, 0.0, 0.0}}; + std::array force{{ + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero() + }}; +}; + +class GrfContactEstimator +{ +public: + GrfContactEstimator() = default; + + bool configure( + const std::string& urdf_path, + const std::array& foot_frame_names, + const std::array, 4>& leg_joint_names, + double threshold, + double threshold_low, + double threshold_high, + bool use_hysteresis, + bool use_force_norm, + bool use_absolute_normal_force, + double force_sign, + bool use_inverse_dynamics_compensation, + const Eigen::Vector3d& contact_normal) + { + pinocchio::urdf::buildModel(urdf_path, model_); + data_ = pinocchio::Data(model_); + q_ = pinocchio::neutral(model_); + v_ = Eigen::VectorXd::Zero(model_.nv); + a_ = Eigen::VectorXd::Zero(model_.nv); + tau_ = Eigen::VectorXd::Zero(model_.nv); + tau_residual_ = Eigen::VectorXd::Zero(model_.nv); + + foot_frame_names_ = foot_frame_names; + leg_joint_names_ = leg_joint_names; + flattened_joint_names_.clear(); + for (const auto& joint_names : leg_joint_names_) + { + flattened_joint_names_.insert(flattened_joint_names_.end(), joint_names.begin(), joint_names.end()); + } + threshold_ = threshold; + threshold_low_ = threshold_low; + threshold_high_ = threshold_high; + use_hysteresis_ = use_hysteresis; + use_force_norm_ = use_force_norm; + use_absolute_normal_force_ = use_absolute_normal_force; + force_sign_ = force_sign; + use_inverse_dynamics_compensation_ = use_inverse_dynamics_compensation; + contact_normal_ = contact_normal.norm() > 1e-9 ? contact_normal.normalized() : Eigen::Vector3d::UnitZ(); + + for (std::size_t leg = 0; leg < foot_frame_names_.size(); ++leg) + { + if (!model_.existFrame(foot_frame_names_[leg])) + { + last_error_ = "missing foot frame '" + foot_frame_names_[leg] + "'"; + configured_ = false; + return false; + } + + if (leg_joint_names_[leg].size() != 3) + { + last_error_ = "leg '" + foot_frame_names_[leg] + "' must have exactly 3 joint names"; + configured_ = false; + return false; + } + + for (const auto& joint_name : leg_joint_names_[leg]) + { + if (!model_.existJointName(joint_name)) + { + last_error_ = "missing joint '" + joint_name + "'"; + configured_ = false; + return false; + } + } + } + + stance_state_.fill(false); + configured_ = true; + last_error_.clear(); + return true; + } + + bool update( + const JointStateSnapshot& joints, + GrfContactEstimate& estimate) + { + if (!configured_) + { + last_error_ = "estimator is not configured"; + return false; + } + + std::unordered_map joint_position; + std::unordered_map joint_velocity; + std::unordered_map joint_acceleration; + std::unordered_map joint_torque; + std::vector incoming_names; + std::size_t named_readings = 0; + const std::size_t joint_count = maxJointFieldSize(joints); + + for (std::size_t joint_index = 0; joint_index < joint_count; ++joint_index) + { + std::string name = joint_index < joints.name.size() ? joints.name[joint_index] : std::string(); + if (!name.empty()) + { + named_readings++; + incoming_names.push_back(name); + } + else if ( + use_reading_order_when_names_missing_ && + joint_count == flattened_joint_names_.size() && + joint_index < flattened_joint_names_.size()) + { + name = flattened_joint_names_[joint_index]; + } + + if (name.empty()) continue; + + insertJointValueIfPresent(joint_position, name, joints.position, joint_index); + insertJointValueIfPresent(joint_velocity, name, joints.velocity, joint_index); + insertJointValueIfPresent(joint_acceleration, name, joints.acceleration, joint_index); + insertJointValueIfPresent(joint_torque, name, joints.effort, joint_index); + } + + q_ = pinocchio::neutral(model_); + v_.setZero(); + a_.setZero(); + tau_.setZero(); + std::size_t matched_torques = 0; + + for (pinocchio::JointIndex joint_id = 1; joint_id < model_.njoints; ++joint_id) + { + const std::string& joint_name = model_.names[joint_id]; + const int q_index = model_.joints[joint_id].idx_q(); + const int v_index = model_.joints[joint_id].idx_v(); + + double joint_value = 0.0; + if (findJointValue(joint_position, joint_name, joint_value) && q_index >= 0 && q_index < q_.size()) + { + q_[q_index] = joint_value; + } + + if (findJointValue(joint_velocity, joint_name, joint_value) && v_index >= 0 && v_index < v_.size()) + { + v_[v_index] = joint_value; + } + + if (findJointValue(joint_acceleration, joint_name, joint_value) && v_index >= 0 && v_index < a_.size()) + { + a_[v_index] = joint_value; + } + + if (findJointValue(joint_torque, joint_name, joint_value) && v_index >= 0 && v_index < tau_.size()) + { + tau_[v_index] = joint_value; + matched_torques++; + } + } + + if (matched_torques == 0) + { + last_error_ = unmatchedNamesError(incoming_names, named_readings, joint_count); + return false; + } + + if (use_inverse_dynamics_compensation_) + { + tau_residual_ = tau_ - pinocchio::rnea(model_, data_, q_, v_, a_); + } + else + { + tau_residual_ = tau_; + } + + pinocchio::forwardKinematics(model_, data_, q_); + pinocchio::computeJointJacobians(model_, data_, q_); + pinocchio::updateFramePlacements(model_, data_); + + for (std::size_t leg = 0; leg < foot_frame_names_.size(); ++leg) + { + estimate.force[leg] = estimateFootForce(leg); + estimate.metric[leg] = contactMetric(estimate.force[leg]); + + if (use_hysteresis_) + { + estimate.stance[leg] = stance_state_[leg] + ? estimate.metric[leg] > threshold_low_ + : estimate.metric[leg] > threshold_high_; + } + else + { + estimate.stance[leg] = estimate.metric[leg] > threshold_; + } + + stance_state_[leg] = estimate.stance[leg]; + } + + return true; + } + + const std::string& lastError() const + { + return last_error_; + } + +private: + Eigen::Vector3d estimateFootForce(std::size_t leg) const + { + Eigen::Matrix frame_jacobian(6, model_.nv); + frame_jacobian.setZero(); + + const pinocchio::FrameIndex frame_id = model_.getFrameId(foot_frame_names_[leg]); + pinocchio::getFrameJacobian(model_, data_, frame_id, pinocchio::LOCAL_WORLD_ALIGNED, frame_jacobian); + + Eigen::Matrix3d linear_jacobian; + Eigen::Vector3d joint_torque; + + for (int i = 0; i < 3; ++i) + { + const auto joint_id = model_.getJointId(leg_joint_names_[leg][i]); + const int v_index = model_.joints[joint_id].idx_v(); + linear_jacobian.col(i) = frame_jacobian.topRows<3>().col(v_index); + joint_torque(i) = tau_residual_(v_index); + } + + return force_sign_ * linear_jacobian.transpose().completeOrthogonalDecomposition().solve(joint_torque); + } + + double contactMetric(const Eigen::Vector3d& force) const + { + if (use_force_norm_) + { + return force.norm(); + } + + const double normal_force = force.dot(contact_normal_); + return use_absolute_normal_force_ ? std::abs(normal_force) : normal_force; + } + + static void insertJointValue(std::unordered_map& values, const std::string& name, double value) + { + values[name] = value; + const std::string normalized_name = normalizeJointName(name); + if (!normalized_name.empty()) + { + values[normalized_name] = value; + } + } + + static void insertJointValueIfPresent( + std::unordered_map& values, + const std::string& name, + const std::vector& source, + std::size_t index) + { + if (index < source.size()) + { + insertJointValue(values, name, source[index]); + } + } + + static std::size_t maxJointFieldSize(const JointStateSnapshot& joints) + { + return std::max({ + joints.name.size(), + joints.position.size(), + joints.velocity.size(), + joints.acceleration.size(), + joints.effort.size() + }); + } + + static bool findJointValue( + const std::unordered_map& values, + const std::string& joint_name, + double& value) + { + auto value_it = values.find(joint_name); + if (value_it != values.end()) + { + value = value_it->second; + return true; + } + + value_it = values.find(normalizeJointName(joint_name)); + if (value_it != values.end()) + { + value = value_it->second; + return true; + } + + return false; + } + + static std::string normalizeJointName(std::string name) + { + trim(name); + + const std::size_t slash_pos = name.find_last_of("/\\"); + if (slash_pos != std::string::npos) + { + name = name.substr(slash_pos + 1); + } + + const std::size_t namespace_pos = name.rfind("::"); + if (namespace_pos != std::string::npos) + { + name = name.substr(namespace_pos + 2); + } + + std::transform(name.begin(), name.end(), name.begin(), [](unsigned char c) { + return static_cast(std::toupper(c)); + }); + + stripSuffix(name, "_JOINT"); + stripSuffix(name, "_MOTOR"); + stripPrefix(name, "JOINT_"); + + return name; + } + + static void trim(std::string& value) + { + value.erase( + value.begin(), + std::find_if(value.begin(), value.end(), [](unsigned char c) { return !std::isspace(c); }) + ); + value.erase( + std::find_if(value.rbegin(), value.rend(), [](unsigned char c) { return !std::isspace(c); }).base(), + value.end() + ); + } + + static void stripSuffix(std::string& value, const std::string& suffix) + { + if (value.size() >= suffix.size() && value.compare(value.size() - suffix.size(), suffix.size(), suffix) == 0) + { + value.erase(value.size() - suffix.size()); + } + } + + static void stripPrefix(std::string& value, const std::string& prefix) + { + if (value.size() >= prefix.size() && value.compare(0, prefix.size(), prefix) == 0) + { + value.erase(0, prefix.size()); + } + } + + std::string unmatchedNamesError( + const std::vector& incoming_names, + std::size_t named_readings, + std::size_t total_readings) const + { + std::ostringstream error; + error << "no ANYmal joint state names matched the configured URDF joint names"; + error << "; expected joints include " << sampleNames(flattened_joint_names_, 6); + if (named_readings == 0) + { + error << "; all " << total_readings << " joint names are empty"; + if (total_readings != flattened_joint_names_.size()) + { + error << " and reading-order fallback requires " << flattened_joint_names_.size() << " joints"; + } + } + else + { + error << "; incoming names include " << sampleNames(incoming_names, 6); + } + return error.str(); + } + + static std::string sampleNames(const std::vector& names, std::size_t max_count) + { + std::ostringstream sample; + sample << "["; + const std::size_t count = std::min(max_count, names.size()); + for (std::size_t i = 0; i < count; ++i) + { + if (i > 0) sample << ", "; + sample << names[i]; + } + if (names.size() > max_count) sample << ", ..."; + sample << "]"; + return sample.str(); + } + + pinocchio::Model model_; + mutable pinocchio::Data data_; + Eigen::VectorXd q_; + Eigen::VectorXd v_; + Eigen::VectorXd a_; + Eigen::VectorXd tau_; + Eigen::VectorXd tau_residual_; + + std::array foot_frame_names_; + std::array, 4> leg_joint_names_; + std::vector flattened_joint_names_; + std::array stance_state_{{false, false, false, false}}; + + double threshold_{30.0}; + double threshold_low_{25.0}; + double threshold_high_{35.0}; + bool use_hysteresis_{true}; + bool use_force_norm_{false}; + bool use_absolute_normal_force_{true}; + bool use_inverse_dynamics_compensation_{true}; + bool use_reading_order_when_names_missing_{true}; + double force_sign_{-1.0}; + Eigen::Vector3d contact_normal_{Eigen::Vector3d::UnitZ()}; + bool configured_{false}; + std::string last_error_; +}; + +} // namespace state_estimator + +#endif // GRF_CONTACT_ESTIMATOR_HPP diff --git a/muse_ws/src/state_estimator/include/state_estimator/Models/joint_state_snapshot.hpp b/muse_ws/src/state_estimator/include/state_estimator/Models/joint_state_snapshot.hpp new file mode 100644 index 0000000..c0477ac --- /dev/null +++ b/muse_ws/src/state_estimator/include/state_estimator/Models/joint_state_snapshot.hpp @@ -0,0 +1,26 @@ +#ifndef STATE_ESTIMATOR_MODELS_JOINT_STATE_SNAPSHOT_HPP +#define STATE_ESTIMATOR_MODELS_JOINT_STATE_SNAPSHOT_HPP + +#include +#include + +namespace state_estimator +{ + +struct JointStateSnapshot +{ + std::vector name; + std::vector position; + std::vector velocity; + std::vector acceleration; + std::vector effort; + + std::size_t size() const + { + return name.size(); + } +}; + +} // namespace state_estimator + +#endif // STATE_ESTIMATOR_MODELS_JOINT_STATE_SNAPSHOT_HPP diff --git a/muse_ws/src/state_estimator/include/state_estimator/Models/leg_odometry.hpp b/muse_ws/src/state_estimator/include/state_estimator/Models/leg_odometry.hpp new file mode 100644 index 0000000..2e787c4 --- /dev/null +++ b/muse_ws/src/state_estimator/include/state_estimator/Models/leg_odometry.hpp @@ -0,0 +1,530 @@ +#ifndef STATE_ESTIMATOR_MODELS_LEG_ODOMETRY_HPP +#define STATE_ESTIMATOR_MODELS_LEG_ODOMETRY_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "state_estimator/Models/joint_state_snapshot.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace state_estimator +{ + +struct LegKinematics +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + LegKinematics() + { + for (std::size_t leg = 0; leg < foot_position_base.size(); ++leg) + { + foot_position_base[leg].setZero(); + foot_velocity_base[leg].setZero(); + foot_jacobian_base[leg].setZero(); + } + } + + std::array foot_position_base; + std::array foot_velocity_base; + std::array foot_jacobian_base; +}; + +struct LegOdometryEstimate +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + LegOdometryEstimate() + { + for (auto& velocity : base_velocity_from_foot_base) + { + velocity.setZero(); + } + base_velocity_base.setZero(); + base_velocity_world.setZero(); + base_height = 0.0; + base_height_valid = false; + } + + LegKinematics kinematics; + std::array base_velocity_from_foot_base; + Eigen::Vector3d base_velocity_base; + Eigen::Vector3d base_velocity_world; + std::size_t stance_count{0}; + bool base_height_valid{false}; + double base_height{0.0}; +}; + +class LegOdometryModel +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + bool configure( + const std::string& urdf_path, + const std::vector& foot_frame_names, + const std::vector& joint_names, + const std::string& base_frame_name) + { + if (foot_frame_names.size() != 4) + { + last_error_ = "leg odometry needs exactly 4 foot frame names"; + configured_ = false; + return false; + } + if (joint_names.size() != 12) + { + last_error_ = "leg odometry needs exactly 12 joint names"; + configured_ = false; + return false; + } + + try + { + pinocchio::urdf::buildModel(urdf_path, model_); + } + catch (const std::exception& e) + { + last_error_ = std::string("failed to load URDF: ") + e.what(); + configured_ = false; + return false; + } + + data_ = pinocchio::Data(model_); + q_ = pinocchio::neutral(model_); + v_ = Eigen::VectorXd::Zero(model_.nv); + frame_jacobian_.resize(6, model_.nv); + frame_jacobian_.setZero(); + + foot_frame_names_ = footFrameNamesToArray(foot_frame_names); + joint_names_ = jointNamesToArray(joint_names); + flattened_joint_names_ = joint_names; + + for (std::size_t leg = 0; leg < foot_frame_names_.size(); ++leg) + { + if (!model_.existFrame(foot_frame_names_[leg])) + { + last_error_ = "missing foot frame '" + foot_frame_names_[leg] + "'"; + configured_ = false; + return false; + } + + foot_frame_ids_[leg] = model_.getFrameId(foot_frame_names_[leg]); + + for (std::size_t joint = 0; joint < joint_names_[leg].size(); ++joint) + { + const std::string& joint_name = joint_names_[leg][joint]; + if (!model_.existJointName(joint_name)) + { + last_error_ = "missing joint '" + joint_name + "'"; + configured_ = false; + return false; + } + + const pinocchio::JointIndex joint_id = model_.getJointId(joint_name); + const int q_index = model_.joints[joint_id].idx_q(); + const int v_index = model_.joints[joint_id].idx_v(); + if (q_index < 0 || q_index >= q_.size() || v_index < 0 || v_index >= v_.size()) + { + last_error_ = "joint '" + joint_name + "' has invalid Pinocchio q/v indices"; + configured_ = false; + return false; + } + + leg_q_indices_[leg][joint] = q_index; + leg_v_indices_[leg][joint] = v_index; + } + } + + has_base_frame_ = !base_frame_name.empty(); + if (has_base_frame_ && !model_.existFrame(base_frame_name)) + { + last_error_ = "missing base frame '" + base_frame_name + "'"; + configured_ = false; + return false; + } + if (has_base_frame_) + { + base_frame_id_ = model_.getFrameId(base_frame_name); + } + + configured_ = true; + last_error_.clear(); + return true; + } + + bool update( + const JointStateSnapshot& joints, + const Eigen::Vector3d& omega_base, + const std::array& stance, + const Eigen::Matrix3d& world_R_base, + LegOdometryEstimate& estimate) + { + if (!configured_) + { + last_error_ = "leg odometry model is not configured"; + return false; + } + + if (!fillStateFromJointState(joints)) + { + return false; + } + + computeKinematics(estimate.kinematics); + + estimate.base_velocity_base.setZero(); + estimate.base_velocity_world.setZero(); + estimate.stance_count = 0; + estimate.base_height = 0.0; + estimate.base_height_valid = false; + + for (std::size_t leg = 0; leg < estimate.base_velocity_from_foot_base.size(); ++leg) + { + estimate.base_velocity_from_foot_base[leg] = -( + estimate.kinematics.foot_velocity_base[leg] + + omega_base.cross(estimate.kinematics.foot_position_base[leg]) + ); + + if (stance[leg]) + { + estimate.base_velocity_base += estimate.base_velocity_from_foot_base[leg]; + estimate.base_height -= (world_R_base * estimate.kinematics.foot_position_base[leg]).z(); + estimate.stance_count++; + } + } + + const double stance_denominator = estimate.stance_count > 0 + ? static_cast(estimate.stance_count) + : 1.0; + estimate.base_velocity_base /= (stance_denominator + 1e-5); + estimate.base_velocity_world = world_R_base * estimate.base_velocity_base; + if (estimate.stance_count > 0) + { + estimate.base_height /= stance_denominator; + estimate.base_height_valid = true; + } + + last_error_.clear(); + return true; + } + + const std::string& lastError() const + { + return last_error_; + } + +private: + bool fillStateFromJointState(const JointStateSnapshot& joints) + { + std::unordered_map joint_position; + std::unordered_map joint_velocity; + std::vector incoming_names; + std::size_t named_readings = 0; + const std::size_t joint_count = maxJointFieldSize(joints); + + for (std::size_t joint_index = 0; joint_index < joint_count; ++joint_index) + { + std::string name = joint_index < joints.name.size() ? joints.name[joint_index] : std::string(); + if (!name.empty()) + { + named_readings++; + incoming_names.push_back(name); + } + else if ( + joint_count == flattened_joint_names_.size() && + joint_index < flattened_joint_names_.size()) + { + name = flattened_joint_names_[joint_index]; + } + + if (name.empty()) continue; + + insertJointValueIfPresent(joint_position, name, joints.position, joint_index); + insertJointValueIfPresent(joint_velocity, name, joints.velocity, joint_index); + } + + q_ = pinocchio::neutral(model_); + v_.setZero(); + + std::size_t matched_positions = 0; + std::size_t matched_velocities = 0; + for (std::size_t leg = 0; leg < joint_names_.size(); ++leg) + { + for (std::size_t joint = 0; joint < joint_names_[leg].size(); ++joint) + { + const std::string& joint_name = joint_names_[leg][joint]; + double joint_value = 0.0; + if (findJointValue(joint_position, joint_name, joint_value)) + { + q_[leg_q_indices_[leg][joint]] = joint_value; + matched_positions++; + } + if (findJointValue(joint_velocity, joint_name, joint_value)) + { + v_[leg_v_indices_[leg][joint]] = joint_value; + matched_velocities++; + } + } + } + + const std::size_t expected_joints = flattened_joint_names_.size(); + if (matched_positions != expected_joints || matched_velocities != expected_joints) + { + std::ostringstream error; + error << unmatchedNamesError(incoming_names, named_readings, joint_count) + << "; matched positions " << matched_positions << "/" << expected_joints + << ", velocities " << matched_velocities << "/" << expected_joints; + last_error_ = error.str(); + return false; + } + + return true; + } + + void computeKinematics(LegKinematics& kinematics) + { + pinocchio::forwardKinematics(model_, data_, q_, v_); + pinocchio::computeJointJacobians(model_, data_, q_); + pinocchio::updateFramePlacements(model_, data_); + + Eigen::Matrix3d world_R_base = Eigen::Matrix3d::Identity(); + Eigen::Vector3d world_p_base = Eigen::Vector3d::Zero(); + if (has_base_frame_) + { + world_R_base = data_.oMf[base_frame_id_].rotation(); + world_p_base = data_.oMf[base_frame_id_].translation(); + } + const Eigen::Matrix3d base_R_world = world_R_base.transpose(); + + for (std::size_t leg = 0; leg < foot_frame_ids_.size(); ++leg) + { + const pinocchio::FrameIndex foot_frame_id = foot_frame_ids_[leg]; + kinematics.foot_position_base[leg] = + base_R_world * (data_.oMf[foot_frame_id].translation() - world_p_base); + + frame_jacobian_.setZero(); + pinocchio::getFrameJacobian( + model_, + data_, + foot_frame_id, + pinocchio::LOCAL_WORLD_ALIGNED, + frame_jacobian_); + + Eigen::Matrix3d foot_jacobian_world; + for (std::size_t joint = 0; joint < 3; ++joint) + { + foot_jacobian_world.col(joint) = frame_jacobian_.topRows<3>().col(leg_v_indices_[leg][joint]); + } + + kinematics.foot_jacobian_base[leg] = base_R_world * foot_jacobian_world; + + Eigen::Vector3d leg_velocity; + for (std::size_t joint = 0; joint < 3; ++joint) + { + leg_velocity[joint] = v_[leg_v_indices_[leg][joint]]; + } + kinematics.foot_velocity_base[leg] = kinematics.foot_jacobian_base[leg] * leg_velocity; + } + } + + static std::array footFrameNamesToArray(const std::vector& names) + { + return {{names[0], names[1], names[2], names[3]}}; + } + + static std::array, 4> jointNamesToArray(const std::vector& names) + { + return {{ + {{names[0], names[1], names[2]}}, + {{names[3], names[4], names[5]}}, + {{names[6], names[7], names[8]}}, + {{names[9], names[10], names[11]}} + }}; + } + + static void insertJointValue(std::unordered_map& values, const std::string& name, double value) + { + values[name] = value; + const std::string normalized_name = normalizeJointName(name); + if (!normalized_name.empty()) + { + values[normalized_name] = value; + } + } + + static void insertJointValueIfPresent( + std::unordered_map& values, + const std::string& name, + const std::vector& source, + std::size_t index) + { + if (index < source.size()) + { + insertJointValue(values, name, source[index]); + } + } + + static std::size_t maxJointFieldSize(const JointStateSnapshot& joints) + { + return std::max({ + joints.name.size(), + joints.position.size(), + joints.velocity.size(), + joints.acceleration.size(), + joints.effort.size() + }); + } + + static bool findJointValue( + const std::unordered_map& values, + const std::string& joint_name, + double& value) + { + auto value_it = values.find(joint_name); + if (value_it != values.end()) + { + value = value_it->second; + return true; + } + + value_it = values.find(normalizeJointName(joint_name)); + if (value_it != values.end()) + { + value = value_it->second; + return true; + } + + return false; + } + + static std::string normalizeJointName(std::string name) + { + trim(name); + + const std::size_t slash_pos = name.find_last_of("/\\"); + if (slash_pos != std::string::npos) + { + name = name.substr(slash_pos + 1); + } + + const std::size_t namespace_pos = name.rfind("::"); + if (namespace_pos != std::string::npos) + { + name = name.substr(namespace_pos + 2); + } + + std::transform(name.begin(), name.end(), name.begin(), [](unsigned char c) { + return static_cast(std::toupper(c)); + }); + + stripSuffix(name, "_JOINT"); + stripSuffix(name, "_MOTOR"); + stripPrefix(name, "JOINT_"); + + return name; + } + + static void trim(std::string& value) + { + value.erase( + value.begin(), + std::find_if(value.begin(), value.end(), [](unsigned char c) { return !std::isspace(c); }) + ); + value.erase( + std::find_if(value.rbegin(), value.rend(), [](unsigned char c) { return !std::isspace(c); }).base(), + value.end() + ); + } + + static void stripSuffix(std::string& value, const std::string& suffix) + { + if (value.size() >= suffix.size() && value.compare(value.size() - suffix.size(), suffix.size(), suffix) == 0) + { + value.erase(value.size() - suffix.size()); + } + } + + static void stripPrefix(std::string& value, const std::string& prefix) + { + if (value.size() >= prefix.size() && value.compare(0, prefix.size(), prefix) == 0) + { + value.erase(0, prefix.size()); + } + } + + std::string unmatchedNamesError( + const std::vector& incoming_names, + std::size_t named_readings, + std::size_t total_readings) const + { + std::ostringstream error; + error << "ANYmal joint state did not cover all configured leg joint position/velocity names"; + error << "; expected joints include " << sampleNames(flattened_joint_names_, 6); + if (named_readings == 0) + { + error << "; all " << total_readings << " joint names are empty"; + if (total_readings != flattened_joint_names_.size()) + { + error << " and reading-order fallback requires " << flattened_joint_names_.size() << " joints"; + } + } + else + { + error << "; incoming names include " << sampleNames(incoming_names, 6); + } + return error.str(); + } + + static std::string sampleNames(const std::vector& names, std::size_t max_count) + { + std::ostringstream sample; + sample << "["; + const std::size_t count = std::min(max_count, names.size()); + for (std::size_t i = 0; i < count; ++i) + { + if (i > 0) sample << ", "; + sample << names[i]; + } + if (names.size() > max_count) sample << ", ..."; + sample << "]"; + return sample.str(); + } + + pinocchio::Model model_; + mutable pinocchio::Data data_; + Eigen::VectorXd q_; + Eigen::VectorXd v_; + Eigen::Matrix frame_jacobian_; + + std::array foot_frame_names_; + std::array foot_frame_ids_; + std::array, 4> joint_names_; + std::array, 4> leg_q_indices_; + std::array, 4> leg_v_indices_; + std::vector flattened_joint_names_; + + pinocchio::FrameIndex base_frame_id_{0}; + bool has_base_frame_{false}; + bool configured_{false}; + std::string last_error_; +}; + +} // namespace state_estimator + +#endif // STATE_ESTIMATOR_MODELS_LEG_ODOMETRY_HPP diff --git a/muse_ws/src/state_estimator/include/state_estimator/Models/sensor_fusion.hpp b/muse_ws/src/state_estimator/include/state_estimator/Models/sensor_fusion.hpp index 5a6be3c..b71a891 100644 --- a/muse_ws/src/state_estimator/include/state_estimator/Models/sensor_fusion.hpp +++ b/muse_ws/src/state_estimator/include/state_estimator/Models/sensor_fusion.hpp @@ -17,6 +17,7 @@ namespace state_estimator void predict(double t, const Eigen::Matrix &u) override; void update(double t, const Eigen::Matrix &z); + void updateZPosition(double t, double z, double measurement_variance); void setMatricesSF(bool slippage); protected: @@ -106,6 +107,30 @@ namespace state_estimator this->fixP(); } + inline void KFSensorFusion::updateZPosition(double t, double z, double measurement_variance) + { + if (measurement_variance <= 0.0) + { + return; + } + + Eigen::Matrix H_z = Eigen::Matrix::Zero(); + H_z(2) = 1.0; + + const double innovation = z - this->xhat(2); + const double S = (H_z * this->P * H_z.transpose())(0, 0) + measurement_variance; + if (S <= 0.0) + { + return; + } + + const Eigen::Matrix K_z = this->P * H_z.transpose() / S; + this->xhat = this->xhat + K_z * innovation; + this->P = (this->I - K_z * H_z) * this->P; + + this->fixP(); + } + inline Eigen::Matrix KFSensorFusion::calc_f(double t, const Eigen::Matrix &x, const Eigen::Matrix &u) { // x is composed of [position; velocity], so extract velocity: diff --git a/muse_ws/src/state_estimator/launch/state_estimator.launch b/muse_ws/src/state_estimator/launch/state_estimator.launch index 16e21e4..af6ca35 100644 --- a/muse_ws/src/state_estimator/launch/state_estimator.launch +++ b/muse_ws/src/state_estimator/launch/state_estimator.launch @@ -2,19 +2,24 @@ + + + - + - + + - + + diff --git a/muse_ws/src/state_estimator/package.xml b/muse_ws/src/state_estimator/package.xml index 012bb9f..21966f5 100644 --- a/muse_ws/src/state_estimator/package.xml +++ b/muse_ws/src/state_estimator/package.xml @@ -5,28 +5,30 @@ The state_estimator package Geoff Fink - - TODO - Geoff Fink + TODO catkin - dynamic_reconfigure + roscpp + roslib + rosconsole + dynamic_reconfigure sensor_msgs + geometry_msgs + nav_msgs + std_msgs tf2_ros pluginlib + message_filters iit_commons - message_generation state_estimator_msgs + pinocchio pinocchio - - - - - + + diff --git a/muse_ws/src/state_estimator/rviz/state_estimator.rviz b/muse_ws/src/state_estimator/rviz/state_estimator.rviz new file mode 100644 index 0000000..9861078 --- /dev/null +++ b/muse_ws/src/state_estimator/rviz/state_estimator.rviz @@ -0,0 +1,155 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /Fused Odometry1 + Splitter Ratio: 0.5 + Tree Height: 561 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 0.8 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Class: rviz/Axes + Enabled: true + Length: 0.7 + Name: Base Axes + Radius: 0.04 + Reference Frame: base + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.3 + Color: 204; 51; 204 + Scale: 1 + Value: false + Value: false + Enabled: true + Keep: 200 + Name: Fused Odometry + Position Tolerance: 0.05 + Shape: Arrow + Topic: /state_estimator/sensor_fusion + Value: true + Enabled: true + Global Options: + Background Color: 36; 36; 36 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.261799 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.65 + Target Frame: + Value: Orbit + Yaw: 0.78 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/muse_ws/src/state_estimator/src/plugins/attitude_estimation_plugin.cpp b/muse_ws/src/state_estimator/src/plugins/attitude_estimation_plugin.cpp index c2c706c..13f3aec 100644 --- a/muse_ws/src/state_estimator/src/plugins/attitude_estimation_plugin.cpp +++ b/muse_ws/src/state_estimator/src/plugins/attitude_estimation_plugin.cpp @@ -56,7 +56,7 @@ namespace state_estimator_plugins std::vector P_vec, Q_vec, R_vec; double ki_param, kp_param; - nh_.param("attitude_estimation_plugin/imu_topic", imu_topic, std::string("/sensors/imu")); + nh_.param("attitude_estimation_plugin/imu_topic", imu_topic, std::string("/anymal/imu")); nh_.param("attitude_estimation_plugin/pub_topic", pub_topic, std::string("attitude")); nh_.param("attitude_estimation_plugin/ki", ki_param, 0.02); nh_.param("attitude_estimation_plugin/kp", kp_param, 10.0); diff --git a/muse_ws/src/state_estimator/src/plugins/contact_detection_plugin.cpp b/muse_ws/src/state_estimator/src/plugins/contact_detection_plugin.cpp index 511e826..68a8f87 100644 --- a/muse_ws/src/state_estimator/src/plugins/contact_detection_plugin.cpp +++ b/muse_ws/src/state_estimator/src/plugins/contact_detection_plugin.cpp @@ -1,157 +1,454 @@ +#include "state_estimator/Models/grf_contact_estimator.hpp" +#include "state_estimator/Models/joint_state_snapshot.hpp" #include "state_estimator/plugin.hpp" -#include -#include "state_estimator_msgs/ContactDetection.h" -#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace state_estimator_plugins { -typedef message_filters::sync_policies::ApproximateTime -< - geometry_msgs::WrenchStamped, - geometry_msgs::WrenchStamped, - geometry_msgs::WrenchStamped, - geometry_msgs::WrenchStamped -> -ApproximateTimePolicy; - -typedef message_filters::sync_policies::ExactTime -< - geometry_msgs::WrenchStamped, - geometry_msgs::WrenchStamped, - geometry_msgs::WrenchStamped, - geometry_msgs::WrenchStamped -> -ExactTimePolicy; - -#define MySyncPolicy ApproximateTimePolicy - - class ContactDetectionPlugin : public PluginBase - { - public: - ContactDetectionPlugin(): - wrench_lf_sub_(nullptr), - wrench_rf_sub_(nullptr), - wrench_lh_sub_(nullptr), - wrench_rh_sub_(nullptr), - pub_(nullptr), - sync_(nullptr) - { } - - ~ContactDetectionPlugin() - { - if (wrench_lf_sub_!=nullptr) delete(wrench_lf_sub_); - if (wrench_rf_sub_!=nullptr) delete(wrench_rf_sub_); - if (wrench_lh_sub_!=nullptr) delete(wrench_lh_sub_); - if (wrench_rh_sub_!=nullptr) delete(wrench_rh_sub_); - if (pub_!=nullptr) delete(pub_); - if (sync_!=nullptr) delete(sync_); - } - - std::string getName() override { return std::string("ContactDetection"); } - std::string getDescription() override { return std::string("Contact Detection Plugin"); } - - void initialize_() override { - - // Load parameters from YAML - // LF: left front leg, RF: right front leg, LH: left hind leg, RH: right hind leg - std::string lf_topic, rf_topic, lh_topic, rh_topic, pub_topic; - nh_.param("contact_detection_plugin/wrench_lf_topic", lf_topic, std::string("/state_estimator/contact_force_lf_foot")); - nh_.param("contact_detection_plugin/wrench_rf_topic", rf_topic, std::string("/state_estimator/contact_force_rf_foot")); - nh_.param("contact_detection_plugin/wrench_lh_topic", lh_topic, std::string("/state_estimator/contact_force_lh_foot")); - nh_.param("contact_detection_plugin/wrench_rh_topic", rh_topic, std::string("/state_estimator/contact_force_rh_foot")); - nh_.param("contact_detection_plugin/pub_topic", pub_topic, std::string("contact_detection")); - nh_.param("contact_detection_plugin/grf_threshold", grf_threshold_, 15.0); - - ROS_INFO_STREAM("ContactDetectionPlugin loaded with GRF threshold: " << grf_threshold_); - - // Set up subscribers - wrench_lf_sub_ = new message_filters::Subscriber(nh_, lf_topic, 250); - wrench_rf_sub_ = new message_filters::Subscriber(nh_, rf_topic, 250); - wrench_lh_sub_ = new message_filters::Subscriber(nh_, lh_topic, 250); - wrench_rh_sub_ = new message_filters::Subscriber(nh_, rh_topic, 250); - - // Synchronizer - sync_ = new message_filters::Synchronizer(MySyncPolicy(250), *wrench_lf_sub_, *wrench_rf_sub_, *wrench_lh_sub_, *wrench_rh_sub_); - sync_->registerCallback(boost::bind(&ContactDetectionPlugin::callback, this, _1, _2, _3, _4)); - - // Publisher - pub_ = new ros::Publisher(nh_.advertise(pub_topic, 250)); - - } - - - void shutdown_() override { } - void pause_() override { } - void resume_() override { } - void reset_() override { } - - void callback - ( - const geometry_msgs::WrenchStamped::ConstPtr& wrench_lf, - const geometry_msgs::WrenchStamped::ConstPtr& wrench_rf, - const geometry_msgs::WrenchStamped::ConstPtr& wrench_lh, - const geometry_msgs::WrenchStamped::ConstPtr& wrench_rh - ) - { - // Calculate the norm of the wrenches - wrench_lf_norm = sqrt(pow(wrench_lf->wrench.force.x,2) + pow(wrench_lf->wrench.force.y,2) + pow(wrench_lf->wrench.force.z,2)); - wrench_rf_norm = sqrt(pow(wrench_rf->wrench.force.x,2) + pow(wrench_rf->wrench.force.y,2) + pow(wrench_rf->wrench.force.z,2)); - wrench_lh_norm = sqrt(pow(wrench_lh->wrench.force.x,2) + pow(wrench_lh->wrench.force.y,2) + pow(wrench_lh->wrench.force.z,2)); - wrench_rh_norm = sqrt(pow(wrench_rh->wrench.force.x,2) + pow(wrench_rh->wrench.force.y,2) + pow(wrench_rh->wrench.force.z,2)); - - if (wrench_lf_norm > grf_threshold_) stance_lf = true; else stance_lf = false; - if (wrench_rf_norm > grf_threshold_) stance_rf = true; else stance_rf = false; - if (wrench_lh_norm > grf_threshold_) stance_lh = true; else stance_lh = false; - if (wrench_rh_norm > grf_threshold_) stance_rh = true; else stance_rh = false; - - - // publishing - msg_.header.stamp = ros::Time::now(); - - msg_.stance_lf = stance_lf; - msg_.stance_rf = stance_rf; - msg_.stance_lh = stance_lh; - msg_.stance_rh = stance_rh; - - pub_->publish(msg_); - - } // end callback - - - private: - - message_filters::Subscriber *wrench_lf_sub_; - message_filters::Subscriber *wrench_rf_sub_; - message_filters::Subscriber *wrench_lh_sub_; - message_filters::Subscriber *wrench_rh_sub_; - message_filters::Synchronizer *sync_; - ros::Publisher *pub_; - - state_estimator_msgs::ContactDetection msg_; - - bool stance_lf; - bool stance_rf; - bool stance_lh; - bool stance_rh; - double wrench_lf_norm; - double wrench_rf_norm; - double wrench_lh_norm; - double wrench_rh_norm; - double grf_threshold_; - - - - }; // end class ContactDetectionPlugin - -} //end namespace state_estimator_plugins +class ContactDetectionPlugin : public PluginBase +{ +public: + ContactDetectionPlugin() = default; + ~ContactDetectionPlugin() override = default; + + std::string getName() override + { + return std::string("ContactDetection"); + } + + std::string getDescription() override + { + return std::string("AnymalState GRF contact detection plugin"); + } + + void initialize_() override + { + + std::string pub_topic; + nh_.param( + "contact_detection_plugin/pub_topic", + pub_topic, + std::string("contact_detection") + ); + + nh_.param( + "contact_detection_plugin/grf_threshold", + grf_threshold_, + 40.0 + ); + + pub_ = nh_.advertise( + pub_topic, + 250 + ); + + initializeAnymalStateInput(); + } + + void shutdown_() override + { + anymal_state_sub_.shutdown(); + pub_.shutdown(); + } + + void pause_() override {} + void resume_() override {} + void reset_() override {} + +private: + void initializeAnymalStateInput() + { + std::string anymal_state_topic; + std::string urdf_path_param; + + nh_.param( + "contact_detection_plugin/anymal_state_topic", + anymal_state_topic, + std::string("/anymal/state_estimator/anymal_state") + ); + + nh_.param( + "contact_detection_plugin/urdf_path", + urdf_path_param, + std::string("$(find state_estimator)/urdfs/anymal.urdf") + ); + + ROS_INFO_STREAM( + "ContactDetectionPlugin subscribing to AnymalState topic: " + << anymal_state_topic + ); + + anymal_state_sub_ = nh_.subscribe( + anymal_state_topic, + 250, + &ContactDetectionPlugin::anymalStateCallback, + this + ); + + const std::string resolved_urdf_path = resolveRosFind(urdf_path_param); + + ROS_INFO_STREAM( + "ContactDetectionPlugin using URDF path: " + << resolved_urdf_path + ); + + std::vector foot_frame_names_vec; + nh_.param( + "contact_detection_plugin/foot_frame_names", + foot_frame_names_vec, + std::vector{"LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"} + ); + + const std::array foot_frame_names = toStringArray4( + foot_frame_names_vec, + {{"LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"}}, + "foot_frame_names" + ); + + std::vector lf_joint_names; + std::vector rf_joint_names; + std::vector lh_joint_names; + std::vector rh_joint_names; + + nh_.param( + "contact_detection_plugin/lf_joint_names", + lf_joint_names, + std::vector{"LF_HAA", "LF_HFE", "LF_KFE"} + ); + + nh_.param( + "contact_detection_plugin/rf_joint_names", + rf_joint_names, + std::vector{"RF_HAA", "RF_HFE", "RF_KFE"} + ); + + nh_.param( + "contact_detection_plugin/lh_joint_names", + lh_joint_names, + std::vector{"LH_HAA", "LH_HFE", "LH_KFE"} + ); + + nh_.param( + "contact_detection_plugin/rh_joint_names", + rh_joint_names, + std::vector{"RH_HAA", "RH_HFE", "RH_KFE"} + ); + + const std::array, 4> leg_joint_names{{ + lf_joint_names, + rf_joint_names, + lh_joint_names, + rh_joint_names + }}; + + std::vector contact_normal_vec; + nh_.param( + "contact_detection_plugin/contact_normal", + contact_normal_vec, + std::vector{0.0, 0.0, 1.0} + ); + + const Eigen::Vector3d contact_normal = toVector3( + contact_normal_vec, + Eigen::Vector3d::UnitZ(), + "contact_normal" + ); + + double threshold_low; + double threshold_high; + double force_sign; + + bool use_hysteresis; + bool use_force_norm; + bool use_absolute_normal_force; + bool use_inverse_dynamics_compensation; + + nh_.param( + "contact_detection_plugin/grf_threshold_low", + threshold_low, + grf_threshold_ + ); + + nh_.param( + "contact_detection_plugin/grf_threshold_high", + threshold_high, + grf_threshold_ + ); + + nh_.param( + "contact_detection_plugin/use_hysteresis", + use_hysteresis, + false + ); + + nh_.param( + "contact_detection_plugin/use_force_norm", + use_force_norm, + false + ); + + nh_.param( + "contact_detection_plugin/use_absolute_normal_force", + use_absolute_normal_force, + true + ); + + nh_.param( + "contact_detection_plugin/use_inverse_dynamics_compensation", + use_inverse_dynamics_compensation, + true + ); + + nh_.param( + "contact_detection_plugin/force_sign", + force_sign, + -1.0 + ); + + try + { + estimator_configured_ = grf_contact_estimator_.configure( + resolved_urdf_path, + foot_frame_names, + leg_joint_names, + grf_threshold_, + threshold_low, + threshold_high, + use_hysteresis, + use_force_norm, + use_absolute_normal_force, + force_sign, + use_inverse_dynamics_compensation, + contact_normal + ); + + if (!estimator_configured_) + { + ROS_ERROR_STREAM( + "Failed to configure GRF contact estimator: " + << grf_contact_estimator_.lastError() + ); + return; + } + + } + catch (const std::exception& e) + { + estimator_configured_ = false; + + ROS_ERROR_STREAM( + "Failed to configure GRF contact estimator: " + << e.what() + ); + + return; + } + + ROS_INFO_STREAM( + "ContactDetectionPlugin estimating contact from AnymalState topic " + << anymal_state_topic + << " with GRF threshold: " + << grf_threshold_ + ); + } + + void anymalStateCallback(const anymal_msgs::AnymalState::ConstPtr& anymal_state) + { + if (!estimator_configured_) + { + ROS_WARN_STREAM_THROTTLE( + 1.0, + "ContactDetectionPlugin received AnymalState, but GRF estimator is not configured" + ); + return; + } + + const ros::Time stamp = stampFromAnymalState(*anymal_state); + + state_estimator::GrfContactEstimate estimate; + + const state_estimator::JointStateSnapshot joints = + jointSnapshotFromAnymalState(*anymal_state); + + if (!grf_contact_estimator_.update(joints, estimate)) + { + ROS_WARN_STREAM_THROTTLE( + 1.0, + "GRF contact estimator update failed: " + << grf_contact_estimator_.lastError() + ); + return; + } + + ROS_DEBUG_STREAM_THROTTLE( + 1.0, + "Estimated contact metrics [LF RF LH RH]: " + << estimate.metric[0] << ", " + << estimate.metric[1] << ", " + << estimate.metric[2] << ", " + << estimate.metric[3] + ); + + publishContact(stamp, estimate.stance); + } + + void publishContact( + const ros::Time& stamp, + const std::array& stance) + { + msg_.header.stamp = stamp.isZero() ? ros::Time::now() : stamp; + + msg_.stance_lf = stance[0]; + msg_.stance_rf = stance[1]; + msg_.stance_lh = stance[2]; + msg_.stance_rh = stance[3]; + + pub_.publish(msg_); + } + + static ros::Time stampFromAnymalState( + const anymal_msgs::AnymalState& anymal_state) + { + if (!anymal_state.header.stamp.isZero()) + { + return anymal_state.header.stamp; + } + + if (!anymal_state.joints.header.stamp.isZero()) + { + return anymal_state.joints.header.stamp; + } + + if (!anymal_state.pose.header.stamp.isZero()) + { + return anymal_state.pose.header.stamp; + } + + if (!anymal_state.twist.header.stamp.isZero()) + { + return anymal_state.twist.header.stamp; + } + + return ros::Time::now(); + } + + static state_estimator::JointStateSnapshot jointSnapshotFromAnymalState( + const anymal_msgs::AnymalState& anymal_state) + { + state_estimator::JointStateSnapshot joints; + + joints.name = anymal_state.joints.name; + joints.position = anymal_state.joints.position; + joints.velocity = anymal_state.joints.velocity; + joints.acceleration = anymal_state.joints.acceleration; + joints.effort = anymal_state.joints.effort; + + return joints; + } + + static std::string resolveRosFind(const std::string& input) + { + std::string resolved = input; + + const std::string find_token = "$(find "; + const size_t token_pos = resolved.find(find_token); + + if (token_pos == std::string::npos) + { + return resolved; + } + + const size_t package_start = token_pos + find_token.length(); + const size_t package_end = resolved.find(")", package_start); + + if (package_end == std::string::npos) + { + return resolved; + } + + const std::string package_name = + resolved.substr(package_start, package_end - package_start); + + const std::string package_path = ros::package::getPath(package_name); + + if (package_path.empty()) + { + return resolved; + } + + resolved.replace( + token_pos, + package_end - token_pos + 1, + package_path + ); + + return resolved; + } + + static std::array toStringArray4( + const std::vector& values, + const std::array& fallback, + const std::string& param_name) + { + if (values.size() != 4) + { + ROS_WARN_STREAM( + "Parameter " + << param_name + << " must contain exactly 4 strings. Using defaults." + ); + + return fallback; + } + + return {{values[0], values[1], values[2], values[3]}}; + } + + static Eigen::Vector3d toVector3( + const std::vector& values, + const Eigen::Vector3d& fallback, + const std::string& param_name) + { + if (values.size() != 3) + { + ROS_WARN_STREAM( + "Parameter " + << param_name + << " must contain exactly 3 values. Using default." + ); + + return fallback; + } + + return Eigen::Vector3d(values[0], values[1], values[2]); + } + + ros::Subscriber anymal_state_sub_; + ros::Publisher pub_; + + bool estimator_configured_{false}; + + state_estimator::GrfContactEstimator grf_contact_estimator_; + state_estimator_msgs::ContactDetection msg_; + + double grf_threshold_{30.0}; +}; + +} // namespace state_estimator_plugins #include -PLUGINLIB_EXPORT_CLASS(state_estimator_plugins::ContactDetectionPlugin, state_estimator_plugins::PluginBase) +PLUGINLIB_EXPORT_CLASS( + state_estimator_plugins::ContactDetectionPlugin, + state_estimator_plugins::PluginBase +) \ No newline at end of file diff --git a/muse_ws/src/state_estimator/src/plugins/leg_odometry_plugin.cpp b/muse_ws/src/state_estimator/src/plugins/leg_odometry_plugin.cpp index 5bf0dcc..624121d 100644 --- a/muse_ws/src/state_estimator/src/plugins/leg_odometry_plugin.cpp +++ b/muse_ws/src/state_estimator/src/plugins/leg_odometry_plugin.cpp @@ -1,299 +1,329 @@ -#include -#include -#include -#include -#include -#include -#include - #include +#include "state_estimator/Models/joint_state_snapshot.hpp" +#include "state_estimator/Models/leg_odometry.hpp" #include "state_estimator/plugin.hpp" -#include -#include - -#include "state_estimator_msgs/LegOdometry.h" -#include "state_estimator_msgs/JointStateWithAcceleration.h" #include "state_estimator_msgs/ContactDetection.h" +#include "state_estimator_msgs/LegOdometry.h" #include "state_estimator_msgs/attitude.h" -#include +#include +#include #include -#include - -#include -#include -#include -#include +#include -#include +#include +#include +#include +#include +#include +#include namespace state_estimator_plugins { -typedef message_filters::sync_policies::ApproximateTime -< - sensor_msgs::Imu, - // state_estimator_msgs::JointStateWithAcceleration, - sensor_msgs::JointState, - state_estimator_msgs::ContactDetection, - state_estimator_msgs::attitude -> -ApproximateTimePolicy; - -typedef message_filters::sync_policies::ExactTime -< - sensor_msgs::Imu, - // state_estimator_msgs::JointStateWithAcceleration, - sensor_msgs::JointState, - state_estimator_msgs::ContactDetection, - state_estimator_msgs::attitude -> -ExactTimePolicy; - -#define MySyncPolicy ApproximateTimePolicy - -// This class calculates the base velocity as estimated from the leg kinematics. These are NOT the velocity of the legs. - class LegOdometryPlugin : public PluginBase +class LegOdometryPlugin : public PluginBase +{ +public: + LegOdometryPlugin(): + imu_sub_(nullptr), + anymal_state_sub_(nullptr), + contact_sub_(nullptr), + attitude_sub_(nullptr), + pub_(nullptr) + { } + + ~LegOdometryPlugin() { - public: - LegOdometryPlugin(): - imu_sub_(nullptr), - joint_state_sub_(nullptr), - contact_sub_(nullptr), - attitude_sub(nullptr), - pub_(nullptr), - sync_(nullptr) - { } - - ~LegOdometryPlugin() + if (imu_sub_!=nullptr) delete(imu_sub_); + if (anymal_state_sub_!=nullptr) delete(anymal_state_sub_); + if (contact_sub_!=nullptr) delete(contact_sub_); + if (attitude_sub_!=nullptr) delete(attitude_sub_); + if (pub_!=nullptr) delete(pub_); + } + + std::string getName() override { return std::string("LegOdometry"); } + std::string getDescription() override { return std::string("Leg Odometry Plugin"); } + + void initialize_() override + { + std::string urdf_path_param; + nh_.param("leg_odometry_plugin/urdf_path", urdf_path_param, std::string("")); + if (urdf_path_param.empty()) { - // if (lo_!=nullptr) delete(lo_); - if (imu_sub_!=nullptr) delete(imu_sub_); - if (joint_state_sub_!=nullptr) delete(joint_state_sub_); - if (contact_sub_!=nullptr) delete(contact_sub_); - if (attitude_sub!=nullptr) delete(attitude_sub); - if (pub_!=nullptr) delete(pub_); - if (sync_!=nullptr) delete(sync_); + ROS_ERROR("URDF path is not set in parameter server."); + return; } - std::string getName() override { return std::string("LegOdometry"); } - std::string getDescription() override { return std::string("Leg Odometry Plugin"); } - - void initialize_() override { - - std::string urdf_path_param; - nh_.param("leg_odometry_plugin/urdf_path", urdf_path_param, std::string("")); - - if (urdf_path_param.empty()) { - ROS_ERROR("URDF path is not set in parameter server."); - return; - } - - // Resolve $(find ...) manually if present - std::string resolved_path = urdf_path_param; - std::string find_token = "$(find "; - if (resolved_path.find(find_token) != std::string::npos) { - size_t start = resolved_path.find(find_token) + find_token.length(); - size_t end = resolved_path.find(")", start); - std::string package_name = resolved_path.substr(start, end - start); - std::string package_path = ros::package::getPath(package_name); - resolved_path.replace(resolved_path.find(find_token), end - resolved_path.find(find_token) + 1, package_path); - } - - ROS_INFO_STREAM("Loading URDF from: " << resolved_path); - - try { - pinocchio::urdf::buildModel(resolved_path, model_); - data_ = pinocchio::Data(model_); - ROS_INFO("URDF loaded into Pinocchio model successfully."); - } catch (const std::exception& e) { - ROS_ERROR_STREAM("Failed to load URDF: " << e.what()); - } - - std::vector base_R_imu_vec; - nh_.param("attitude_estimation_plugin/base_R_imu", base_R_imu_vec, std::vector(9, 0.0)); - - if (base_R_imu_vec.size() == 9) { - base_R_imu_ = Eigen::Map>(base_R_imu_vec.data()); - // ROS_INFO_STREAM("Loaded base_R_imu:\n" << base_R_imu_); - } else { - ROS_WARN("base_R_imu is not the correct size (should be 9). Using identity."); - base_R_imu_ = Eigen::Matrix3d::Identity(); - } - - std::string imu_topic, joint_states_topic, contact_topic, attitude_topic, pub_topic; - - // Get topic names from the parameter server - nh_.param("leg_odometry_plugin/imu_topic", imu_topic, std::string("/sensors/imu")); - nh_.param("leg_odometry_plugin/joint_states_topic", joint_states_topic, std::string("/state_estimator/joint_states")); - nh_.param("leg_odometry_plugin/contact_topic", contact_topic, std::string("/state_estimator/contact_detection")); - nh_.param("leg_odometry_plugin/attitude_topic", attitude_topic, std::string("/state_estimator/attitude")); - - // Set up subscribers using the loaded topic names - imu_sub_ = new message_filters::Subscriber(nh_, imu_topic, 250); - // joint_state_sub_ = new message_filters::Subscriber(nh_, joint_states_topic, 250); - joint_state_sub_ = new message_filters::Subscriber(nh_, joint_states_topic, 250); - contact_sub_ = new message_filters::Subscriber(nh_, contact_topic, 250); - attitude_sub = new message_filters::Subscriber(nh_, attitude_topic, 250); - - sync_ = new message_filters::Synchronizer(MySyncPolicy(100),*imu_sub_,*joint_state_sub_,*contact_sub_,*attitude_sub); - sync_->registerCallback(boost::bind(&LegOdometryPlugin::callback, this, _1, _2, _3,_4)); - - nh_.param("leg_odometry_plugin/pub_topic", pub_topic, std::string("/state_estimator/leg_odometry")); - pub_ = new ros::Publisher(nh_.advertise(pub_topic, 250)); + const std::string resolved_path = resolveRosFind(urdf_path_param); + ROS_INFO_STREAM("Loading URDF from: " << resolved_path); + std::vector base_R_imu_vec; + nh_.param("leg_odometry_plugin/base_R_imu", base_R_imu_vec, defaultBaseRImuVector()); + if (base_R_imu_vec.size() == 9) + { + base_R_imu_ = Eigen::Map>(base_R_imu_vec.data()); + } + else + { + ROS_WARN("leg_odometry_plugin/base_R_imu is not the correct size (should be 9). Using offline ANYmal-D default."); + const std::vector default_base_R_imu = defaultBaseRImuVector(); + base_R_imu_ = Eigen::Map>(default_base_R_imu.data()); } + std::vector foot_frame_names; + nh_.param("leg_odometry_plugin/foot_frame_names", foot_frame_names, defaultFootFrameNames()); + if (foot_frame_names.size() != 4) + { + ROS_WARN("leg_odometry_plugin/foot_frame_names must contain exactly 4 names. Using ANYmal defaults."); + foot_frame_names = defaultFootFrameNames(); + } - void shutdown_() override { } - void pause_() override { } - void resume_() override { } - void reset_() override { } + std::vector joint_names; + nh_.param("leg_odometry_plugin/joint_names", joint_names, defaultJointNames()); + if (joint_names.size() != 12) + { + ROS_WARN("leg_odometry_plugin/joint_names must contain exactly 12 names. Using ANYmal defaults."); + joint_names = defaultJointNames(); + } + + std::string base_frame_name; + nh_.param("leg_odometry_plugin/base_frame_name", base_frame_name, std::string("base")); + + model_configured_ = leg_odometry_.configure(resolved_path, foot_frame_names, joint_names, base_frame_name); + if (!model_configured_) + { + ROS_ERROR_STREAM("Failed to configure leg odometry model: " << leg_odometry_.lastError()); + return; + } + ROS_INFO("Leg odometry Pinocchio model configured successfully."); + + std::string imu_topic, anymal_state_topic, contact_topic, attitude_topic, pub_topic; + nh_.param("leg_odometry_plugin/imu_topic", imu_topic, std::string("/anymal/imu")); + nh_.param("leg_odometry_plugin/anymal_state_topic", anymal_state_topic, std::string("/anymal/state")); + nh_.param("leg_odometry_plugin/contact_topic", contact_topic, std::string("/state_estimator/contact_detection")); + nh_.param("leg_odometry_plugin/attitude_topic", attitude_topic, std::string("/state_estimator/attitude")); + nh_.param("leg_odometry_plugin/pub_topic", pub_topic, std::string("leg_odometry")); + + imu_sub_ = new ros::Subscriber(nh_.subscribe(imu_topic, 250, &LegOdometryPlugin::imuCallback, this)); + anymal_state_sub_ = new ros::Subscriber(nh_.subscribe(anymal_state_topic, 250, &LegOdometryPlugin::anymalStateCallback, this)); + contact_sub_ = new ros::Subscriber(nh_.subscribe(contact_topic, 250, &LegOdometryPlugin::contactCallback, this)); + attitude_sub_ = new ros::Subscriber(nh_.subscribe(attitude_topic, 250, &LegOdometryPlugin::attitudeCallback, this)); + pub_ = new ros::Publisher(nh_.advertise(pub_topic, 250)); + + ROS_INFO_STREAM("LegOdometryPlugin reading ANYmal joint states from topic " << anymal_state_topic); + } + + void shutdown_() override { } + void pause_() override { } + void resume_() override { } + void reset_() override { } + +private: + void imuCallback(const sensor_msgs::Imu::ConstPtr& imu) + { + latest_imu_ = *imu; + has_imu_ = true; + } + + void anymalStateCallback(const state_estimator_msgs::AnymalState::ConstPtr& anymal_state) + { + latest_joint_state_ = jointSnapshotFromAnymalState(*anymal_state); + latest_joint_state_stamp_ = stampFromAnymalState(*anymal_state); + has_joint_state_ = true; + } + + void attitudeCallback(const state_estimator_msgs::attitude::ConstPtr& attitude) + { + latest_attitude_ = *attitude; + has_attitude_ = true; + } + + void contactCallback(const state_estimator_msgs::ContactDetection::ConstPtr& contact) + { + latest_contact_ = *contact; + has_contact_ = true; + publishLegOdometry(); + } + + void publishLegOdometry() + { + if (!model_configured_ || !has_imu_ || !has_joint_state_ || !has_contact_ || !has_attitude_) + { + ROS_WARN_THROTTLE( + 2.0, + "LegOdometryPlugin waiting for inputs: imu=%d anymal_state=%d contact=%d attitude=%d", + has_imu_, + has_joint_state_, + has_contact_, + has_attitude_ + ); + return; + } - void callback - ( - const sensor_msgs::Imu::ConstPtr& imu, - // const state_estimator_msgs::JointStateWithAcceleration::ConstPtr& js, - const sensor_msgs::JointState::ConstPtr& js, - const state_estimator_msgs::ContactDetection::ConstPtr& contact, - const state_estimator_msgs::attitude::ConstPtr& attitude - ) + const Eigen::Vector3d omega_imu( + latest_imu_.angular_velocity.x, + latest_imu_.angular_velocity.y, + latest_imu_.angular_velocity.z + ); + const Eigen::Vector3d omega_base = base_R_imu_ * omega_imu; + + Eigen::Quaterniond quat_est; + quat_est.w() = latest_attitude_.quaternion[0]; + quat_est.vec() << latest_attitude_.quaternion[1], latest_attitude_.quaternion[2], latest_attitude_.quaternion[3]; + if (quat_est.norm() > 1e-9) { - std::cout << "LegOdometryPlugin callback called" << std::endl; - - // Robot joint states - // Fill q and v from joint state - Eigen::VectorXd q(model_.nq); - Eigen::VectorXd v(model_.nv); - - if (js->position.size() != model_.nq || js->velocity.size() != model_.nv) { - ROS_WARN_THROTTLE(1.0, "Mismatch in joint state size"); - return; - } - - // Build map from joint name to position/velocity - std::unordered_map joint_pos_map, joint_vel_map; - for (size_t i = 0; i < js->name.size(); ++i) - { - joint_pos_map[js->name[i]] = js->position[i]; - if (i < js->velocity.size()) - joint_vel_map[js->name[i]] = js->velocity[i]; - } - - // Fill q and v in model's joint order (skip index 0 — "universe") - for (pinocchio::JointIndex i = 1; i < model_.njoints; ++i) - { - const std::string& joint_name = model_.names[i]; - - if (joint_pos_map.count(joint_name)) - q[i - 1] = joint_pos_map[joint_name]; - else - q[i - 1] = 0.0; // fallback - - if (joint_vel_map.count(joint_name)) - v[i - 1] = joint_vel_map[joint_name]; - else - v[i - 1] = 0.0; - } - - - omega << imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z; - stance_lf = contact->stance_lf; - stance_rf = contact->stance_rf; - stance_lh = contact->stance_lh; - stance_rh = contact->stance_rh; - - std::vector foot_velocities; - - // Compute the forward kinematics first - pinocchio::forwardKinematics(model_, data_, q, v); - pinocchio::updateFramePlacements(model_, data_); - std::vector foot_vels; - - for (size_t i = 0; i < feet_frame_names.size(); ++i) { - const auto& foot_name = feet_frame_names[i]; - std::size_t frame_id = model_.getFrameId(foot_name); - - // Get spatial velocity in LOCAL_WORLD_ALIGNED frame - pinocchio::Motion foot_vel_global = pinocchio::getFrameVelocity(model_, data_, frame_id, pinocchio::LOCAL_WORLD_ALIGNED); - - // Get position of the foot in the base frame - Eigen::Vector3d foot_pos_base = data_.oMf[frame_id].translation(); // Position of foot in world - Eigen::Vector3d omega_rotated = base_R_imu_ * omega; - - // Compute velocity contribution from base angular motion: ω x r - Eigen::Vector3d omega_cross_r = omega_rotated.cross(foot_pos_base); - - // Compute linear velocity of the foot relative to base - Eigen::Vector3d rel_vel = -(foot_vel_global.linear() - omega_cross_r); - foot_vels.push_back(rel_vel); - - } - - Eigen::Vector3d lin_leg_lf = foot_vels[0]; - Eigen::Vector3d lin_leg_rf = foot_vels[1]; - Eigen::Vector3d lin_leg_lh = foot_vels[2]; - Eigen::Vector3d lin_leg_rh = foot_vels[3]; - - double sum_stance = stance_lf + stance_rf + stance_lh + stance_rh; - Eigen::Vector3d base_velocity = (stance_lf*lin_leg_lf + stance_rf*lin_leg_rf + stance_lh*lin_leg_lh + stance_rh*lin_leg_rh)/(sum_stance + 1e-5); - - Eigen::Quaterniond quat_est; - quat_est.w() = attitude->quaternion[0]; - quat_est.vec() << attitude->quaternion[1], attitude->quaternion[2], attitude->quaternion[3]; - Eigen::Matrix3d w_R_b = iit::commons::quatToRotMat(quat_est).transpose(); - - base_velocity = w_R_b*base_velocity; - - // publishing - msg_.header.stamp = ros::Time::now(); - - for (int j=0;j<3;j++) { - msg_.lin_vel_lf[j] = lin_leg_lf.data()[j]; - msg_.lin_vel_rf[j] = lin_leg_rf.data()[j]; - msg_.lin_vel_lh[j] = lin_leg_lh.data()[j]; - msg_.lin_vel_rh[j] = lin_leg_rh.data()[j]; - msg_.base_velocity[j] = base_velocity.data()[j]; - } - - pub_->publish(msg_); - - } // end callback - - - private: - - message_filters::Subscriber *imu_sub_; - // message_filters::Subscriber *joint_state_sub_; - message_filters::Subscriber *joint_state_sub_; - message_filters::Subscriber *contact_sub_; - message_filters::Subscriber *attitude_sub; - message_filters::Synchronizer *sync_; - - ros::Publisher *pub_; - - state_estimator_msgs::LegOdometry msg_; - - pinocchio::Model model_; - pinocchio::Data data_; - std::vector feet_frame_names = {"LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"}; // Update with your actual link names - // std::vector feet_frame_names = {"FL_foot", "FR_foot", "RL_foot", "RR_foot"}; // Aliengo robot - - bool model_loaded_{false}; - - Eigen::Vector3d omega; - Eigen::Vector3d base_omega; - bool stance_lf; - bool stance_rf; - bool stance_lh; - bool stance_rh; - Eigen::Matrix3d base_R_imu_; - - }; // end class LegOdometryPlugin - -} //end namespace state_estimator_plugins + quat_est.normalize(); + } + else + { + quat_est.setIdentity(); + } + + const Eigen::Matrix3d w_R_b = iit::commons::quatToRotMat(quat_est).transpose(); + + const std::array stance{{ + static_cast(latest_contact_.stance_lf), + static_cast(latest_contact_.stance_rf), + static_cast(latest_contact_.stance_lh), + static_cast(latest_contact_.stance_rh) + }}; + + state_estimator::LegOdometryEstimate estimate; + if (!leg_odometry_.update(latest_joint_state_, omega_base, stance, w_R_b, estimate)) + { + ROS_WARN_THROTTLE(1.0, "LegOdometryPlugin update failed: %s", leg_odometry_.lastError().c_str()); + return; + } + + msg_.header.stamp = outputStamp(); + for (int j = 0; j < 3; ++j) + { + msg_.lin_vel_lf[j] = estimate.base_velocity_from_foot_base[0][j]; + msg_.lin_vel_rf[j] = estimate.base_velocity_from_foot_base[1][j]; + msg_.lin_vel_lh[j] = estimate.base_velocity_from_foot_base[2][j]; + msg_.lin_vel_rh[j] = estimate.base_velocity_from_foot_base[3][j]; + msg_.base_velocity[j] = estimate.base_velocity_world[j]; + } + msg_.stance_count = static_cast(estimate.stance_count); + msg_.base_height_valid = estimate.base_height_valid; + msg_.base_height = estimate.base_height; + for (std::size_t leg = 0; leg < stance.size(); ++leg) + { + msg_.stance[leg] = stance[leg]; + msg_.base_to_foot_position_world_z[leg] = + (w_R_b * estimate.kinematics.foot_position_base[leg]).z(); + } + + pub_->publish(msg_); + } + + ros::Time outputStamp() const + { + if (!latest_contact_.header.stamp.isZero()) return latest_contact_.header.stamp; + if (!latest_joint_state_stamp_.isZero()) return latest_joint_state_stamp_; + if (!latest_imu_.header.stamp.isZero()) return latest_imu_.header.stamp; + return ros::Time::now(); + } + + static ros::Time stampFromAnymalState(const state_estimator_msgs::AnymalState& anymal_state) + { + if (!anymal_state.header.stamp.isZero()) return anymal_state.header.stamp; + if (!anymal_state.joints.header.stamp.isZero()) return anymal_state.joints.header.stamp; + if (!anymal_state.pose.header.stamp.isZero()) return anymal_state.pose.header.stamp; + if (!anymal_state.twist.header.stamp.isZero()) return anymal_state.twist.header.stamp; + return ros::Time(); + } + + static state_estimator::JointStateSnapshot jointSnapshotFromAnymalState( + const state_estimator_msgs::AnymalState& anymal_state) + { + state_estimator::JointStateSnapshot joints; + joints.name = anymal_state.joints.name; + joints.position = anymal_state.joints.position; + joints.velocity = anymal_state.joints.velocity; + joints.acceleration = anymal_state.joints.acceleration; + joints.effort = anymal_state.joints.effort; + return joints; + } + + static std::string resolveRosFind(const std::string& input) + { + std::string resolved = input; + const std::string find_token = "$(find "; + const std::size_t token_pos = resolved.find(find_token); + if (token_pos == std::string::npos) + { + return resolved; + } + + const std::size_t package_start = token_pos + find_token.length(); + const std::size_t package_end = resolved.find(")", package_start); + if (package_end == std::string::npos) + { + return resolved; + } + + const std::string package_name = resolved.substr(package_start, package_end - package_start); + const std::string package_path = ros::package::getPath(package_name); + if (package_path.empty()) + { + return resolved; + } + + resolved.replace(token_pos, package_end - token_pos + 1, package_path); + return resolved; + } + + static std::vector defaultFootFrameNames() + { + return {"LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"}; + } + + static std::vector defaultJointNames() + { + return { + "LF_HAA", "LF_HFE", "LF_KFE", + "RF_HAA", "RF_HFE", "RF_KFE", + "LH_HAA", "LH_HFE", "LH_KFE", + "RH_HAA", "RH_HFE", "RH_KFE" + }; + } + + static std::vector defaultBaseRImuVector() + { + return { + -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 + }; + } + + ros::Subscriber *imu_sub_; + ros::Subscriber *anymal_state_sub_; + ros::Subscriber *contact_sub_; + ros::Subscriber *attitude_sub_; + ros::Publisher *pub_; + + state_estimator_msgs::LegOdometry msg_; + sensor_msgs::Imu latest_imu_; + state_estimator::JointStateSnapshot latest_joint_state_; + ros::Time latest_joint_state_stamp_; + state_estimator_msgs::ContactDetection latest_contact_; + state_estimator_msgs::attitude latest_attitude_; + + state_estimator::LegOdometryModel leg_odometry_; + + bool model_configured_{false}; + bool has_imu_{false}; + bool has_joint_state_{false}; + bool has_contact_{false}; + bool has_attitude_{false}; + Eigen::Matrix3d base_R_imu_{Eigen::Matrix3d::Identity()}; +}; + +} // namespace state_estimator_plugins #include PLUGINLIB_EXPORT_CLASS(state_estimator_plugins::LegOdometryPlugin, state_estimator_plugins::PluginBase) diff --git a/muse_ws/src/state_estimator/src/plugins/sensor_fusion_plugin.cpp b/muse_ws/src/state_estimator/src/plugins/sensor_fusion_plugin.cpp index 9557339..93977e9 100644 --- a/muse_ws/src/state_estimator/src/plugins/sensor_fusion_plugin.cpp +++ b/muse_ws/src/state_estimator/src/plugins/sensor_fusion_plugin.cpp @@ -1,234 +1,403 @@ #include "state_estimator/Models/sensor_fusion.hpp" #include "state_estimator/plugin.hpp" -#include -#include -#include - -#include // read acceleration from imu -#include "state_estimator_msgs/LegOdometry.h" // read base velocity from leg odometry -#include "state_estimator_msgs/attitude.h" // read orientation from attitude estimation -#include // read position from lidar odometry +#include "state_estimator_msgs/LegOdometry.h" +#include "state_estimator_msgs/attitude.h" #include +#include +#include +#include + +#include +#include +#include +#include + namespace state_estimator_plugins { -typedef message_filters::sync_policies::ApproximateTime -< - sensor_msgs::Imu, - state_estimator_msgs::attitude, - state_estimator_msgs::LegOdometry -> -ApproximateTimePolicy; - -typedef message_filters::sync_policies::ExactTime -< - sensor_msgs::Imu, - state_estimator_msgs::attitude, - state_estimator_msgs::LegOdometry -> -ExactTimePolicy; - -#define MySyncPolicy ApproximateTimePolicy - - class SensorFusionPlugin : public PluginBase +class SensorFusionPlugin : public PluginBase +{ +public: + SensorFusionPlugin(): + sensor_fusion_(nullptr), + imu_sub_(nullptr), + attitude_sub_(nullptr), + leg_odom_sub_(nullptr), + pub_(nullptr) + { } + + ~SensorFusionPlugin() { - public: - SensorFusionPlugin(): - sensor_fusion_(nullptr), - imu_sub_(nullptr), - attitude_sub_(nullptr), - leg_odom_sub_(nullptr), - pub_(nullptr), - sync_(nullptr) - { } + if (sensor_fusion_!=nullptr) delete(sensor_fusion_); + if (imu_sub_!=nullptr) delete(imu_sub_); + if (attitude_sub_!=nullptr) delete(attitude_sub_); + if (leg_odom_sub_!=nullptr) delete(leg_odom_sub_); + if (pub_!=nullptr) delete(pub_); + } - ~SensorFusionPlugin() - { - if (sensor_fusion_!=nullptr) delete(sensor_fusion_); - if (imu_sub_!=nullptr) delete(imu_sub_); - if (attitude_sub_!=nullptr) delete(attitude_sub_); - if (leg_odom_sub_!=nullptr) delete(leg_odom_sub_); - if (pub_!=nullptr) delete(pub_); - if (sync_!=nullptr) delete(sync_); - } + std::string getName() override { return std::string("SensorFusion"); } + std::string getDescription() override { return std::string("Sensor Fusion Plugin"); } - std::string getName() override { return std::string("SensorFusion"); } - std::string getDescription() override { return std::string("Sensor Fusion Plugin"); } - - void initialize_() override - { - t0 = 0.0; - xhat_estimated << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + void initialize_() override + { + t0 = 0.0; + xhat_estimated << 0.0, 0.0, 0.45, 0.0, 0.0, 0.0; - std::vector P_vec, Q_vec, R_vec; - nh_.param("sensor_fusion_plugin/P", P_vec, std::vector(36, 0.0)); - nh_.param("sensor_fusion_plugin/Q", Q_vec, std::vector(36, 0.0)); - nh_.param("sensor_fusion_plugin/R", R_vec, std::vector(9, 0.0)); + std::vector P_vec, Q_vec, R_vec; + nh_.param("sensor_fusion_plugin/P", P_vec, std::vector(36, 0.0)); + nh_.param("sensor_fusion_plugin/Q", Q_vec, std::vector(36, 0.0)); + nh_.param("sensor_fusion_plugin/R", R_vec, std::vector(9, 0.0)); - if (P_vec.size() != 36 || Q_vec.size() != 36 || R_vec.size() != 9) { - ROS_ERROR("P0, Q0, or R0 parameters have incorrect size. Each should have 36 elements."); - return; - } + if (P_vec.size() != 36 || Q_vec.size() != 36 || R_vec.size() != 9) + { + ROS_ERROR("P0, Q0, or R0 parameters have incorrect size. P and Q need 36 elements, R needs 9."); + return; + } - P = Eigen::Map>(P_vec.data()); - Q = Eigen::Map>(Q_vec.data()); - R = Eigen::Map>(R_vec.data()); + P = Eigen::Map>(P_vec.data()); + Q = Eigen::Map>(Q_vec.data()); + R = Eigen::Map>(R_vec.data()); - sensor_fusion_ = new state_estimator::KFSensorFusion(t0, xhat_estimated, P, Q, R, false, false); + sensor_fusion_ = new state_estimator::KFSensorFusion(t0, xhat_estimated, P, Q, R, false, false); - std::vector base_R_imu_vec; - nh_.param("attitude_estimation_plugin/base_R_imu", base_R_imu_vec, std::vector(9, 0.0)); + std::vector base_R_imu_vec; + nh_.param("attitude_estimation_plugin/base_R_imu", base_R_imu_vec, std::vector(9, 0.0)); + if (base_R_imu_vec.size() == 9) + { + base_R_imu_ = Eigen::Map>(base_R_imu_vec.data()); + } + else + { + ROS_WARN("base_R_imu is not the correct size (should be 9). Using identity."); + base_R_imu_ = Eigen::Matrix3d::Identity(); + } - if (base_R_imu_vec.size() == 9) { - base_R_imu_ = Eigen::Map>(base_R_imu_vec.data()); - // ROS_INFO_STREAM("Loaded base_R_imu:\n" << base_R_imu_); - } else { - ROS_WARN("base_R_imu is not the correct size (should be 9). Using identity."); - base_R_imu_ = Eigen::Matrix3d::Identity(); - } + std::string imu_topic, attitude_topic, leg_odom_topic, pub_topic; + nh_.param("sensor_fusion_plugin/imu_topic", imu_topic, "/anymal/imu"); + nh_.param("sensor_fusion_plugin/attitude_topic", attitude_topic, "/state_estimator/attitude"); + nh_.param("sensor_fusion_plugin/leg_odometry_topic", leg_odom_topic, "/state_estimator/leg_odometry"); + nh_.param("sensor_fusion_plugin/pub_topic", pub_topic, "sensor_fusion"); + nh_.param("sensor_fusion_plugin/frame_id", frame_id_, "world"); + nh_.param("sensor_fusion_plugin/child_frame_id", child_frame_id_, "base"); + nh_.param("sensor_fusion_plugin/z_height_correction/enabled", z_height_correction_enabled_, true); + nh_.param("sensor_fusion_plugin/z_height_correction/assume_flat_ground", z_height_assume_flat_ground_, false); + nh_.param("sensor_fusion_plugin/z_height_correction/ground_height", z_height_ground_height_, 0.0); + nh_.param("sensor_fusion_plugin/z_height_correction/measurement_variance", z_height_measurement_variance_, 1.0e-8); + nh_.param("sensor_fusion_plugin/z_height_correction/max_innovation", z_height_max_innovation_, 0.25); + nh_.param("sensor_fusion_plugin/z_height_correction/filter_alpha", z_height_filter_alpha_, 1.0); + + int min_stance_legs = 1; + nh_.param("sensor_fusion_plugin/z_height_correction/min_stance_legs", min_stance_legs, 1); + z_height_min_stance_legs_ = std::max(1, std::min(4, min_stance_legs)); + + imu_sub_ = new ros::Subscriber(nh_.subscribe(imu_topic, 250, &SensorFusionPlugin::imuCallback, this)); + attitude_sub_ = new ros::Subscriber(nh_.subscribe(attitude_topic, 250, &SensorFusionPlugin::attitudeCallback, this)); + leg_odom_sub_ = new ros::Subscriber(nh_.subscribe(leg_odom_topic, 250, &SensorFusionPlugin::legOdometryCallback, this)); + pub_ = new ros::Publisher(nh_.advertise(pub_topic, 250)); + + ROS_INFO_STREAM( + "SensorFusionPlugin initialized with imu=" << imu_topic + << ", attitude=" << attitude_topic + << ", leg_odometry=" << leg_odom_topic + ); + } + + void shutdown_() override { } + void pause_() override { } + void resume_() override { } + void reset_() override { } + +private: + void imuCallback(const sensor_msgs::Imu::ConstPtr& imu) + { + latest_imu_ = *imu; + has_imu_ = true; + } - std::string imu_topic, attitude_topic, leg_odom_topic, pub_topic; - nh_.param("sensor_fusion_plugin/imu_topic", imu_topic, "/sensors/imu"); - nh_.param("sensor_fusion_plugin/attitude_topic", attitude_topic, "/state_estimator/attitude"); - nh_.param("sensor_fusion_plugin/leg_odometry_topic", leg_odom_topic, "/state_estimator/leg_odometry"); - nh_.param("sensor_fusion_plugin/pub_topic", pub_topic, "sensor_fusion"); + void attitudeCallback(const state_estimator_msgs::attitude::ConstPtr& attitude) + { + latest_attitude_ = *attitude; + has_attitude_ = true; + } - imu_sub_ = new message_filters::Subscriber(nh_, imu_topic, 250); - attitude_sub_ = new message_filters::Subscriber(nh_, attitude_topic, 250); - leg_odom_sub_ = new message_filters::Subscriber(nh_, leg_odom_topic, 250); + void legOdometryCallback(const state_estimator_msgs::LegOdometry::ConstPtr& leg_odom) + { + latest_leg_odom_ = *leg_odom; + has_leg_odom_ = true; + callback_proprioception(); + } - sync_ = new message_filters::Synchronizer(MySyncPolicy(100), *imu_sub_, *attitude_sub_, *leg_odom_sub_); - sync_->registerCallback(boost::bind(&SensorFusionPlugin::callback_proprioception, this, _1, _2, _3)); + void callback_proprioception() + { + if (!has_imu_ || !has_attitude_ || !has_leg_odom_) + { + ROS_WARN_THROTTLE( + 2.0, + "SensorFusionPlugin waiting for inputs: imu=%d attitude=%d leg_odometry=%d", + has_imu_, + has_attitude_, + has_leg_odom_ + ); + return; + } - pub_ = new ros::Publisher(nh_.advertise(pub_topic, 250)); + ROS_DEBUG_STREAM_THROTTLE( + 1.0, + "SensorFusionPlugin received inputs: imu timestamp = " << latest_imu_.header.stamp + << ", attitude timestamp = " << latest_attitude_.header.stamp + << ", leg odometry timestamp = " << latest_leg_odom_.header.stamp + ); + + Eigen::Vector3d acc( + latest_imu_.linear_acceleration.x, + latest_imu_.linear_acceleration.y, + latest_imu_.linear_acceleration.z + ); + + omega << latest_attitude_.angular_velocity[0], latest_attitude_.angular_velocity[1], latest_attitude_.angular_velocity[2]; + quat_est.w() = latest_attitude_.quaternion[0]; + quat_est.vec() << latest_attitude_.quaternion[1], latest_attitude_.quaternion[2], latest_attitude_.quaternion[3]; + rpy = iit::commons::quatToRPY(quat_est); + w_R_b = iit::commons::quatToRotMat(quat_est).transpose(); + + v_b << latest_leg_odom_.base_velocity[0], latest_leg_odom_.base_velocity[1], latest_leg_odom_.base_velocity[2]; + computeLinPosVel(acc, w_R_b, v_b, outputStamp()); + } + + void computeLinPosVel(Eigen::Vector3d& acc, Eigen::Matrix3d& w_R_b, Eigen::Vector3d& v_b, const ros::Time& stamp) + { + const ros::Time effective_stamp = stamp.isZero() ? ros::Time::now() : stamp; + if (begin) + { + time_begin_ = effective_stamp.toSec(); + last_update_time_ = 0.0; + w_R_wb = w_R_b; + begin = false; + } - ROS_INFO("SensorFusionPlugin initialized"); + time_ = effective_stamp.toSec() - time_begin_; + if (time_ < last_update_time_) + { + ROS_WARN_THROTTLE(1.0, "SensorFusionPlugin received out-of-order timestamps. Skipping update."); + return; } - void shutdown_() override { } - void pause_() override { } - void resume_() override { } - void reset_() override { } + Eigen::Vector3d f_b = base_R_imu_ * acc; + Eigen::Vector3d gravity; + gravity << 0.0, 0.0, -9.81; + Eigen::Vector3d u = w_R_b * f_b + gravity; - void callback_proprioception - ( - const sensor_msgs::Imu::ConstPtr& imu, - const state_estimator_msgs::attitude::ConstPtr& attitude, - const state_estimator_msgs::LegOdometry::ConstPtr& leg_odom - ) - { - // Reading imu - Eigen::Vector3d acc(imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z); + sensor_fusion_->predict(time_, u); - // Reading attitude estimation - omega << attitude->angular_velocity[0], attitude->angular_velocity[1], attitude->angular_velocity[2]; - quat_est.w() = attitude->quaternion[0]; - quat_est.vec() << attitude->quaternion[1], attitude->quaternion[2], attitude->quaternion[3]; - rpy = iit::commons::quatToRPY(quat_est); - w_R_b = iit::commons::quatToRotMat(quat_est).transpose(); + Eigen::Vector3d z_proprio; + z_proprio << v_b; + sensor_fusion_->update(time_, z_proprio); + applyZHeightCorrection(); + xhat_estimated = sensor_fusion_->getX(); + last_update_time_ = time_; - // Reading leg odometry - v_b << leg_odom->base_velocity[0], leg_odom->base_velocity[1], leg_odom->base_velocity[2]; - computeLinPosVel(acc,w_R_b,v_b); - } + msg_.header.stamp = effective_stamp; + msg_.header.frame_id = frame_id_; + msg_.child_frame_id = child_frame_id_; - void computeLinPosVel(Eigen::Vector3d &acc, Eigen::Matrix3d &w_R_b, Eigen::Vector3d &v_b) - { - if (begin) - { - time_begin_ = ros::Time::now().toSec(); - w_R_wb = w_R_b; - begin = false; - } + msg_.pose.pose.orientation.w = quat_est.w(); + msg_.pose.pose.orientation.x = quat_est.x(); + msg_.pose.pose.orientation.y = quat_est.y(); + msg_.pose.pose.orientation.z = quat_est.z(); - time_ = ros::Time::now().toSec() - time_begin_; + msg_.pose.pose.position.x = xhat_estimated(0); + msg_.pose.pose.position.y = xhat_estimated(1); + msg_.pose.pose.position.z = xhat_estimated(2); - // reading acceleration from imu - Eigen::Vector3d f_b = base_R_imu_*acc; + msg_.twist.twist.linear.x = xhat_estimated(3); + msg_.twist.twist.linear.y = xhat_estimated(4); + msg_.twist.twist.linear.z = xhat_estimated(5); - // input u = w_R_b*f_b - gravity - Eigen::Vector3d gravity; gravity << 0.0, 0.0, -9.81; - Eigen::Vector3d u = w_R_b*f_b + gravity; - // prediction - sensor_fusion_->predict(time_,u); + msg_.twist.twist.angular.x = omega[0]; + msg_.twist.twist.angular.y = omega[1]; + msg_.twist.twist.angular.z = omega[2]; - // reading leg odometry - Eigen::Vector3d w_v_b = w_R_b*v_b; + pub_->publish(msg_); + } - Eigen::Vector3d z_proprio; - z_proprio << w_v_b; + void applyZHeightCorrection() + { + if (!z_height_correction_enabled_) + { + return; + } - // // correction - sensor_fusion_->update(time_,z_proprio); - xhat_estimated = sensor_fusion_->getX(); + if (z_height_assume_flat_ground_) + { + applyFlatGroundZHeightCorrection(); + return; + } - // publish - msg_.header.stamp = ros::Time::now(); - - msg_.pose.pose.orientation.w = quat_est.w(); - msg_.pose.pose.orientation.x = quat_est.x(); - msg_.pose.pose.orientation.y = quat_est.y(); - msg_.pose.pose.orientation.z = quat_est.z(); + applyContactAnchorZCorrection(); + } - msg_.pose.pose.position.x = xhat_estimated(0); - msg_.pose.pose.position.y = xhat_estimated(1); - msg_.pose.pose.position.z = xhat_estimated(2); + void applyFlatGroundZHeightCorrection() + { + if (!latest_leg_odom_.base_height_valid) + { + return; + } + if (latest_leg_odom_.stance_count < z_height_min_stance_legs_) + { + return; + } - msg_.twist.twist.linear.x = xhat_estimated(3); - msg_.twist.twist.linear.y = xhat_estimated(4); - msg_.twist.twist.linear.z = xhat_estimated(5); + const double raw_measurement = latest_leg_odom_.base_height + z_height_ground_height_; + if (!std::isfinite(raw_measurement)) + { + return; + } - msg_.twist.twist.angular.x = omega[0]; - msg_.twist.twist.angular.y = omega[1]; - msg_.twist.twist.angular.z = omega[2]; + applyBoundedZMeasurement(raw_measurement); + } - pub_->publish(msg_); + void applyContactAnchorZCorrection() + { + const Eigen::Matrix xhat = sensor_fusion_->getX(); + const double current_base_z = xhat(2); - } // end computeLinPosVel + double z_measurement_sum = 0.0; + std::size_t valid_measurements = 0; + for (std::size_t leg = 0; leg < foot_anchor_world_z_.size(); ++leg) + { + const bool stance = static_cast(latest_leg_odom_.stance[leg]); + const double base_to_foot_z = latest_leg_odom_.base_to_foot_position_world_z[leg]; - private: + if (!stance) + { + foot_anchor_valid_[leg] = false; + previous_foot_stance_[leg] = false; + continue; + } + if (!std::isfinite(base_to_foot_z)) + { + continue; + } - state_estimator::KFSensorFusion *sensor_fusion_; + if (!previous_foot_stance_[leg] || !foot_anchor_valid_[leg]) + { + foot_anchor_world_z_[leg] = current_base_z + base_to_foot_z; + foot_anchor_valid_[leg] = true; + } - double t0; - Eigen::Matrix xhat_estimated; - Eigen::Matrix P; - Eigen::Matrix Q; - Eigen::Matrix R; + z_measurement_sum += foot_anchor_world_z_[leg] - base_to_foot_z; + valid_measurements++; + previous_foot_stance_[leg] = true; + } - message_filters::Subscriber *imu_sub_; - message_filters::Subscriber *attitude_sub_; - message_filters::Subscriber *leg_odom_sub_; - - message_filters::Synchronizer *sync_; + if (valid_measurements < static_cast(z_height_min_stance_legs_)) + { + return; + } - ros::Publisher *pub_; + applyBoundedZMeasurement(z_measurement_sum / static_cast(valid_measurements)); + } - nav_msgs::Odometry msg_; + void applyBoundedZMeasurement(double raw_measurement) + { + if (z_height_measurement_variance_ <= 0.0) + { + ROS_WARN_THROTTLE(2.0, "SensorFusionPlugin z height correction disabled by non-positive measurement variance."); + return; + } + if (!std::isfinite(raw_measurement)) + { + return; + } - double time_{}; - bool begin{true}; - double time_begin_; - Eigen::Vector3d v_b; - Eigen::Quaterniond quat_est; - Eigen::Vector3d omega; - double yaw; - Eigen::Vector3d rpy; - Eigen::Matrix3d w_R_b; - Eigen::Matrix3d base_R_imu_; + const double alpha = std::max(0.0, std::min(1.0, z_height_filter_alpha_)); + if (!has_filtered_z_height_) + { + filtered_z_height_ = raw_measurement; + has_filtered_z_height_ = true; + } + else + { + filtered_z_height_ = alpha * raw_measurement + (1.0 - alpha) * filtered_z_height_; + } - Eigen::Matrix3d w_R_wb; // world frame + const double innovation = filtered_z_height_ - sensor_fusion_->getX()(2); + if (z_height_max_innovation_ > 0.0 && std::abs(innovation) > z_height_max_innovation_) + { + filtered_z_height_ = sensor_fusion_->getX()(2) + std::copysign(z_height_max_innovation_, innovation); + ROS_WARN_THROTTLE( + 1.0, + "SensorFusionPlugin clamped z height correction: innovation %.3f exceeds gate %.3f", + innovation, + z_height_max_innovation_ + ); + } - }; // end class SensorFusionPlugin + sensor_fusion_->updateZPosition(time_, filtered_z_height_, z_height_measurement_variance_); + } -} // end namespace state_estimator_plugins + ros::Time outputStamp() const + { + if (!latest_leg_odom_.header.stamp.isZero()) return latest_leg_odom_.header.stamp; + if (!latest_attitude_.header.stamp.isZero()) return latest_attitude_.header.stamp; + if (!latest_imu_.header.stamp.isZero()) return latest_imu_.header.stamp; + return ros::Time::now(); + } + + state_estimator::KFSensorFusion *sensor_fusion_; + + double t0; + Eigen::Matrix xhat_estimated; + Eigen::Matrix P; + Eigen::Matrix Q; + Eigen::Matrix R; + + ros::Subscriber *imu_sub_; + ros::Subscriber *attitude_sub_; + ros::Subscriber *leg_odom_sub_; + ros::Publisher *pub_; + + sensor_msgs::Imu latest_imu_; + state_estimator_msgs::attitude latest_attitude_; + state_estimator_msgs::LegOdometry latest_leg_odom_; + nav_msgs::Odometry msg_; + std::string frame_id_; + std::string child_frame_id_; + + bool has_imu_{false}; + bool has_attitude_{false}; + bool has_leg_odom_{false}; + double time_{}; + double last_update_time_{0.0}; + bool begin{true}; + double time_begin_{0.0}; + Eigen::Vector3d v_b; + Eigen::Quaterniond quat_est; + Eigen::Vector3d omega; + Eigen::Vector3d rpy; + Eigen::Matrix3d w_R_b; + Eigen::Matrix3d base_R_imu_; + + Eigen::Matrix3d w_R_wb; + bool z_height_correction_enabled_{true}; + bool z_height_assume_flat_ground_{false}; + int z_height_min_stance_legs_{1}; + double z_height_ground_height_{0.0}; + double z_height_measurement_variance_{1.0e-8}; + double z_height_max_innovation_{0.25}; + double z_height_filter_alpha_{1.0}; + bool has_filtered_z_height_{false}; + double filtered_z_height_{0.0}; + std::array foot_anchor_valid_{{false, false, false, false}}; + std::array previous_foot_stance_{{false, false, false, false}}; + std::array foot_anchor_world_z_{{0.0, 0.0, 0.0, 0.0}}; +}; + +} // namespace state_estimator_plugins #include PLUGINLIB_EXPORT_CLASS(state_estimator_plugins::SensorFusionPlugin, state_estimator_plugins::PluginBase) - - - diff --git a/muse_ws/src/state_estimator/src/plugins/tf_publisher_plugin.cpp b/muse_ws/src/state_estimator/src/plugins/tf_publisher_plugin.cpp new file mode 100644 index 0000000..02a6d05 --- /dev/null +++ b/muse_ws/src/state_estimator/src/plugins/tf_publisher_plugin.cpp @@ -0,0 +1,92 @@ +#include "state_estimator/plugin.hpp" + +#include +#include +#include +#include + +#include + +namespace state_estimator_plugins +{ + + class TfPublisherPlugin : public PluginBase + { + public: + TfPublisherPlugin(): + odom_sub_(nullptr), + tf_broadcaster_(nullptr) + { } + + ~TfPublisherPlugin() + { + if (odom_sub_!=nullptr) delete(odom_sub_); + if (tf_broadcaster_!=nullptr) delete(tf_broadcaster_); + } + + std::string getName() override { return std::string("TfPublisher"); } + std::string getDescription() override { return std::string("TF Publisher Plugin"); } + + void initialize_() override + { + nh_.param("tf_publisher_plugin/odom_topic", odom_topic_, "/state_estimator/sensor_fusion"); + nh_.param("tf_publisher_plugin/parent_frame", parent_frame_, "world"); + nh_.param("tf_publisher_plugin/child_frame", child_frame_, "base"); + nh_.param("tf_publisher_plugin/use_odom_frame_ids", use_odom_frame_ids_, false); + + tf_broadcaster_ = new tf2_ros::TransformBroadcaster(); + odom_sub_ = new ros::Subscriber(nh_.subscribe(odom_topic_, 250, &TfPublisherPlugin::callback, this)); + + ROS_INFO_STREAM( + "TfPublisherPlugin initialized: broadcasting " + << parent_frame_ << " -> " << child_frame_ + << " from " << odom_topic_ + ); + } + + void shutdown_() override + { + if (odom_sub_!=nullptr) odom_sub_->shutdown(); + } + + void pause_() override { } + void resume_() override { } + void reset_() override { } + + private: + void callback(const nav_msgs::Odometry::ConstPtr& odom) + { + if (isPaused()) return; + + geometry_msgs::TransformStamped transform; + transform.header.stamp = odom->header.stamp.isZero() ? ros::Time::now() : odom->header.stamp; + transform.header.frame_id = parent_frame_; + transform.child_frame_id = child_frame_; + + if (use_odom_frame_ids_) + { + if (!odom->header.frame_id.empty()) transform.header.frame_id = odom->header.frame_id; + if (!odom->child_frame_id.empty()) transform.child_frame_id = odom->child_frame_id; + } + + transform.transform.translation.x = odom->pose.pose.position.x; + transform.transform.translation.y = odom->pose.pose.position.y; + transform.transform.translation.z = odom->pose.pose.position.z; + transform.transform.rotation = odom->pose.pose.orientation; + + tf_broadcaster_->sendTransform(transform); + } + + ros::Subscriber *odom_sub_; + tf2_ros::TransformBroadcaster *tf_broadcaster_; + + std::string odom_topic_; + std::string parent_frame_; + std::string child_frame_; + bool use_odom_frame_ids_{false}; + }; + +} // end namespace state_estimator_plugins + +#include +PLUGINLIB_EXPORT_CLASS(state_estimator_plugins::TfPublisherPlugin, state_estimator_plugins::PluginBase) diff --git a/muse_ws/src/state_estimator/state_estimator_plugins.xml b/muse_ws/src/state_estimator/state_estimator_plugins.xml index 15c1e20..e878445 100644 --- a/muse_ws/src/state_estimator/state_estimator_plugins.xml +++ b/muse_ws/src/state_estimator/state_estimator_plugins.xml @@ -16,5 +16,9 @@ A plugin to fuse the estimated attitude and the leg odometry + + A plugin to publish TF from the fused odometry output + + diff --git a/muse_ws/src/state_estimator/urdfs/anymal.urdf b/muse_ws/src/state_estimator/urdfs/anymal.urdf index 42819d8..c2af19c 100644 --- a/muse_ws/src/state_estimator/urdfs/anymal.urdf +++ b/muse_ws/src/state_estimator/urdfs/anymal.urdf @@ -1,730 +1,1531 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 100.0 - 0.8 - 0.8 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 100.0 - 0.8 - 0.8 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 100.0 - 0.8 - 0.8 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 100.0 - 0.8 - 0.8 - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + This accounts for the fact that the Lidar driver rotates the measurements 90 degrees in yaw --> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/muse_ws/src/state_estimator_msgs/CMakeLists.txt b/muse_ws/src/state_estimator_msgs/CMakeLists.txt index 56a10c7..c4a7d1b 100644 --- a/muse_ws/src/state_estimator_msgs/CMakeLists.txt +++ b/muse_ws/src/state_estimator_msgs/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10) project(state_estimator_msgs) find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs std_msgs message_generation) @@ -8,7 +8,7 @@ include_directories(include) add_message_files( DIRECTORY msg FILES - attitude.msg + attitude.msg ContactDetection.msg LegOdometry.msg sensor_fusion.msg diff --git a/muse_ws/src/state_estimator_msgs/msg/LegOdometry.msg b/muse_ws/src/state_estimator_msgs/msg/LegOdometry.msg index aa32aed..2a7786c 100644 --- a/muse_ws/src/state_estimator_msgs/msg/LegOdometry.msg +++ b/muse_ws/src/state_estimator_msgs/msg/LegOdometry.msg @@ -5,4 +5,10 @@ float64[3] lin_vel_rf float64[3] lin_vel_lh float64[3] lin_vel_rh -float64[3] base_velocity \ No newline at end of file +float64[3] base_velocity + +uint8 stance_count +bool base_height_valid +float64 base_height +bool[4] stance +float64[4] base_to_foot_position_world_z