From 34814acf3ec98247b0fded30bc0fb701c47bd6f7 Mon Sep 17 00:00:00 2001
From: robofish <1683502971@qq.com>
Date: Sun, 26 Jan 2025 04:02:54 +0800
Subject: [PATCH] =?UTF-8?q?imu=E6=AD=A3=E5=B8=B8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../profiles/default/devel_collisions.txt | 2 +-
.../legged_control/devel_manifest.txt | 2 -
.../packages/legged_control/package.xml | 20 -----
.../packages/ocs2_cartpole/devel_manifest.txt | 6 --
.../packages/ocs2_cartpole/package.xml | 23 ------
.../packages/ocs2_frank_wolfe/package.xml | 57 --------------
.../ocs2_perceptive/devel_manifest.txt | 7 --
.../packages/ocs2_perceptive/package.xml | 16 ----
.../ocs2_python_interface/devel_manifest.txt | 6 --
.../ocs2_python_interface/package.xml | 22 ------
.../packages/ocs2_raisim_core/package.xml | 22 ------
.../packages/ocs2_slp/devel_manifest.txt | 7 --
.../default/packages/ocs2_slp/package.xml | 20 -----
.../devel_manifest.txt | 44 -----------
.../ocs2_switched_model_msgs/package.xml | 19 -----
.catkin_workspace | 1 -
.vscode/settings.json | 74 -------------------
README.md | 8 +-
contactThreshold_ | 0
run.sh | 4 +
src/fdilink_ahrs/launch/ahrs_data.launch | 2 +-
.../include/legged_unitree_hw/UnitreeHW.h | 74 ++++++++++---------
.../legged_unitree_hw/src/UnitreeHW.cpp | 54 ++++++++++++--
.../src/legged_unitree_hw.cpp | 56 ++++++--------
24 files changed, 122 insertions(+), 424 deletions(-)
delete mode 100644 .catkin_tools/profiles/default/packages/legged_control/devel_manifest.txt
delete mode 100644 .catkin_tools/profiles/default/packages/legged_control/package.xml
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_cartpole/devel_manifest.txt
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_cartpole/package.xml
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_frank_wolfe/package.xml
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_perceptive/devel_manifest.txt
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_perceptive/package.xml
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_python_interface/devel_manifest.txt
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_python_interface/package.xml
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_raisim_core/package.xml
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_slp/devel_manifest.txt
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_slp/package.xml
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/devel_manifest.txt
delete mode 100644 .catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/package.xml
delete mode 100644 .catkin_workspace
delete mode 100644 .vscode/settings.json
create mode 100644 contactThreshold_
create mode 100644 run.sh
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index ec4e143..c290d7d 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1 +1 @@
-/home/robofish/CM_DOG/devel/./cmake.lock 1618
+/home/robofish/CM_DOG/devel/./cmake.lock 843
diff --git a/.catkin_tools/profiles/default/packages/legged_control/devel_manifest.txt b/.catkin_tools/profiles/default/packages/legged_control/devel_manifest.txt
deleted file mode 100644
index 04feea4..0000000
--- a/.catkin_tools/profiles/default/packages/legged_control/devel_manifest.txt
+++ /dev/null
@@ -1,2 +0,0 @@
-legged_control/legged_control
-/home/robofish/CM_DOG/devel/.private/legged_control/cmake.lock /home/robofish/CM_DOG/devel/./cmake.lock
diff --git a/.catkin_tools/profiles/default/packages/legged_control/package.xml b/.catkin_tools/profiles/default/packages/legged_control/package.xml
deleted file mode 100644
index ef5b06a..0000000
--- a/.catkin_tools/profiles/default/packages/legged_control/package.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>legged_control</name>
- <version>0.0.0</version>
- <description>Meta package that contains package of legged.</description>
- <maintainer email="liaoqiayuan@gmail.com">Qiayuan Liao</maintainer>
-
- <license>BSD</license>
-
- <url type="repository">https://github.com/qiayuanliao/legged_control</url>
- <url type="bugtracker">https://github.com/qiayuanliao/legged_control/issues</url>
-
- <author email="liaoqiayuan@gmail.com">Qiayuan Liao</author>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <export>
- <metapackage/>
- </export>
-</package>
diff --git a/.catkin_tools/profiles/default/packages/ocs2_cartpole/devel_manifest.txt b/.catkin_tools/profiles/default/packages/ocs2_cartpole/devel_manifest.txt
deleted file mode 100644
index 315cafa..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_cartpole/devel_manifest.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-ocs2/ocs2_robotic_examples/ocs2_cartpole
-/home/robofish/CM_DOG/devel/.private/ocs2_cartpole/cmake.lock /home/robofish/CM_DOG/devel/./cmake.lock
-/home/robofish/CM_DOG/devel/.private/ocs2_cartpole/share/ocs2_cartpole/cmake/ocs2_cartpoleConfig.cmake /home/robofish/CM_DOG/devel/share/ocs2_cartpole/cmake/ocs2_cartpoleConfig.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_cartpole/share/ocs2_cartpole/cmake/ocs2_cartpoleConfig-version.cmake /home/robofish/CM_DOG/devel/share/ocs2_cartpole/cmake/ocs2_cartpoleConfig-version.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_cartpole/lib/libocs2_cartpole.so /home/robofish/CM_DOG/devel/lib/libocs2_cartpole.so
-/home/robofish/CM_DOG/devel/.private/ocs2_cartpole/lib/pkgconfig/ocs2_cartpole.pc /home/robofish/CM_DOG/devel/lib/pkgconfig/ocs2_cartpole.pc
diff --git a/.catkin_tools/profiles/default/packages/ocs2_cartpole/package.xml b/.catkin_tools/profiles/default/packages/ocs2_cartpole/package.xml
deleted file mode 100644
index 09a2d96..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_cartpole/package.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>ocs2_cartpole</name>
- <version>0.0.0</version>
- <description>The ocs2_cartpole package</description>
-
- <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
- <maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
- <maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>
-
- <license>TODO</license>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <build_depend>roslib </build_depend>
- <build_depend>cmake_clang_tools</build_depend>
-
- <depend>ocs2_core</depend>
- <depend>ocs2_ddp</depend>
- <depend>ocs2_mpc</depend>
- <depend>ocs2_robotic_tools</depend>
-
-</package>
diff --git a/.catkin_tools/profiles/default/packages/ocs2_frank_wolfe/package.xml b/.catkin_tools/profiles/default/packages/ocs2_frank_wolfe/package.xml
deleted file mode 100644
index cf9c0a1..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_frank_wolfe/package.xml
+++ /dev/null
@@ -1,57 +0,0 @@
-<?xml version="1.0"?>
-<package>
- <name>ocs2_frank_wolfe</name>
- <version>0.0.0</version>
- <description>The OCS2 Frank Wolfe package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
- <maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
- <maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>TODO</license>
-
-
- <!-- Url tags are optional, but mutiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/c_ocs2_basic_gradient_descent</url> -->
-
-
- <!-- Author tags are optional, mutiple are allowed, one per tag -->
- <!-- Authors do not have to be maintianers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
- <!-- The *_depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use run_depend for packages you need at runtime: -->
- <!-- <run_depend>message_runtime</run_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>ocs2_core</build_depend>
- <build_depend>glpk</build_depend>
- <run_depend>ocs2_core</run_depend>
- <run_depend>libglpk-dev</run_depend> <!-- required for the buildserver to install libglpk-dev -->
-
-
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- Other tools can request additional information be placed here -->
-
- </export>
-</package>
diff --git a/.catkin_tools/profiles/default/packages/ocs2_perceptive/devel_manifest.txt b/.catkin_tools/profiles/default/packages/ocs2_perceptive/devel_manifest.txt
deleted file mode 100644
index e36773b..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_perceptive/devel_manifest.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-ocs2/ocs2_perceptive
-/home/robofish/CM_DOG/devel/.private/ocs2_perceptive/cmake.lock /home/robofish/CM_DOG/devel/./cmake.lock
-/home/robofish/CM_DOG/devel/.private/ocs2_perceptive/share/ocs2_perceptive/cmake/ocs2_perceptiveConfig.cmake /home/robofish/CM_DOG/devel/share/ocs2_perceptive/cmake/ocs2_perceptiveConfig.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_perceptive/share/ocs2_perceptive/cmake/ocs2_perceptiveConfig-version.cmake /home/robofish/CM_DOG/devel/share/ocs2_perceptive/cmake/ocs2_perceptiveConfig-version.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_perceptive/lib/libocs2_perceptive.so /home/robofish/CM_DOG/devel/lib/libocs2_perceptive.so
-/home/robofish/CM_DOG/devel/.private/ocs2_perceptive/lib/ocs2_perceptive/ocs2_perceptive_lintTarget /home/robofish/CM_DOG/devel/lib/ocs2_perceptive/ocs2_perceptive_lintTarget
-/home/robofish/CM_DOG/devel/.private/ocs2_perceptive/lib/pkgconfig/ocs2_perceptive.pc /home/robofish/CM_DOG/devel/lib/pkgconfig/ocs2_perceptive.pc
diff --git a/.catkin_tools/profiles/default/packages/ocs2_perceptive/package.xml b/.catkin_tools/profiles/default/packages/ocs2_perceptive/package.xml
deleted file mode 100644
index a85af33..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_perceptive/package.xml
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>ocs2_perceptive</name>
- <version>0.0.0</version>
- <description>The ocs2_perceptive package</description>
-
- <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
-
- <license>BSD</license>
-
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>cmake_clang_tools</build_depend>
- <depend>ocs2_core</depend>
- <depend>ocs2_robotic_tools</depend>
-
-</package>
diff --git a/.catkin_tools/profiles/default/packages/ocs2_python_interface/devel_manifest.txt b/.catkin_tools/profiles/default/packages/ocs2_python_interface/devel_manifest.txt
deleted file mode 100644
index c6d1397..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_python_interface/devel_manifest.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-ocs2/ocs2_python_interface
-/home/robofish/CM_DOG/devel/.private/ocs2_python_interface/cmake.lock /home/robofish/CM_DOG/devel/./cmake.lock
-/home/robofish/CM_DOG/devel/.private/ocs2_python_interface/share/ocs2_python_interface/cmake/ocs2_python_interfaceConfig.cmake /home/robofish/CM_DOG/devel/share/ocs2_python_interface/cmake/ocs2_python_interfaceConfig.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_python_interface/share/ocs2_python_interface/cmake/ocs2_python_interfaceConfig-version.cmake /home/robofish/CM_DOG/devel/share/ocs2_python_interface/cmake/ocs2_python_interfaceConfig-version.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_python_interface/lib/libocs2_python_interface.so /home/robofish/CM_DOG/devel/lib/libocs2_python_interface.so
-/home/robofish/CM_DOG/devel/.private/ocs2_python_interface/lib/pkgconfig/ocs2_python_interface.pc /home/robofish/CM_DOG/devel/lib/pkgconfig/ocs2_python_interface.pc
diff --git a/.catkin_tools/profiles/default/packages/ocs2_python_interface/package.xml b/.catkin_tools/profiles/default/packages/ocs2_python_interface/package.xml
deleted file mode 100644
index 993fced..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_python_interface/package.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>ocs2_python_interface</name>
- <version>0.0.0</version>
- <description>The ocs2_python_interface package</description>
-
- <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
- <maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
- <maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>
-
- <license>TODO</license>
-
- <buildtool_depend>catkin</buildtool_depend>
-
- <build_depend>cmake_modules</build_depend>
- <build_depend>cmake_clang_tools</build_depend>
-
- <depend>ocs2_mpc</depend>
- <depend>ocs2_ddp</depend>
- <depend>ocs2_robotic_tools</depend>
-
-</package>
diff --git a/.catkin_tools/profiles/default/packages/ocs2_raisim_core/package.xml b/.catkin_tools/profiles/default/packages/ocs2_raisim_core/package.xml
deleted file mode 100644
index 9f6ca82..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_raisim_core/package.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>ocs2_raisim_core</name>
- <version>0.0.1</version>
- <description>The ocs2_raisim_core package</description>
-
- <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
- <maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
- <maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>
-
- <license>BSD</license>
-
- <author email="jcarius@ethz.ch">Jan Carius</author>
-
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>cmake_clang_tools</build_depend>
-
- <depend>ocs2_core</depend>
- <depend>ocs2_oc</depend>
-
-</package>
diff --git a/.catkin_tools/profiles/default/packages/ocs2_slp/devel_manifest.txt b/.catkin_tools/profiles/default/packages/ocs2_slp/devel_manifest.txt
deleted file mode 100644
index a16101d..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_slp/devel_manifest.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-ocs2/ocs2_slp
-/home/robofish/CM_DOG/devel/.private/ocs2_slp/cmake.lock /home/robofish/CM_DOG/devel/./cmake.lock
-/home/robofish/CM_DOG/devel/.private/ocs2_slp/share/ocs2_slp/cmake/ocs2_slpConfig-version.cmake /home/robofish/CM_DOG/devel/share/ocs2_slp/cmake/ocs2_slpConfig-version.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_slp/share/ocs2_slp/cmake/ocs2_slpConfig.cmake /home/robofish/CM_DOG/devel/share/ocs2_slp/cmake/ocs2_slpConfig.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_slp/lib/libocs2_slp.so /home/robofish/CM_DOG/devel/lib/libocs2_slp.so
-/home/robofish/CM_DOG/devel/.private/ocs2_slp/lib/pkgconfig/ocs2_slp.pc /home/robofish/CM_DOG/devel/lib/pkgconfig/ocs2_slp.pc
-/home/robofish/CM_DOG/devel/.private/ocs2_slp/lib/ocs2_slp/ocs2_slp_lintTarget /home/robofish/CM_DOG/devel/lib/ocs2_slp/ocs2_slp_lintTarget
diff --git a/.catkin_tools/profiles/default/packages/ocs2_slp/package.xml b/.catkin_tools/profiles/default/packages/ocs2_slp/package.xml
deleted file mode 100644
index 52ba012..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_slp/package.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>ocs2_slp</name>
- <version>0.0.0</version>
- <description>A numerical implementation of a first order primal-dual MPC basee on PIPG.</description>
-
- <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
- <maintainer email="zhengfuaj@gmail.com">Zhengyu Fu</maintainer>
-
- <license>BSD3</license>
-
- <buildtool_depend>catkin</buildtool_depend>
- <depend>ocs2_core</depend>
- <depend>ocs2_mpc</depend>
- <depend>ocs2_oc</depend>
-
- <!-- Test dependancy -->
- <depend>ocs2_qp_solver</depend>
-
-</package>
\ No newline at end of file
diff --git a/.catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/devel_manifest.txt b/.catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/devel_manifest.txt
deleted file mode 100644
index 5521580..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/devel_manifest.txt
+++ /dev/null
@@ -1,44 +0,0 @@
-ocs2/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/cmake.lock /home/robofish/CM_DOG/devel/./cmake.lock
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/roseus/ros/ocs2_switched_model_msgs/manifest.l /home/robofish/CM_DOG/devel/share/roseus/ros/ocs2_switched_model_msgs/manifest.l
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/roseus/ros/ocs2_switched_model_msgs/srv/trajectory_request.l /home/robofish/CM_DOG/devel/share/roseus/ros/ocs2_switched_model_msgs/srv/trajectory_request.l
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/roseus/ros/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.l /home/robofish/CM_DOG/devel/share/roseus/ros/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.l
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/roseus/ros/ocs2_switched_model_msgs/msg/gait.l /home/robofish/CM_DOG/devel/share/roseus/ros/ocs2_switched_model_msgs/msg/gait.l
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/roseus/ros/ocs2_switched_model_msgs/msg/gait_sequence.l /home/robofish/CM_DOG/devel/share/roseus/ros/ocs2_switched_model_msgs/msg/gait_sequence.l
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/srv/trajectory_request.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/srv/trajectory_request.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/srv/_package_trajectory_request.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/srv/_package_trajectory_request.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/srv/_package.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/srv/_package.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/srv/ocs2_switched_model_msgs-srv.asd /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/srv/ocs2_switched_model_msgs-srv.asd
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/gait.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/gait.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package_gait.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package_gait.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package_gait_sequence.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package_gait_sequence.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/gait_sequence.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/gait_sequence.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package_scheduled_gait_sequence.lisp /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/_package_scheduled_gait_sequence.lisp
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/common-lisp/ros/ocs2_switched_model_msgs/msg/ocs2_switched_model_msgs-msg.asd /home/robofish/CM_DOG/devel/share/common-lisp/ros/ocs2_switched_model_msgs/msg/ocs2_switched_model_msgs-msg.asd
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/gennodejs/ros/ocs2_switched_model_msgs/_index.js /home/robofish/CM_DOG/devel/share/gennodejs/ros/ocs2_switched_model_msgs/_index.js
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/gennodejs/ros/ocs2_switched_model_msgs/srv/trajectory_request.js /home/robofish/CM_DOG/devel/share/gennodejs/ros/ocs2_switched_model_msgs/srv/trajectory_request.js
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/gennodejs/ros/ocs2_switched_model_msgs/srv/_index.js /home/robofish/CM_DOG/devel/share/gennodejs/ros/ocs2_switched_model_msgs/srv/_index.js
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/gennodejs/ros/ocs2_switched_model_msgs/msg/gait_sequence.js /home/robofish/CM_DOG/devel/share/gennodejs/ros/ocs2_switched_model_msgs/msg/gait_sequence.js
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/gennodejs/ros/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.js /home/robofish/CM_DOG/devel/share/gennodejs/ros/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.js
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/gennodejs/ros/ocs2_switched_model_msgs/msg/_index.js /home/robofish/CM_DOG/devel/share/gennodejs/ros/ocs2_switched_model_msgs/msg/_index.js
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/gennodejs/ros/ocs2_switched_model_msgs/msg/gait.js /home/robofish/CM_DOG/devel/share/gennodejs/ros/ocs2_switched_model_msgs/msg/gait.js
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgs-msg-extras.cmake /home/robofish/CM_DOG/devel/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgs-msg-extras.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgsConfig.cmake /home/robofish/CM_DOG/devel/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgsConfig.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgs-msg-paths.cmake /home/robofish/CM_DOG/devel/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgs-msg-paths.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgsConfig-version.cmake /home/robofish/CM_DOG/devel/share/ocs2_switched_model_msgs/cmake/ocs2_switched_model_msgsConfig-version.cmake
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/include/ocs2_switched_model_msgs/gait.h /home/robofish/CM_DOG/devel/include/ocs2_switched_model_msgs/gait.h
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/include/ocs2_switched_model_msgs/trajectory_requestResponse.h /home/robofish/CM_DOG/devel/include/ocs2_switched_model_msgs/trajectory_requestResponse.h
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/include/ocs2_switched_model_msgs/gait_sequence.h /home/robofish/CM_DOG/devel/include/ocs2_switched_model_msgs/gait_sequence.h
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/include/ocs2_switched_model_msgs/trajectory_requestRequest.h /home/robofish/CM_DOG/devel/include/ocs2_switched_model_msgs/trajectory_requestRequest.h
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/include/ocs2_switched_model_msgs/scheduled_gait_sequence.h /home/robofish/CM_DOG/devel/include/ocs2_switched_model_msgs/scheduled_gait_sequence.h
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/include/ocs2_switched_model_msgs/trajectory_request.h /home/robofish/CM_DOG/devel/include/ocs2_switched_model_msgs/trajectory_request.h
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/pkgconfig/ocs2_switched_model_msgs.pc /home/robofish/CM_DOG/devel/lib/pkgconfig/ocs2_switched_model_msgs.pc
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/python3/dist-packages/ocs2_switched_model_msgs/__init__.py /home/robofish/CM_DOG/devel/lib/python3/dist-packages/ocs2_switched_model_msgs/__init__.py
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/python3/dist-packages/ocs2_switched_model_msgs/srv/_trajectory_request.py /home/robofish/CM_DOG/devel/lib/python3/dist-packages/ocs2_switched_model_msgs/srv/_trajectory_request.py
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/python3/dist-packages/ocs2_switched_model_msgs/srv/__init__.py /home/robofish/CM_DOG/devel/lib/python3/dist-packages/ocs2_switched_model_msgs/srv/__init__.py
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/_scheduled_gait_sequence.py /home/robofish/CM_DOG/devel/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/_scheduled_gait_sequence.py
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/_gait.py /home/robofish/CM_DOG/devel/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/_gait.py
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/__init__.py /home/robofish/CM_DOG/devel/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/__init__.py
-/home/robofish/CM_DOG/devel/.private/ocs2_switched_model_msgs/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/_gait_sequence.py /home/robofish/CM_DOG/devel/lib/python3/dist-packages/ocs2_switched_model_msgs/msg/_gait_sequence.py
diff --git a/.catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/package.xml b/.catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/package.xml
deleted file mode 100644
index 85bdd01..0000000
--- a/.catkin_tools/profiles/default/packages/ocs2_switched_model_msgs/package.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-<package format="2">
- <name>ocs2_switched_model_msgs</name>
- <version>0.0.1</version>
- <description>Provides ocs2_switched_model_msgs</description>
-
- <maintainer email="mbjelonic@ethz.ch">Marko Bjelonic</maintainer>
- <maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>
- <maintainer email="oharley@student.ethz.ch">Oliver Harley</maintainer>
- <author>Oliver Harley</author>
-
- <license>TODO</license>
-
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>message_generation</build_depend>
- <depend>std_msgs</depend>
- <depend>ocs2_msgs</depend>
- <exec_depend>message_runtime</exec_depend>
-
-</package>
diff --git a/.catkin_workspace b/.catkin_workspace
deleted file mode 100644
index 52fd97e..0000000
--- a/.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/.vscode/settings.json b/.vscode/settings.json
deleted file mode 100644
index 099a7e4..0000000
--- a/.vscode/settings.json
+++ /dev/null
@@ -1,74 +0,0 @@
-{
- "files.associations": {
- "deque": "cpp",
- "string": "cpp",
- "vector": "cpp",
- "hash_map": "cpp",
- "hash_set": "cpp",
- "*.tcc": "cpp",
- "unordered_map": "cpp",
- "unordered_set": "cpp",
- "string_view": "cpp",
- "ranges": "cpp",
- "cctype": "cpp",
- "clocale": "cpp",
- "cmath": "cpp",
- "cstdarg": "cpp",
- "cstddef": "cpp",
- "cstdio": "cpp",
- "cstdlib": "cpp",
- "cstring": "cpp",
- "ctime": "cpp",
- "cwchar": "cpp",
- "cwctype": "cpp",
- "array": "cpp",
- "atomic": "cpp",
- "bit": "cpp",
- "bitset": "cpp",
- "chrono": "cpp",
- "compare": "cpp",
- "complex": "cpp",
- "concepts": "cpp",
- "condition_variable": "cpp",
- "cstdint": "cpp",
- "forward_list": "cpp",
- "list": "cpp",
- "map": "cpp",
- "set": "cpp",
- "exception": "cpp",
- "algorithm": "cpp",
- "functional": "cpp",
- "iterator": "cpp",
- "memory": "cpp",
- "memory_resource": "cpp",
- "numeric": "cpp",
- "optional": "cpp",
- "random": "cpp",
- "ratio": "cpp",
- "system_error": "cpp",
- "tuple": "cpp",
- "type_traits": "cpp",
- "utility": "cpp",
- "fstream": "cpp",
- "initializer_list": "cpp",
- "iomanip": "cpp",
- "iosfwd": "cpp",
- "iostream": "cpp",
- "istream": "cpp",
- "limits": "cpp",
- "mutex": "cpp",
- "new": "cpp",
- "ostream": "cpp",
- "sstream": "cpp",
- "stdexcept": "cpp",
- "stop_token": "cpp",
- "streambuf": "cpp",
- "thread": "cpp",
- "cfenv": "cpp",
- "cinttypes": "cpp",
- "typeinfo": "cpp",
- "valarray": "cpp",
- "*.ipp": "cpp",
- "shared_mutex": "cpp"
- }
-}
\ No newline at end of file
diff --git a/README.md b/README.md
index 82a78c9..eac5313 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,8 @@
-# JJump_Dog
+# JJump_Dog（跳跳狗）
+
+基于legged_control创建的四足机器狗
+
+用于CURC——Robocon大赛马术赛道
+
+## 食用指南
diff --git a/contactThreshold_ b/contactThreshold_
new file mode 100644
index 0000000..e69de29
diff --git a/run.sh b/run.sh
new file mode 100644
index 0000000..ec22255
--- /dev/null
+++ b/run.sh
@@ -0,0 +1,4 @@
+roslaunch legged_unitree_hw legged_unitree_hw.launch
+roslaunch legged_hw legged_hw.launch
+roslaunch legged_controllers load_controller.launch cheater:=false
+roslaunch fdilink_ahrs ahrs_data.launch
\ No newline at end of file
diff --git a/src/fdilink_ahrs/launch/ahrs_data.launch b/src/fdilink_ahrs/launch/ahrs_data.launch
index 5c8b7d5..57c743b 100644
--- a/src/fdilink_ahrs/launch/ahrs_data.launch
+++ b/src/fdilink_ahrs/launch/ahrs_data.launch
@@ -3,7 +3,7 @@
<!-- 是否输出debug信息 -->
<param name="debug" value="false"/>
- <param name="port" value="/dev/wheeltec_FDI_IMU_GNSS"/>
+ <param name="port" value="/dev/ttyUSB0"/>
<!-- 波特率 -->
<param name="baud" value="921600"/>
diff --git a/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/include/legged_unitree_hw/UnitreeHW.h b/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/include/legged_unitree_hw/UnitreeHW.h
index de5e7fd..9c0b8c7 100644
--- a/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/include/legged_unitree_hw/UnitreeHW.h
+++ b/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/include/legged_unitree_hw/UnitreeHW.h
@@ -1,3 +1,4 @@
+
//
// Created by qiayuan on 1/24/22.
//
@@ -5,6 +6,7 @@
#pragma once
#include <legged_hw/LeggedHW.h>
+#include <sensor_msgs/Imu.h> // 确保包含这个头文件
#ifdef UNITREE_SDK_3_3_1
#include "unitree_legged_sdk_3_3_1/safety.h"
@@ -18,48 +20,49 @@ namespace legged {
const std::vector<std::string> CONTACT_SENSOR_NAMES = {"RF_FOOT", "LF_FOOT", "RH_FOOT", "LH_FOOT"};
struct UnitreeMotorData {
- double pos_, vel_, tau_; // 状态
- double posDes_, velDes_, kp_, kd_, ff_; // 命令
+ double pos_, vel_, tau_; // state
+ double posDes_, velDes_, kp_, kd_, ff_; // command
};
struct UnitreeImuData {
- double ori_[4]; // 方向（四元数）
- double oriCov_[9]; // 方向协方差
- double angularVel_[3]; // 角速度
- double angularVelCov_[9]; // 角速度协方差
- double linearAcc_[3]; // 线性加速度
- double linearAccCov_[9]; // 线性加速度协方差
+ double ori_[4]; // NOLINT(modernize-avoid-c-arrays)
+ double oriCov_[9]; // NOLINT(modernize-avoid-c-arrays)
+ double angularVel_[3]; // NOLINT(modernize-avoid-c-arrays)
+ double angularVelCov_[9]; // NOLINT(modernize-avoid-c-arrays)
+ double linearAcc_[3]; // NOLINT(modernize-avoid-c-arrays)
+ double linearAccCov_[9]; // NOLINT(modernize-avoid-c-arrays)
};
class UnitreeHW : public LeggedHW {
public:
UnitreeHW() = default;
- /** \brief 从参数服务器获取必要的参数。初始化 hardware_interface。
+ /** \brief Get necessary params from param server. Init hardware_interface.
*
- * 从参数服务器获取参数并检查这些参数是否已设置。加载机器人的 URDF。设置传动和关节限制。
- * 获取 CAN 总线的配置并创建指向从 CAN 总线接收的数据的指针。
+ * Get params from param server and check whether these params are set. Load urdf of robot. Set up transmission and
+ * joint limit. Get configuration of can bus and create data pointer which point to data received from Can bus.
*
- * @param root_nh ROS 节点的根节点句柄。
- * @param robot_hw_nh 机器人硬件的节点句柄。
- * @return 成功初始化时返回 True，失败时返回 False。
+ * @param root_nh Root node-handle of a ROS node.
+ * @param robot_hw_nh Node-handle for robot hardware.
+ * @return True when init successful, False when failed.
*/
bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
- /** \brief 与硬件通信。获取机器人的数据和状态。
+ /** \brief Communicate with hardware. Get data, status of robot.
*
- * 调用 @ref UNITREE_LEGGED_SDK::UDP::Recv() 获取机器人的状态。
+ * Call @ref UNITREE_LEGGED_SDK::UDP::Recv() to get robot's state.
*
- * @param time 当前时间
- * @param period 当前时间 - 上次时间
+ * @param time Current time
+ * @param period Current time - last time
*/
void read(const ros::Time& time, const ros::Duration& period) override;
- /** \brief 与硬件通信。向机器人发布命令。
+ /** \brief Comunicate with hardware. Publish command to robot.
*
- * 将关节状态传播到存储的执行器状态。将命令扭矩限制在适当的值内。调用 @ref UNITREE_LEGGED_SDK::UDP::Recv()。
- * 发布执行器的当前状态。
+ * Propagate joint state to actuator state for the stored
+ * transmission. Limit cmd_effort into suitable value. Call @ref UNITREE_LEGGED_SDK::UDP::Recv(). Publish actuator
+ * current state.
*
- * @param time 当前时间
- * @param period 当前时间 - 上次时间
+ * @param time Current time
+ * @param period Current time - last time
*/
void write(const ros::Time& time, const ros::Duration& period) override;
@@ -68,6 +71,8 @@ class UnitreeHW : public LeggedHW {
void updateContact(const ros::Time& time);
private:
+ void imuCallback(const sensor_msgs::Imu::ConstPtr& imuMsg);
+
bool setupJoints();
bool setupImu();
@@ -78,17 +83,18 @@ class UnitreeHW : public LeggedHW {
std::shared_ptr<UNITREE_LEGGED_SDK::Safety> safety_;
UNITREE_LEGGED_SDK::LowState lowState_{};
UNITREE_LEGGED_SDK::LowCmd lowCmd_{};
+ ros::Subscriber imuSubscriber_;
+ sensor_msgs::Imu::ConstPtr lastImuMsg_;
+ UnitreeMotorData jointData_[12]{}; // NOLINT(modernize-avoid-c-arrays)
+ UnitreeImuData imuData_{};
+ bool contactState_[4]{}; // NOLINT(modernize-avoid-c-arrays)
+ sensor_msgs::Imu imuDataMsg_;
+ int powerLimit_{};
+ int contactThreshold_{};
- UnitreeMotorData jointData_[12]{}; // 关节数据
- UnitreeImuData imuData_{}; // IMU 数据
- bool contactState_[4]{}; // 接触状态
-
- int powerLimit_{}; // 功率限制
- int contactThreshold_{}; // 接触阈值
-
- ros::Publisher joyPublisher_; // 手柄发布者
- ros::Publisher contactPublisher_; // 接触发布者
- ros::Time lastJoyPub_, lastContactPub_; // 上次手柄和接触发布的时间
+ ros::Publisher joyPublisher_;
+ ros::Publisher contactPublisher_;
+ ros::Time lastJoyPub_, lastContactPub_;
};
-} // namespace legged
\ No newline at end of file
+} // namespace legged
diff --git a/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp b/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp
index c79db26..16e9e95 100644
--- a/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp
+++ b/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp
@@ -1,9 +1,9 @@
-
//
// Created by qiayuan on 1/24/22.
//
#include "legged_unitree_hw/UnitreeHW.h"
+#include <ros/ros.h> // 确保包含这个头文件
#ifdef UNITREE_SDK_3_3_1
#include "unitree_legged_sdk_3_3_1/unitree_joystick.h"
@@ -13,8 +13,10 @@
#include <sensor_msgs/Joy.h>
#include <std_msgs/Int16MultiArray.h>
+#include <sensor_msgs/Imu.h>
namespace legged {
+
bool UnitreeHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
if (!LeggedHW::init(root_nh, robot_hw_nh)) {
return false;
@@ -54,12 +56,52 @@ bool UnitreeHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) {
joyPublisher_ = root_nh.advertise<sensor_msgs::Joy>("/joy", 10);
contactPublisher_ = root_nh.advertise<std_msgs::Int16MultiArray>(std::string("/contact"), 10);
+ imuSubscriber_ = root_nh.subscribe("/imu", 10, &UnitreeHW::imuCallback, this);
+
return true;
}
+void UnitreeHW::imuCallback(const sensor_msgs::Imu::ConstPtr& imuMsg) {
+ lastImuMsg_ = imuMsg;
+}
+
void UnitreeHW::read(const ros::Time& time, const ros::Duration& /*period*/) {
- udp_->Recv();
- udp_->GetRecv(lowState_);
+ // 模拟接收数据
+ // udp_->Recv();
+ // udp_->GetRecv(lowState_);
+
+ // 模拟数据
+ for (int i = 0; i < 12; ++i) {
+ // lowState_.motorState[i].q = static_cast<float>(i * 0.1); // 模拟位置
+ // lowState_.motorState[i].dq = static_cast<float>(i * 0.01); // 模拟速度
+ // lowState_.motorState[i].tauEst = static_cast<float>(i * 0.001); // 模拟力矩
+ lowState_.motorState[i].q = 0.0; // 模拟位置
+ lowState_.motorState[i].dq = 0.0; // 模拟速度
+ lowState_.motorState[i].tauEst = 0.0; // 模拟力矩
+ }
+
+ // 读取 /imu 话题的数据
+ if (lastImuMsg_) {
+ lowState_.imu.quaternion[0] = lastImuMsg_->orientation.w;
+ lowState_.imu.quaternion[1] = lastImuMsg_->orientation.x;
+ lowState_.imu.quaternion[2] = lastImuMsg_->orientation.y;
+ lowState_.imu.quaternion[3] = lastImuMsg_->orientation.z;
+ lowState_.imu.gyroscope[0] = lastImuMsg_->angular_velocity.x;
+ lowState_.imu.gyroscope[1] = lastImuMsg_->angular_velocity.y;
+ lowState_.imu.gyroscope[2] = lastImuMsg_->angular_velocity.z;
+ lowState_.imu.accelerometer[0] = lastImuMsg_->linear_acceleration.x;
+ lowState_.imu.accelerometer[1] = lastImuMsg_->linear_acceleration.y;
+ lowState_.imu.accelerometer[2] = lastImuMsg_->linear_acceleration.z;
+
+ // 打印接收到的数据
+ ROS_INFO("imuMsg->orientation.w: %f", lastImuMsg_->orientation.w);
+ } else {
+ ROS_WARN("Failed to receive IMU data");
+ }
+
+ for (size_t i = 0; i < CONTACT_SENSOR_NAMES.size(); ++i) {
+ lowState_.footForce[i] = 10.0; // 模拟接触传感器数据
+ }
for (int i = 0; i < 12; ++i) {
jointData_[i].pos_ = lowState_.motorState[i].q;
@@ -82,7 +124,7 @@ void UnitreeHW::read(const ros::Time& time, const ros::Duration& /*period*/) {
contactState_[i] = lowState_.footForce[i] > contactThreshold_;
}
- // Set feedforward and velocity cmd to zero to avoid for safety when not controller setCommand
+ // 设置前馈和期望速度为零，以避免在未设置控制器 setCommand 时出现安全问题
std::vector<std::string> names = hybridJointInterface_.getNames();
for (const auto& name : names) {
HybridJointHandle handle = hybridJointInterface_.getHandle(name);
@@ -175,7 +217,7 @@ void UnitreeHW::updateJoystick(const ros::Time& time) {
lastJoyPub_ = time;
xRockerBtnDataStruct keyData;
memcpy(&keyData, &lowState_.wirelessRemote[0], 40);
- sensor_msgs::Joy joyMsg; // Pack as same as Logitech F710
+ sensor_msgs::Joy joyMsg; // 打包与 Logitech F710 相同
joyMsg.axes.push_back(-keyData.lx);
joyMsg.axes.push_back(keyData.ly);
joyMsg.axes.push_back(-keyData.rx);
@@ -206,4 +248,4 @@ void UnitreeHW::updateContact(const ros::Time& time) {
contactPublisher_.publish(contactMsg);
}
-} // namespace legged
+} // namespace legged
\ No newline at end of file
diff --git a/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/legged_unitree_hw.cpp b/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/legged_unitree_hw.cpp
index 0fd7733..32b929f 100644
--- a/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/legged_unitree_hw.cpp
+++ b/src/legged_control/legged_examples/legged_unitree/legged_unitree_hw/src/legged_unitree_hw.cpp
@@ -1,38 +1,25 @@
/*******************************************************************************
* BSD 3-Clause License
*
- * Copyright (c) 2021, Qiayuan Liao
- * All rights reserved.
+ * 版权所有 (c) 2021, Qiayuan Liao
+ * 保留所有权利。
*
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
+ * 允许在源代码和二进制形式中重新分发和使用，无论是否经过修改，只要满足以下条件：
*
- * * Redistributions of source code must retain the above copyright notice, this
- * list of conditions and the following disclaimer.
+ * * 源代码的重新分发必须保留上述版权声明、本条件列表和以下免责声明。
*
- * * Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
+ * * 二进制形式的重新分发必须在随附的文档和/或其他材料中包含上述版权声明、本条件列表和以下免责声明。
*
- * * Neither the name of the copyright holder nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
+ * * 未经特定事先书面许可，版权持有者或其贡献者的名称不得用于支持或推广源自本软件的产品。
*
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * 本软件由版权持有者和贡献者“按原样”提供，不提供任何明示或暗示的担保，包括但不限于对适销性和特定用途适用性的暗示担保。
+ * 在任何情况下，版权持有者或贡献者均不对任何直接、间接、附带、特殊、惩罚性或后果性损害（包括但不限于采购替代商品或服务；
+ * 使用、数据或利润的损失；或业务中断）承担责任，无论是如何造成的，也无论是在合同、严格责任或侵权行为（包括疏忽或其他原因）中，
+ * 即使已被告知可能会发生此类损害。
*******************************************************************************/
//
-// Created by qiayuan on 12/27/20.
+// 由 qiayuan 创建于 2020 年 12 月 27 日。
//
#include "legged_unitree_hw/UnitreeHW.h"
@@ -44,33 +31,32 @@ int main(int argc, char** argv) {
ros::NodeHandle nh;
ros::NodeHandle robotHwNh("~");
- // Run the hardware interface node
+ // 运行硬件接口节点
// -------------------------------
- // We run the ROS loop in a separate thread as external calls, such
- // as service callbacks loading controllers, can block the (main) control loop
+ // 我们在一个单独的线程中运行 ROS 循环，因为外部调用（例如服务回调加载控制器）可能会阻塞（主）控制循环
ros::AsyncSpinner spinner(3);
spinner.start();
try {
- // Create the hardware interface specific to your robot
+ // 创建特定于您的机器人的硬件接口
std::shared_ptr<legged::UnitreeHW> unitreeHw = std::make_shared<legged::UnitreeHW>();
- // Initialize the hardware interface:
- // 1. retrieve configuration from rosparam
- // 2. initialize the hardware and interface it with ros_control
+ // 初始化硬件接口：
+ // 1. 从 rosparam 检索配置
+ // 2. 初始化硬件并将其与 ros_control 接口
unitreeHw->init(nh, robotHwNh);
- // Start the control loop
+ // 启动控制循环
legged::LeggedHWLoop controlLoop(nh, unitreeHw);
- // Wait until shutdown signal received
+ // 等待接收到关闭信号
ros::waitForShutdown();
} catch (const ros::Exception& e) {
- ROS_FATAL_STREAM("Error in the hardware interface:\n"
+ ROS_FATAL_STREAM("硬件接口错误:\n"
<< "\t" << e.what());
return 1;
}
return 0;
-}
+}
\ No newline at end of file