├── LICENSE ├── README.md ├── doc ├── controller.png ├── gazebo.png ├── ipconfig.png ├── launch.png ├── rqt_gui.png └── simulator.gif ├── robot_common ├── CMakeLists.txt ├── include │ └── robot_common │ │ └── hardware_interface │ │ ├── ContactSensorInterface.h │ │ └── HybridJointInterface.h └── package.xml ├── robot_controllers ├── CMakeLists.txt ├── config │ ├── controllers.yaml │ └── pointfoot │ │ ├── PF_P441A │ │ ├── params.yaml │ │ └── policy │ │ │ ├── encoder.onnx │ │ │ └── policy.onnx │ │ ├── PF_P441B │ │ ├── params.yaml │ │ └── policy │ │ │ ├── encoder.onnx │ │ │ └── policy.onnx │ │ ├── PF_P441C │ │ ├── params.yaml │ │ └── policy │ │ │ ├── encoder.onnx │ │ │ └── policy.onnx │ │ ├── PF_P441C2 │ │ ├── params.yaml │ │ └── policy │ │ │ ├── encoder.onnx │ │ │ └── policy.onnx │ │ ├── PF_TRON1A │ │ ├── params.yaml │ │ └── policy │ │ │ ├── encoder.onnx │ │ │ └── policy.onnx │ │ ├── SF_TRON1A │ │ ├── params.yaml │ │ └── policy │ │ │ ├── encoder.onnx │ │ │ └── policy.onnx │ │ └── WF_TRON1A │ │ ├── params.yaml │ │ └── policy │ │ ├── encoder.onnx │ │ └── policy.onnx ├── include │ └── robot_controllers │ │ ├── ControllerBase.h │ │ ├── PointfootController.h │ │ ├── SolefootController.h │ │ └── WheelfootController.h ├── package.xml ├── robot_controllers_plugins.xml └── src │ ├── ControllerBase.cpp │ ├── PointfootController.cpp │ ├── SolefootController.cpp │ └── WheelfootController.cpp └── robot_hw ├── CMakeLists.txt ├── config ├── joystick.yaml ├── joystick_sim.yaml └── robot_hw.yaml ├── include └── robot_hw │ ├── PointfootHW.h │ ├── RobotData.h │ ├── RobotHW.h │ └── RobotHWLoop.h ├── launch ├── pointfoot_controller_sim.launch ├── pointfoot_hw.launch └── pointfoot_hw_sim.launch ├── package.xml └── src ├── PointfootHW.cpp ├── RobotHW.cpp ├── RobotHWLoop.cpp └── pointfootNode.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 训练结果部署 2 | 3 | 4 | 5 | ## 1. 部署环境配置 6 | 7 | - 安装ROS Noetic：我们推荐在Ubuntu 20.04操作系统上建立基于ROS Noetic的算法开发环境。ROS提供了一系列工具和库，如核心库、通信库和仿真工具（如Gazebo），极大地便利了机器人算法的开发、测试和部署。这些资源为用户提供了一个丰富而完整的算法开发环境。ROS Noetic 安装请参考文档：https://wiki.ros.org/noetic/Installation/Ubuntu ，选择“ros-noetic-desktop-full”进行安装。ROS Noetic 安装完成后，Bash终端输入以下Shell命令，安装开发环境所依赖的库： 8 | 9 | ```bash 10 | sudo apt-get update 11 | sudo apt install ros-noetic-urdf \ 12 | ros-noetic-kdl-parser \ 13 | ros-noetic-urdf-parser-plugin \ 14 | ros-noetic-hardware-interface \ 15 | ros-noetic-controller-manager \ 16 | ros-noetic-controller-interface \ 17 | ros-noetic-controller-manager-msgs \ 18 | ros-noetic-control-msgs \ 19 | ros-noetic-ros-control \ 20 | ros-noetic-gazebo-* \ 21 | ros-noetic-robot-state-* \ 22 | ros-noetic-joint-state-* \ 23 | ros-noetic-rqt-gui \ 24 | ros-noetic-rqt-controller-manager \ 25 | ros-noetic-plotjuggler* \ 26 | cmake build-essential libpcl-dev libeigen3-dev libopencv-dev libmatio-dev \ 27 | python3-pip libboost-all-dev libtbb-dev liburdfdom-dev liborocos-kdl-dev -y 28 | ``` 29 | 30 | 31 | 32 | - 安装onnxruntime依赖，下载连接：https://github.com/microsoft/onnxruntime/releases/tag/v1.10.0 。请您根据自己的操作系统和平台选择合适版本下载。如在Ubuntu 20.04 x86_64，请按下面步骤进行安装： 33 | 34 | ```Bash 35 | wget https://github.com/microsoft/onnxruntime/releases/download/v1.10.0/onnxruntime-linux-x64-1.10.0.tgz 36 | 37 | tar xvf onnxruntime-linux-x64-1.10.0.tgz 38 | 39 | sudo cp -a onnxruntime-linux-x64-1.10.0/include/* /usr/include 40 | sudo cp -a onnxruntime-linux-x64-1.10.0/lib/* /usr/lib 41 | ``` 42 | 43 | 44 | 45 | ## 2. 创建工作空间 46 | 47 | 可以按照以下步骤，创建一个RL部署开发工作空间： 48 | 49 | - 打开一个Bash终端。 50 | 51 | - 创建一个新目录来存放工作空间。例如，可以在用户的主目录下创建一个名为“limx_ws”的目录： 52 | ```Bash 53 | mkdir -p ~/limx_ws/src 54 | ``` 55 | 56 | - 下载运动控制开发接口： 57 | ```Bash 58 | cd ~/limx_ws/src 59 | git clone https://github.com/limxdynamics/pointfoot-sdk-lowlevel.git 60 | ``` 61 | 62 | - 下载Gazebo仿真器： 63 | ```Bash 64 | cd ~/limx_ws/src 65 | git clone https://github.com/limxdynamics/pointfoot-gazebo-ros.git 66 | ``` 67 | 68 | - 下载机器人模型描述文件 69 | ```Bash 70 | cd ~/limx_ws/src 71 | git clone https://github.com/limxdynamics/robot-description.git 72 | ``` 73 | 74 | - 下载可视化工具 75 | ```Bash 76 | cd ~/limx_ws/src 77 | git clone https://github.com/limxdynamics/robot-visualization.git 78 | ``` 79 | 80 | - 下载RL部署源码： 81 | ```Bash 82 | cd ~/limx_ws/src 83 | git clone https://github.com/limxdynamics/rl-deploy-ros-cpp.git 84 | ``` 85 | 86 | - 编译工程： 87 | ```Bash 88 | cd ~/limx_ws 89 | catkin_make install 90 | ``` 91 | 92 | - 选择机器人类型 93 | 94 | - 通过 Shell 命令 `tree -L 1 src/robot-description/pointfoot ` 列出可用的机器人类型： 95 | 96 | ``` 97 | src/robot-description/pointfoot 98 | ├── PF_P441A 99 | ├── PF_P441B 100 | ├── PF_P441C 101 | ├── PF_P441C2 102 | ├── PF_TRON1A 103 | ├── SF_TRON1A 104 | └── WF_TRON1A 105 | ``` 106 | 107 | - 以`PF_P441C`（请根据实际机器人类型进行替换）为例，设置机器人型号类型： 108 | 109 | ``` 110 | echo 'export ROBOT_TYPE=PF_P441C' >> ~/.bashrc && source ~/.bashrc 111 | ``` 112 | 113 | - 运行仿真 114 | 115 | 通过运行Shell命令启动Gazebo仿真器，然后在仿真器窗口中按 `Ctrl + Shift + R`，机器人将开始移动。您还可以通过将 `Robot Steering` 插件的发布主题设置为 `/cmd_vel` 来控制机器人的行走。 116 | 117 | 118 | ``` 119 | source install/setup.bash 120 | roslaunch robot_hw pointfoot_hw_sim.launch 121 | ``` 122 | ![](doc/simulator.gif) 123 | 124 | -------------------------------------------------------------------------------- /doc/controller.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/doc/controller.png -------------------------------------------------------------------------------- /doc/gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/doc/gazebo.png -------------------------------------------------------------------------------- /doc/ipconfig.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/doc/ipconfig.png -------------------------------------------------------------------------------- /doc/launch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/doc/launch.png -------------------------------------------------------------------------------- /doc/rqt_gui.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/doc/rqt_gui.png -------------------------------------------------------------------------------- /doc/simulator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/doc/simulator.gif -------------------------------------------------------------------------------- /robot_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright information 2 | # 3 | # © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | # Declare the minimum required CMake version 6 | cmake_minimum_required(VERSION 3.10) 7 | 8 | # Declare the project 9 | project(robot_common) 10 | 11 | # Declare the include directories for the Catkin package 12 | # The directories specified here contain header files that other packages need when building or linking to this package. 13 | catkin_package(INCLUDE_DIRS include) 14 | 15 | # Install the include directory to the global Catkin include destination 16 | install(DIRECTORY include/. DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 17 | -------------------------------------------------------------------------------- /robot_common/include/robot_common/hardware_interface/ContactSensorInterface.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_CONTACT_SENSOR_INTERFACE_H_ 6 | #define _LIMX_CONTACT_SENSOR_INTERFACE_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace robot_common { 14 | 15 | // Class representing a handle for a contact sensor. 16 | class ContactSensorHandle { 17 | public: 18 | // Default constructor. 19 | ContactSensorHandle() = default; 20 | 21 | // Constructor with parameters. 22 | ContactSensorHandle(const std::string& name, const bool* is_contact) 23 | : name_(name), is_contact_(is_contact) { 24 | // Check if the pointer is null. 25 | if (is_contact == nullptr) { 26 | throw hardware_interface::HardwareInterfaceException( 27 | "Cannot create handle '" + name + "'. is_contact pointer is null."); 28 | } 29 | } 30 | 31 | // Getter function for the name of the contact sensor. 32 | std::string getName() const { 33 | return name_; 34 | } 35 | 36 | // Function to check if the contact sensor is in contact. 37 | bool isContact() const { 38 | // Ensure that the pointer is not null and return its value. 39 | assert(is_contact_); 40 | return *is_contact_; 41 | } 42 | 43 | private: 44 | std::string name_; // Name of the contact sensor. 45 | const bool* is_contact_{nullptr}; // Pointer to a boolean indicating if the sensor is in contact. 46 | }; 47 | 48 | // Interface for managing contact sensors. 49 | class ContactSensorInterface 50 | : public hardware_interface::HardwareResourceManager { 52 | }; 53 | 54 | } // namespace robot_common 55 | 56 | #endif // _LIMX_CONTACT_SENSOR_INTERFACE_H_ -------------------------------------------------------------------------------- /robot_common/include/robot_common/hardware_interface/HybridJointInterface.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_HYBRID_JOINT_INTERFACE_H_ 6 | #define _LIMX_HYBRID_JOINT_INTERFACE_H_ 7 | 8 | #include 9 | #include 10 | 11 | namespace robot_common { 12 | 13 | // Class defining a hybrid joint handle 14 | class HybridJointHandle : public hardware_interface::JointStateHandle { 15 | public: 16 | // Default constructor 17 | HybridJointHandle() = default; 18 | 19 | // Constructor with initialization 20 | HybridJointHandle(const JointStateHandle& js, double* posDes, double* velDes, double* kp, double* kd, double* ff, uint8_t* mode) 21 | : JointStateHandle(js), posDes_(posDes), velDes_(velDes), kp_(kp), kd_(kd), ff_(ff), mode_(mode) { 22 | // Check for null pointers 23 | if (posDes_ == nullptr) { 24 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + 25 | "'. Position desired data pointer is null."); 26 | } 27 | if (velDes_ == nullptr) { 28 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + 29 | "'. Velocity desired data pointer is null."); 30 | } 31 | if (kp_ == nullptr) { 32 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + "'. Kp data pointer is null."); 33 | } 34 | if (kd_ == nullptr) { 35 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + "'. Kd data pointer is null."); 36 | } 37 | if (ff_ == nullptr) { 38 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + 39 | "'. Feedforward data pointer is null."); 40 | } 41 | } 42 | 43 | // Setter methods 44 | 45 | // Set the desired position 46 | void setPositionDesired(double cmd) { 47 | assert(posDes_); 48 | *posDes_ = cmd; 49 | } 50 | 51 | // Set the desired velocity 52 | void setVelocityDesired(double cmd) { 53 | assert(velDes_); 54 | *velDes_ = cmd; 55 | } 56 | 57 | // Set the proportional gain 58 | void setKp(double cmd) { 59 | assert(kp_); 60 | *kp_ = cmd; 61 | } 62 | 63 | // Set the derivative gain 64 | void setKd(double cmd) { 65 | assert(kd_); 66 | *kd_ = cmd; 67 | } 68 | 69 | // Set the feedforward term 70 | void setFeedforward(double cmd) { 71 | assert(ff_); 72 | *ff_ = cmd; 73 | } 74 | 75 | // Set the control mode 76 | void setMode(uint8_t cmd) { 77 | assert(mode_); 78 | *mode_ = cmd; 79 | } 80 | 81 | // Method to set all control parameters at once 82 | void setCommand(double pos_des, double vel_des, double kp, double kd, double ff, uint8_t mode) { 83 | setPositionDesired(pos_des); 84 | setVelocityDesired(vel_des); 85 | setKp(kp); 86 | setKd(kd); 87 | setFeedforward(ff); 88 | setMode(mode); 89 | } 90 | 91 | // Getter methods 92 | 93 | // Get the desired position 94 | double getPositionDesired() { 95 | assert(posDes_); 96 | return *posDes_; 97 | } 98 | 99 | // Get the desired velocity 100 | double getVelocityDesired() { 101 | assert(velDes_); 102 | return *velDes_; 103 | } 104 | 105 | // Get the proportional gain 106 | double getKp() { 107 | assert(kp_); 108 | return *kp_; 109 | } 110 | 111 | // Get the derivative gain 112 | double getKd() { 113 | assert(kd_); 114 | return *kd_; 115 | } 116 | 117 | // Get the feedforward term 118 | double getFeedforward() { 119 | assert(ff_); 120 | return *ff_; 121 | } 122 | 123 | private: 124 | // Pointers to control parameters 125 | double* posDes_{nullptr}; // Pointer to desired position 126 | double* velDes_{nullptr}; // Pointer to desired velocity 127 | double* kp_{nullptr}; // Pointer to proportional gain 128 | double* kd_{nullptr}; // Pointer to derivative gain 129 | double* ff_{nullptr}; // Pointer to feedforward term 130 | uint8_t* mode_{nullptr}; // Pointer to control mode 131 | }; 132 | 133 | // Class defining a hybrid joint interface 134 | class HybridJointInterface : public hardware_interface::HardwareResourceManager {}; 135 | 136 | } // namespace robot_common 137 | 138 | #endif // _LIMX_HYBRID_JOINT_INTERFACE_H_ -------------------------------------------------------------------------------- /robot_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_common 4 | 0.0.1 5 | robot_common 6 | max 7 | TODO 8 | catkin 9 | 10 | -------------------------------------------------------------------------------- /robot_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright information 2 | # 3 | # © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | # Set the minimum required CMake version 6 | cmake_minimum_required(VERSION 3.10) 7 | 8 | # Define the project name 9 | project(robot_controllers) 10 | 11 | # Find the Eigen3 library required for linear algebra operations 12 | find_package(Eigen3 REQUIRED) 13 | 14 | # Enable generation of compile commands database (used by some code editors) 15 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 16 | 17 | ## Find catkin macros and libraries 18 | # Find and specify required catkin packages and components 19 | find_package(catkin REQUIRED 20 | COMPONENTS 21 | roscpp 22 | pointfoot_sdk_lowlevel 23 | robot_common 24 | robot_description 25 | controller_interface 26 | angles 27 | ) 28 | 29 | # Specify catkin-specific package configuration 30 | catkin_package( 31 | INCLUDE_DIRS 32 | include 33 | LIBRARIES 34 | ${PROJECT_NAME} 35 | CATKIN_DEPENDS 36 | roscpp 37 | pointfoot_sdk_lowlevel 38 | robot_common 39 | robot_description 40 | controller_interface 41 | angles 42 | ) 43 | 44 | # Include directories for header files 45 | include_directories( 46 | include 47 | ${CATKIN_DEVEL_PREFIX}/include 48 | ${EIGEN3_INCLUDE_DIRS} 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | 52 | link_directories(${CATKIN_DEVEL_PREFIX}/lib) 53 | 54 | # Add a shared library target 55 | add_library(${PROJECT_NAME} SHARED 56 | src/ControllerBase.cpp 57 | src/PointfootController.cpp 58 | src/WheelfootController.cpp 59 | src/SolefootController.cpp 60 | ) 61 | 62 | # Link libraries to the target 63 | target_link_libraries(${PROJECT_NAME} 64 | pointfoot_sdk_lowlevel 65 | ${catkin_LIBRARIES} # Link with catkin libraries 66 | onnxruntime # Link with onnxruntime library 67 | ) 68 | 69 | # Mark executables and/or libraries for installation 70 | install(TARGETS ${PROJECT_NAME} 71 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 73 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 74 | ) 75 | 76 | # Mark cpp header files for installation 77 | install(DIRECTORY include/. DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 78 | 79 | # Mark resource files for installation 80 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 81 | 82 | # Mark other files for installation 83 | install(FILES robot_controllers_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 84 | -------------------------------------------------------------------------------- /robot_controllers/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | pointfoot_controller: 3 | type: robot_controller/PointfootController 4 | 5 | wheelfoot_controller: 6 | type: robot_controller/WheelfootController 7 | 8 | solefoot_controller: 9 | type: robot_controller/SolefootController 10 | -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | joint_names: 3 | - "abad_L_Joint" 4 | - "hip_L_Joint" 5 | - "knee_L_Joint" 6 | - "abad_R_Joint" 7 | - "hip_R_Joint" 8 | - "knee_R_Joint" 9 | 10 | init_state: 11 | # target angles [rad] when action = 0.0 12 | default_joint_angle: 13 | abad_L_Joint: 0.0 14 | hip_L_Joint: 0.0 15 | knee_L_Joint: 0.0 16 | 17 | abad_R_Joint: 0.0 18 | hip_R_Joint: 0.0 19 | knee_R_Joint: 0.0 20 | 21 | control: 22 | # PD Drive parameters: 23 | stiffness: 42.0 24 | damping: 3.5 25 | action_scale_pos: 0.25 26 | decimation: 10 27 | user_torque_limit: 80 28 | 29 | normalization: 30 | encoder_normalize: True 31 | clip_scales: 32 | clip_observations: 100. 33 | clip_actions: 100. 34 | obs_scales: 35 | lin_vel: 2.0 36 | ang_vel: 0.25 37 | dof_pos: 1.0 38 | dof_vel: 0.05 39 | 40 | size: 41 | actions_size: 6 42 | observations_size: 30 43 | commands_size: 3 44 | obs_history_length: 10 45 | encoder_output_size: 3 46 | 47 | stand_mode: 48 | stand_duration: 1.0 49 | 50 | imu_orientation_offset: 51 | roll: 0.0 52 | pitch: 0.0 53 | yaw: 0.0 54 | 55 | user_cmd_scales: 56 | lin_vel_x: 1.5 57 | lin_vel_y: 1.0 58 | ang_vel_yaw: 0.5 59 | -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441A/policy/encoder.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441A/policy/policy.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441B/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | joint_names: 3 | - "abad_L_Joint" 4 | - "hip_L_Joint" 5 | - "knee_L_Joint" 6 | - "abad_R_Joint" 7 | - "hip_R_Joint" 8 | - "knee_R_Joint" 9 | 10 | init_state: 11 | # target angles [rad] when action = 0.0 12 | default_joint_angle: 13 | abad_L_Joint: 0.0 14 | hip_L_Joint: 0.0 15 | knee_L_Joint: 0.0 16 | 17 | abad_R_Joint: 0.0 18 | hip_R_Joint: 0.0 19 | knee_R_Joint: 0.0 20 | 21 | control: 22 | # PD Drive parameters: 23 | stiffness: 42.0 24 | damping: 3.5 25 | action_scale_pos: 0.25 26 | decimation: 10 27 | user_torque_limit: 80 28 | 29 | normalization: 30 | encoder_normalize: True 31 | clip_scales: 32 | clip_observations: 100. 33 | clip_actions: 100. 34 | obs_scales: 35 | lin_vel: 2.0 36 | ang_vel: 0.25 37 | dof_pos: 1.0 38 | dof_vel: 0.05 39 | 40 | size: 41 | actions_size: 6 42 | observations_size: 30 43 | commands_size: 3 44 | obs_history_length: 10 45 | encoder_output_size: 3 46 | 47 | stand_mode: 48 | stand_duration: 1.0 49 | 50 | imu_orientation_offset: 51 | roll: 0.0 52 | pitch: 0.0 53 | yaw: 0.0 54 | 55 | user_cmd_scales: 56 | lin_vel_x: 1.5 57 | lin_vel_y: 1.0 58 | ang_vel_yaw: 0.5 59 | -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441B/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441B/policy/encoder.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441B/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441B/policy/policy.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441C/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | joint_names: 3 | - "abad_L_Joint" 4 | - "hip_L_Joint" 5 | - "knee_L_Joint" 6 | - "abad_R_Joint" 7 | - "hip_R_Joint" 8 | - "knee_R_Joint" 9 | 10 | init_state: 11 | # target angles [rad] when action = 0.0 12 | default_joint_angle: 13 | abad_L_Joint: 0.0 14 | hip_L_Joint: 0.0 15 | knee_L_Joint: 0.0 16 | 17 | abad_R_Joint: 0.0 18 | hip_R_Joint: 0.0 19 | knee_R_Joint: 0.0 20 | 21 | control: 22 | # PD Drive parameters: 23 | stiffness: 42.0 24 | damping: 3.5 25 | action_scale_pos: 0.25 26 | decimation: 10 27 | user_torque_limit: 80 28 | 29 | normalization: 30 | encoder_normalize: True 31 | clip_scales: 32 | clip_observations: 100. 33 | clip_actions: 100. 34 | obs_scales: 35 | lin_vel: 2.0 36 | ang_vel: 0.25 37 | dof_pos: 1.0 38 | dof_vel: 0.05 39 | 40 | size: 41 | actions_size: 6 42 | observations_size: 30 43 | commands_size: 3 44 | obs_history_length: 10 45 | encoder_output_size: 3 46 | 47 | stand_mode: 48 | stand_duration: 1.0 49 | 50 | imu_orientation_offset: 51 | roll: 0.0 52 | pitch: 0.0 53 | yaw: 0.0 54 | 55 | user_cmd_scales: 56 | lin_vel_x: 1.5 57 | lin_vel_y: 1.0 58 | ang_vel_yaw: 0.5 59 | -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441C/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441C/policy/encoder.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441C/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441C/policy/policy.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441C2/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | joint_names: 3 | - "abad_L_Joint" 4 | - "hip_L_Joint" 5 | - "knee_L_Joint" 6 | - "abad_R_Joint" 7 | - "hip_R_Joint" 8 | - "knee_R_Joint" 9 | 10 | init_state: 11 | # target angles [rad] when action = 0.0 12 | default_joint_angle: 13 | abad_L_Joint: 0.0 14 | hip_L_Joint: 0.0 15 | knee_L_Joint: 0.0 16 | 17 | abad_R_Joint: 0.0 18 | hip_R_Joint: 0.0 19 | knee_R_Joint: 0.0 20 | 21 | control: 22 | # PD Drive parameters: 23 | stiffness: 42.0 24 | damping: 3.5 25 | action_scale_pos: 0.25 26 | decimation: 10 27 | user_torque_limit: 80 28 | 29 | normalization: 30 | encoder_normalize: True 31 | clip_scales: 32 | clip_observations: 100. 33 | clip_actions: 100. 34 | obs_scales: 35 | lin_vel: 2.0 36 | ang_vel: 0.25 37 | dof_pos: 1.0 38 | dof_vel: 0.05 39 | 40 | size: 41 | actions_size: 6 42 | observations_size: 30 43 | commands_size: 3 44 | obs_history_length: 10 45 | encoder_output_size: 3 46 | 47 | stand_mode: 48 | stand_duration: 1.0 49 | 50 | imu_orientation_offset: 51 | roll: 0.0 52 | pitch: 0.0 53 | yaw: 0.0 54 | 55 | user_cmd_scales: 56 | lin_vel_x: 1.5 57 | lin_vel_y: 1.0 58 | ang_vel_yaw: 0.5 59 | -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441C2/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441C2/policy/encoder.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_P441C2/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_P441C2/policy/policy.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_TRON1A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | joint_names: 3 | - "abad_L_Joint" 4 | - "hip_L_Joint" 5 | - "knee_L_Joint" 6 | - "abad_R_Joint" 7 | - "hip_R_Joint" 8 | - "knee_R_Joint" 9 | 10 | init_state: 11 | # target angles [rad] when action = 0.0 12 | default_joint_angle: 13 | abad_L_Joint: 0.0 14 | hip_L_Joint: 0.0 15 | knee_L_Joint: 0.0 16 | 17 | abad_R_Joint: 0.0 18 | hip_R_Joint: 0.0 19 | knee_R_Joint: 0.0 20 | 21 | control: 22 | # PD Drive parameters: 23 | stiffness: 42.0 24 | damping: 3.5 25 | action_scale_pos: 0.25 26 | decimation: 10 27 | user_torque_limit: 80 28 | 29 | normalization: 30 | clip_scales: 31 | clip_observations: 100. 32 | clip_actions: 100. 33 | obs_scales: 34 | lin_vel: 2.0 35 | ang_vel: 0.25 36 | dof_pos: 1.0 37 | dof_vel: 0.05 38 | 39 | size: 40 | actions_size: 6 41 | observations_size: 30 42 | commands_size: 3 43 | obs_history_length: 10 44 | encoder_output_size: 3 45 | 46 | stand_mode: 47 | stand_duration: 1.0 48 | 49 | imu_orientation_offset: 50 | roll: 0.0 51 | pitch: 0.0 52 | yaw: 0.0 53 | 54 | user_cmd_scales: 55 | lin_vel_x: 1.5 56 | lin_vel_y: 1.0 57 | ang_vel_yaw: 0.5 58 | -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_TRON1A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_TRON1A/policy/encoder.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/PF_TRON1A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/PF_TRON1A/policy/policy.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/SF_TRON1A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | joint_names: 3 | - "abad_L_Joint" 4 | - "hip_L_Joint" 5 | - "knee_L_Joint" 6 | - "ankle_L_Joint" 7 | - "abad_R_Joint" 8 | - "hip_R_Joint" 9 | - "knee_R_Joint" 10 | - "ankle_R_Joint" 11 | 12 | init_state: 13 | # target angles [rad] when action = 0.0 14 | default_joint_angle: 15 | abad_L_Joint: 0.0 16 | hip_L_Joint: 0.13 17 | knee_L_Joint: 0.0 18 | ankle_L_Joint: 0.0 19 | 20 | abad_R_Joint: 0.0 21 | hip_R_Joint: -0.13 22 | knee_R_Joint: 0.0 23 | ankle_R_Joint: 0.0 24 | 25 | control: 26 | # PD Drive parameters: 27 | stiffness: 45.0 28 | damping: 3.0 # 1.5 29 | ankle_joint_damping: 1.5 # 0.8 30 | ankle_joint_torque_limit: 20 31 | action_scale_pos: 0.25 32 | decimation: 10 33 | user_torque_limit: 80 34 | 35 | normalization: 36 | clip_scales: 37 | clip_observations: 100. 38 | clip_actions: 100. 39 | obs_scales: 40 | lin_vel: 2.0 41 | ang_vel: 0.25 42 | dof_pos: 1.0 43 | dof_vel: 0.05 44 | 45 | size: 46 | actions_size: 8 47 | observations_size: 36 48 | obs_history_length: 5 49 | encoder_output_size: 3 50 | commands_size: 5 51 | 52 | gait: 53 | frequencies: 1.3 54 | swing_height: 0.12 55 | 56 | stand_mode: 57 | stand_duration: 1.0 58 | 59 | imu_orientation_offset: 60 | roll: 0.0 61 | pitch: 0.0 62 | yaw: 0.0 63 | 64 | user_cmd_scales: 65 | lin_vel_x: 1.0 66 | lin_vel_y: 0.5 67 | ang_vel_yaw: 1.0 -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/SF_TRON1A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/SF_TRON1A/policy/encoder.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/SF_TRON1A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/SF_TRON1A/policy/policy.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/WF_TRON1A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | joint_names: 3 | - "abad_L_Joint" 4 | - "hip_L_Joint" 5 | - "knee_L_Joint" 6 | - "wheel_L_Joint" 7 | - "abad_R_Joint" 8 | - "hip_R_Joint" 9 | - "knee_R_Joint" 10 | - "wheel_R_Joint" 11 | 12 | init_state: 13 | # target angles [rad] when action = 0.0 14 | default_joint_angle: 15 | abad_L_Joint: 0.0 16 | hip_L_Joint: 0.0 17 | knee_L_Joint: 0.0 18 | wheel_L_Joint: 0.0 19 | 20 | abad_R_Joint: 0.0 21 | hip_R_Joint: 0.0 22 | knee_R_Joint: 0.0 23 | wheel_R_Joint: 0.0 24 | 25 | control: 26 | # PD Drive parameters: 27 | stiffness: 42.0 28 | damping: 2.5 29 | wheel_joint_damping: 0.8 30 | wheel_joint_torque_limit: 40 31 | action_scale_pos: 0.25 32 | decimation: 10 33 | user_torque_limit: 80 34 | 35 | normalization: 36 | clip_scales: 37 | clip_observations: 100. 38 | clip_actions: 100. 39 | obs_scales: 40 | lin_vel: 2.0 41 | ang_vel: 0.25 42 | dof_pos: 1.0 43 | dof_vel: 0.05 44 | 45 | size: 46 | actions_size: 8 47 | observations_size: 28 # 31 48 | commands_size: 3 49 | obs_history_length: 10 50 | jointpos_idxs: [0, 1, 2, 4, 5, 6] 51 | encoder_output_size: 3 52 | 53 | stand_mode: 54 | stand_duration: 1.0 55 | 56 | imu_orientation_offset: 57 | roll: 0.0 58 | pitch: 0.0 59 | yaw: 0.0 60 | 61 | user_cmd_scales: 62 | lin_vel_x: 1.5 63 | lin_vel_y: 1.0 64 | ang_vel_yaw: 0.5 65 | -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/WF_TRON1A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/WF_TRON1A/policy/encoder.onnx -------------------------------------------------------------------------------- /robot_controllers/config/pointfoot/WF_TRON1A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-ros-cpp/27b7428509d24061f7485cf87edfda502a9a38ea/robot_controllers/config/pointfoot/WF_TRON1A/policy/policy.onnx -------------------------------------------------------------------------------- /robot_controllers/include/robot_controllers/ControllerBase.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_CONTROLLER_BASE_H_ 6 | #define _LIMX_CONTROLLER_BASE_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace robot_controller { 19 | 20 | // Define scalar and vector types for Eigen 21 | using scalar_t = double; 22 | using vector3_t = Eigen::Matrix; 23 | using vector_t = Eigen::Matrix; 24 | 25 | // Utility class to measure time intervals 26 | class TicToc { 27 | public: 28 | TicToc() { 29 | tic(); 30 | } 31 | 32 | void tic() { 33 | start = std::chrono::system_clock::now(); 34 | } 35 | 36 | double toc() { 37 | end = std::chrono::system_clock::now(); 38 | std::chrono::duration elapsed_seconds = end - start; 39 | return elapsed_seconds.count() * 1000; 40 | } 41 | 42 | private: 43 | std::chrono::time_point start, end; 44 | }; 45 | 46 | // Structure to hold robot configuration settings 47 | struct RobotCfg { 48 | // Control configuration settings 49 | struct ControlCfg { 50 | float stiffness{0.0}; // Stiffness parameter 51 | float damping{0.0}; // Damping parameter 52 | float action_scale_pos{0.0}; // Scaling factor for position action 53 | float action_scale_vel{0.0}; // Scaling factor for velocity action 54 | int decimation{0}; // Decimation factor 55 | float user_torque_limit{0.0}; // User-defined torque limit 56 | 57 | // Print control configuration settings 58 | void print() { 59 | ROS_INFO_STREAM("=======start ObsScales========"); 60 | ROS_INFO_STREAM("stiffness: " << stiffness); 61 | ROS_INFO_STREAM("damping: " << damping); 62 | ROS_INFO_STREAM("action_scale_pos: " << action_scale_pos); 63 | ROS_INFO_STREAM("action_scale_vel: " << action_scale_vel); 64 | ROS_INFO_STREAM("decimation: " << decimation); 65 | ROS_INFO_STREAM("=======end ObsScales========\n"); 66 | } 67 | }; 68 | 69 | // Reinforcement learning configuration settings 70 | struct RlCfg { 71 | // Observation scaling parameters 72 | struct ObsScales { 73 | scalar_t linVel{0.0}; // Linear velocity scaling 74 | scalar_t angVel{0.0}; // Angular velocity scaling 75 | scalar_t dofPos{0.0}; // Degree of freedom position scaling 76 | scalar_t dofVel{0.0}; // Degree of freedom velocity scaling 77 | 78 | // Print observation scaling parameters 79 | void print() { 80 | ROS_INFO_STREAM("=======start ObsScales========"); 81 | ROS_INFO_STREAM("linVel: " << linVel); 82 | ROS_INFO_STREAM("angVel: " << angVel); 83 | ROS_INFO_STREAM("dofPos: " << dofPos); 84 | ROS_INFO_STREAM("dofVel: " << dofVel); 85 | ROS_INFO_STREAM("=======end ObsScales========\n"); 86 | } 87 | }; 88 | 89 | scalar_t clipActions{0.0}; // Action clipping parameter 90 | scalar_t clipObs{0.0}; // Observation clipping parameter 91 | ObsScales obsScales; // Observation scaling settings 92 | }; 93 | 94 | // User command configuration settings 95 | struct UserCmdCfg { 96 | double linVel_x{0.0}; 97 | double linVel_y{0.0}; 98 | double angVel_yaw{0.0}; 99 | 100 | // Print user command scaling parameters 101 | void print() { 102 | ROS_INFO_STREAM("=======Start User Cmd Scales========"); 103 | ROS_INFO_STREAM("lin_vel_x: " << linVel_x); 104 | ROS_INFO_STREAM("lin_vel_y: " << linVel_y); 105 | ROS_INFO_STREAM("ang_vel_yaw: " << angVel_yaw); 106 | ROS_INFO_STREAM("=======End User Cmd Scales========\n"); 107 | } 108 | }; 109 | 110 | // gait settings 111 | struct GaitCfg 112 | { 113 | float frequencies; 114 | float swing_height; 115 | void print(const char *tag) 116 | { 117 | ROS_INFO_STREAM("=======Start GaitCfg========"); 118 | ROS_INFO_STREAM("frequencies: " << frequencies); 119 | ROS_INFO_STREAM("swing_height: " << swing_height); 120 | ROS_INFO_STREAM("=======end GaitCfg========\n"); 121 | } 122 | }; 123 | 124 | RlCfg rlCfg; // RL configuration settings 125 | UserCmdCfg userCmdCfg; // User command configuration settings 126 | GaitCfg gaitCfg; 127 | std::map initState; // Initial state settings 128 | ControlCfg controlCfg; // Control configuration settings 129 | 130 | // Print robot configuration settings 131 | void print() { 132 | rlCfg.obsScales.print(); 133 | controlCfg.print(); 134 | userCmdCfg.print(); 135 | ROS_INFO_STREAM("clipActions: " << rlCfg.clipActions); 136 | ROS_INFO_STREAM("clipObs: " << rlCfg.clipObs); 137 | } 138 | }; 139 | 140 | // Base class for controllers 141 | class ControllerBase 142 | : public controller_interface::MultiInterfaceController { 144 | public: 145 | enum class Mode : uint8_t; // Enumeration for controller modes 146 | 147 | ControllerBase() = default; // Default constructor 148 | 149 | virtual ~ControllerBase() = default; // Virtual destructor 150 | 151 | // Initialize the controller 152 | virtual bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh); 153 | 154 | // Perform actions when the controller starts 155 | virtual void starting(const ros::Time &time) {} 156 | 157 | // Update the controller 158 | virtual void update(const ros::Time &time, const ros::Duration &period) {} 159 | 160 | // Load the model for the controller 161 | virtual bool loadModel() { return false; } 162 | 163 | // Load RL configuration settings 164 | virtual bool loadRLCfg() { return false; } 165 | 166 | // Compute encoder for the controller 167 | virtual void computeEncoder() {} 168 | 169 | // Compute actions for the controller 170 | virtual void computeActions() {} 171 | 172 | // Compute observations for the controller 173 | virtual void computeObservation() {} 174 | 175 | // Display joint information 176 | virtual void showJointInfo() {} 177 | 178 | // Handle stand mode 179 | virtual void handleStandMode() {} 180 | 181 | // Handle walk mode 182 | virtual void handleWalkMode() {} 183 | 184 | protected: 185 | // Callback function for velocity commands 186 | virtual void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg){}; 187 | 188 | // Get the robot configuration 189 | virtual RobotCfg &getRobotCfg() { return robotCfg_; } 190 | 191 | scalar_t loopFrequency_; //Control Frequency 192 | 193 | Mode mode_; // Controller mode 194 | int64_t loopCount_; // Loop count 195 | vector3_t commands_; // Command vector 196 | vector3_t scaled_commands_; // Scaled command vector 197 | RobotCfg robotCfg_{}; // Robot configuration 198 | 199 | std::vector jointNames_; // Joint names 200 | vector_t measuredRbdState_; // Measured RBD state 201 | // Hardware interface 202 | std::vector hybridJointHandles_; // Hybrid joint handles 203 | hardware_interface::ImuSensorHandle imuSensorHandles_; // IMU sensor handles 204 | std::vector contactHandles_; // Contact sensor handles 205 | 206 | vector_t defaultJointAngles_; // Default joint angles 207 | vector_t initJointAngles_; // Initial joint angles in standard standing pose 208 | 209 | scalar_t standPercent_; // Standing percent 210 | scalar_t standDuration_; // Standing duration 211 | 212 | ros::NodeHandle nh_; // ROS node handle 213 | ros::Subscriber cmdVelSub_; // Command velocity subscriber 214 | 215 | std::string robot_type_; // Type of the robot (e.g., point foot, wheel foot, sole foot) 216 | bool is_point_foot_{false}; // Indicates if the robot has a point foot configuration 217 | bool is_wheel_foot_{false}; // Indicates if the robot has a wheel foot configuration 218 | bool is_sole_foot_{false}; // Indicates if the robot has a sole foot configuration 219 | }; 220 | 221 | // Function to compute square of a value 222 | template 223 | T square(T a) { 224 | return a * a; 225 | } 226 | 227 | // Function to convert quaternion to ZYX Euler angles 228 | template 229 | Eigen::Matrix quatToZyx(const Eigen::Quaternion &q) { 230 | Eigen::Matrix zyx; 231 | 232 | SCALAR_T as = std::min(-2. * (q.x() * q.z() - q.w() * q.y()), .99999); 233 | zyx(0) = std::atan2(2 * (q.x() * q.y() + q.w() * q.z()), square(q.w()) + square(q.x()) - square(q.y()) - square(q.z())); 234 | zyx(1) = std::asin(as); 235 | zyx(2) = std::atan2(2 * (q.y() * q.z() + q.w() * q.x()), square(q.w()) - square(q.x()) - square(q.y()) + square(q.z())); 236 | return zyx; 237 | } 238 | 239 | // Function to compute rotation matrix from ZYX Euler angles 240 | template 241 | Eigen::Matrix getRotationMatrixFromZyxEulerAngles(const Eigen::Matrix &eulerAngles) { 242 | const SCALAR_T z = eulerAngles(0); 243 | const SCALAR_T y = eulerAngles(1); 244 | const SCALAR_T x = eulerAngles(2); 245 | 246 | const SCALAR_T c1 = cos(z); 247 | const SCALAR_T c2 = cos(y); 248 | const SCALAR_T c3 = cos(x); 249 | const SCALAR_T s1 = sin(z); 250 | const SCALAR_T s2 = sin(y); 251 | const SCALAR_T s3 = sin(x); 252 | 253 | const SCALAR_T s2s3 = s2 * s3; 254 | const SCALAR_T s2c3 = s2 * c3; 255 | 256 | // Construct rotation matrix 257 | Eigen::Matrix rotationMatrix; 258 | rotationMatrix << c1 * c2, c1 * s2s3 - s1 * c3, c1 * s2c3 + s1 * s3, 259 | s1 * c2, s1 * s2s3 + c1 * c3, s1 * s2c3 - c1 * s3, 260 | -s2, c2 * s3, c2 * c3; 261 | return rotationMatrix; 262 | } 263 | } // namespace 264 | 265 | #endif //_LIMX_CONTROLLER_BASE_H_ -------------------------------------------------------------------------------- /robot_controllers/include/robot_controllers/PointfootController.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_POINTFOOT_CONTROLLER_H_ 6 | #define _LIMX_POINTFOOT_CONTROLLER_H_ 7 | 8 | #include "ros/ros.h" 9 | #include "robot_controllers/ControllerBase.h" 10 | #include "limxsdk/pointfoot.h" 11 | 12 | namespace robot_controller { 13 | // Struct for holding configuration settings for a biped robot 14 | struct BipedRobotCfg : public RobotCfg {}; 15 | 16 | // Class for controlling a biped robot with point foot 17 | class PointfootController : public ControllerBase { 18 | using tensor_element_t = float; // Type alias for tensor elements 19 | using matrix_t = Eigen::Matrix; // Type alias for matrices 20 | 21 | public: 22 | PointfootController() = default; // Default constructor 23 | 24 | ~PointfootController() override = default; // Destructor 25 | 26 | // Enumeration for controller modes 27 | enum class Mode : uint8_t { 28 | STAND, // Stand mode 29 | WALK, // Walk mode 30 | }; 31 | 32 | // Initialize the controller 33 | bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh) override; 34 | 35 | // Perform actions when the controller starts 36 | void starting(const ros::Time &time) override; 37 | 38 | // Update the controller 39 | void update(const ros::Time &time, const ros::Duration &period) override; 40 | 41 | protected: 42 | // Load the model for the controller 43 | bool loadModel() override; 44 | 45 | // Load RL configuration settings 46 | bool loadRLCfg() override; 47 | 48 | // Compute observations for the controller 49 | void computeObservation() override; 50 | 51 | // Compute encoder for the controller 52 | void computeEncoder() override; 53 | 54 | // Compute actions for the controller 55 | void computeActions() override; 56 | 57 | // Handle walk mode 58 | void handleWalkMode() override; 59 | 60 | // Handle stand mode 61 | void handleStandMode() override; 62 | 63 | // Callback function for command velocity 64 | void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg) override; 65 | 66 | // Get the robot configuration 67 | RobotCfg &getRobotCfg() override { return robotCfg_; } 68 | 69 | BipedRobotCfg robotCfg_; // Biped robot configuration 70 | 71 | Mode mode_; // Controller mode 72 | 73 | private: 74 | // File path for policy model 75 | std::string policyFilePath_; 76 | 77 | std::shared_ptr onnxEnvPrt_; // Shared pointer to ONNX environment 78 | 79 | // ONNX session pointers 80 | std::unique_ptr policySessionPtr_; 81 | std::unique_ptr encoderSessionPtr_; 82 | 83 | // Names and shapes of inputs and outputs for ONNX sessions 84 | std::vector> policyInputShapes_; 85 | std::vector> policyOutputShapes_; 86 | std::vector policyInputNames_; 87 | std::vector policyOutputNames_; 88 | 89 | std::vector> encoderInputShapes_; 90 | std::vector> encoderOutputShapes_; 91 | std::vector encoderInputNames_; 92 | std::vector encoderOutputNames_; 93 | 94 | std::vector proprioHistoryVector_; 95 | Eigen::Matrix proprioHistoryBuffer_; 96 | 97 | vector3_t baseLinVel_; // Base linear velocity 98 | vector3_t basePosition_; // Base position 99 | vector_t lastActions_; // Last actions 100 | 101 | int actionsSize_; // Size of actions 102 | int observationSize_; // Size of observations 103 | int obsHistoryLength_; // Size of history observations 104 | int encoderInputSize_, encoderOutputSize_; // Input and output size of encoder 105 | float imu_orientation_offset[3]; // IMU orientation offset 106 | std::vector jointPosIdxs_; 107 | std::vector actions_; // Actions 108 | std::vector observations_; // Observations 109 | std::vector encoderOut_; // Encoder 110 | 111 | double gait_index_{0.0}; 112 | bool isfirstRecObs_{true}; 113 | }; 114 | 115 | } // namespace robot_controller 116 | 117 | #endif //_LIMX_POINTFOOT_CONTROLLER_H_ 118 | -------------------------------------------------------------------------------- /robot_controllers/include/robot_controllers/SolefootController.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_SOLEFOOT_CONTROLLER_H_ 6 | #define _LIMX_SOLEFOOT_CONTROLLER_H_ 7 | 8 | #include "ros/ros.h" 9 | #include "robot_controllers/ControllerBase.h" 10 | #include "limxsdk/pointfoot.h" 11 | 12 | namespace robot_controller { 13 | // Struct for holding configuration settings for a biped robot 14 | struct SoleBipedRobotCfg : public RobotCfg {}; 15 | 16 | // Class for controlling a biped robot with point foot 17 | class SolefootController : public ControllerBase { 18 | using tensor_element_t = float; // Type alias for tensor elements 19 | using matrix_t = Eigen::Matrix; // Type alias for matrices 20 | 21 | public: 22 | SolefootController() = default; // Default constructor 23 | 24 | ~SolefootController() override = default; // Destructor 25 | 26 | // Enumeration for controller modes 27 | enum class Mode : uint8_t { 28 | STAND, // Stand mode 29 | WALK, // Walk mode 30 | }; 31 | 32 | // Initialize the controller 33 | bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh) override; 34 | 35 | // Perform actions when the controller starts 36 | void starting(const ros::Time &time) override; 37 | 38 | // Update the controller 39 | void update(const ros::Time &time, const ros::Duration &period) override; 40 | 41 | protected: 42 | // Load the model for the controller 43 | bool loadModel() override; 44 | 45 | // Load RL configuration settings 46 | bool loadRLCfg() override; 47 | 48 | // Compute actions for the controller 49 | void computeActions() override; 50 | 51 | // Compute observations for the controller 52 | void computeObservation() override; 53 | 54 | // Compute encoder for the controller 55 | void computeEncoder() override; 56 | 57 | // Handle walk mode 58 | void handleWalkMode() override; 59 | 60 | // Handle stand mode 61 | void handleStandMode() override; 62 | 63 | // Callback function for command velocity 64 | void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg) override; 65 | 66 | // compute gait 67 | vector_t handleGait(); 68 | 69 | // compute gait clock 70 | vector_t handleGaitClock(vector_t &curr_gait); 71 | 72 | // Get the robot configuration 73 | RobotCfg &getRobotCfg() override { return robotCfg_; } 74 | 75 | SoleBipedRobotCfg robotCfg_; // Biped robot configuration 76 | 77 | Mode mode_; // Controller mode 78 | 79 | private: 80 | // File path for policy model 81 | std::string policyFilePath_; 82 | 83 | std::shared_ptr onnxEnvPrt_; // Shared pointer to ONNX environment 84 | 85 | // ONNX session pointers 86 | std::unique_ptr policySessionPtr_; 87 | std::unique_ptr encoderSessionPtr_; 88 | 89 | // Names and shapes of inputs and outputs for ONNX sessions 90 | std::vector> policyInputShapes_; 91 | std::vector> policyOutputShapes_; 92 | std::vector policyInputNames_; 93 | std::vector policyOutputNames_; 94 | 95 | std::vector> encoderInputShapes_; 96 | std::vector> encoderOutputShapes_; 97 | std::vector encoderInputNames_; 98 | std::vector encoderOutputNames_; 99 | 100 | std::vector proprioHistoryVector_; 101 | Eigen::Matrix proprioHistoryBuffer_; 102 | 103 | vector3_t baseLinVel_; // Base linear velocity 104 | vector3_t basePosition_; // Base position 105 | vector_t lastActions_; // Last actions 106 | int commandSizeSolefoot_; 107 | vector_t commandSolefoot_; // command for solefoot(size=5) 108 | vector_t scaledCommandSolefoot_; // scaled command for solefoot(size=5) 109 | 110 | int actionsSize_; // Size of actions 111 | int observationSize_; // Size of observations 112 | int obsHistoryLength_; // Size of history observations 113 | int encoderInputSize_, encoderOutputSize_; // Input and output size of encoder 114 | float imu_orientation_offset[3]; // IMU orientation offset 115 | float ankleJointDamping_, ankleJointTorqueLimit_; 116 | std::vector jointPosIdxs_; 117 | std::vector actions_; // Actions 118 | std::vector observations_; // Observations 119 | std::vector encoderOut_; // Encoder 120 | 121 | double gait_index_{0.0}; 122 | bool isfirstRecObs_{true}; 123 | }; 124 | 125 | } // namespace robot_controller 126 | 127 | #endif //_LIMX_SOLEFOOT_CONTROLLER_H_ 128 | -------------------------------------------------------------------------------- /robot_controllers/include/robot_controllers/WheelfootController.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_WHEELFOOT_CONTROLLER_H_ 6 | #define _LIMX_WHEELFOOT_CONTROLLER_H_ 7 | 8 | #include "ros/ros.h" 9 | #include "robot_controllers/ControllerBase.h" 10 | #include "limxsdk/pointfoot.h" 11 | 12 | namespace robot_controller { 13 | // Struct for holding configuration settings for a biped robot 14 | struct WheelBipedRobotCfg : public RobotCfg {}; 15 | 16 | // Class for controlling a biped robot with point foot 17 | class WheelfootController : public ControllerBase { 18 | using tensor_element_t = float; // Type alias for tensor elements 19 | using matrix_t = Eigen::Matrix; // Type alias for matrices 20 | 21 | public: 22 | WheelfootController() = default; // Default constructor 23 | 24 | ~WheelfootController() override = default; // Destructor 25 | 26 | // Enumeration for controller modes 27 | enum class Mode : uint8_t { 28 | STAND, // Stand mode 29 | WALK, // Walk mode 30 | }; 31 | 32 | // Initialize the controller 33 | bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh) override; 34 | 35 | // Perform actions when the controller starts 36 | void starting(const ros::Time &time) override; 37 | 38 | // Update the controller 39 | void update(const ros::Time &time, const ros::Duration &period) override; 40 | 41 | protected: 42 | // Load the model for the controller 43 | bool loadModel() override; 44 | 45 | // Load RL configuration settings 46 | bool loadRLCfg() override; 47 | 48 | // Compute observations for the controller 49 | void computeObservation() override; 50 | 51 | // Compute encoder for the controller 52 | void computeEncoder() override; 53 | 54 | // Compute actions for the controller 55 | void computeActions() override; 56 | 57 | // Handle walk mode 58 | void handleWalkMode() override; 59 | 60 | // Handle stand mode 61 | void handleStandMode() override; 62 | 63 | // Callback function for command velocity 64 | void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg) override; 65 | 66 | // Get the robot configuration 67 | RobotCfg &getRobotCfg() override { return robotCfg_; } 68 | 69 | WheelBipedRobotCfg robotCfg_; // Biped robot configuration 70 | 71 | Mode mode_; // Controller mode 72 | 73 | private: 74 | // File path for policy model 75 | std::string policyFilePath_; 76 | 77 | std::shared_ptr onnxEnvPrt_; // Shared pointer to ONNX environment 78 | 79 | // ONNX session pointers 80 | std::unique_ptr policySessionPtr_; 81 | std::unique_ptr encoderSessionPtr_; 82 | 83 | // Names and shapes of inputs and outputs for ONNX sessions 84 | std::vector> policyInputShapes_; 85 | std::vector> policyOutputShapes_; 86 | std::vector policyInputNames_; 87 | std::vector policyOutputNames_; 88 | 89 | std::vector> encoderInputShapes_; 90 | std::vector> encoderOutputShapes_; 91 | std::vector encoderInputNames_; 92 | std::vector encoderOutputNames_; 93 | 94 | std::vector proprioHistoryVector_; 95 | Eigen::Matrix proprioHistoryBuffer_; 96 | 97 | vector3_t baseLinVel_; // Base linear velocity 98 | vector3_t basePosition_; // Base position 99 | vector_t lastActions_; // Last actions 100 | 101 | int actionsSize_; // Size of actions 102 | int observationSize_; // Size of observations 103 | int obsHistoryLength_; // Size of history observations 104 | int encoderInputSize_, encoderOutputSize_; // Input and output size of encoder 105 | float imu_orientation_offset[3]; // IMU orientation offset 106 | float wheelJointDamping_, wheelJointTorqueLimit_; 107 | std::vector jointPosIdxs_; 108 | std::vector actions_; // Actions 109 | std::vector observations_; // Observations 110 | std::vector encoderOut_; // Encoder 111 | 112 | double gait_index_{0.0}; 113 | bool isfirstRecObs_{true}; 114 | }; 115 | 116 | } // namespace robot_controller 117 | 118 | #endif //_LIMX_WHEELFOOT_CONTROLLER_H_ 119 | -------------------------------------------------------------------------------- /robot_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_controllers 4 | 0.0.1 5 | The robot_controllers packages. 6 | max 7 | TODO 8 | max 9 | catkin 10 | roscpp 11 | controller_interface 12 | angles 13 | pointfoot_sdk_lowlevel 14 | robot_common 15 | robot_description 16 | pluginlib 17 | pluginlib 18 | controller_manager 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /robot_controllers/robot_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | The pointfoot controller. 6 | 7 | 8 | 9 | 11 | 12 | The wheelfoot controller. 13 | 14 | 15 | 16 | 18 | 19 | The solefoot controller. 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /robot_controllers/src/ControllerBase.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #include "robot_controllers/ControllerBase.h" 6 | #include "pluginlib/class_list_macros.hpp" 7 | #include 8 | 9 | namespace robot_controller { 10 | 11 | // Initializes the controller. 12 | bool ControllerBase::init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh) { 13 | // Retrieve the robot type from the environment variable "ROBOT_TYPE" 14 | const char* value = ::getenv("ROBOT_TYPE"); 15 | if (value && strlen(value) > 0) { 16 | robot_type_ = std::string(value); 17 | } else { 18 | ROS_ERROR("Error: Please set the ROBOT_TYPE using 'export ROBOT_TYPE='."); 19 | abort(); 20 | } 21 | 22 | // Determine the specific robot configuration based on the robot type 23 | is_point_foot_ = (robot_type_.find("PF") != std::string::npos); 24 | is_wheel_foot_ = (robot_type_.find("WF") != std::string::npos); 25 | is_sole_foot_ = (robot_type_.find("SF") != std::string::npos); 26 | 27 | if (!loadRLCfg()) { 28 | ROS_ERROR("Error in loadRLCfg"); 29 | } 30 | if (!loadModel()) { 31 | ROS_ERROR("Error in loadModel"); 32 | } 33 | 34 | auto &initState = getRobotCfg().initState; 35 | 36 | // Hardware interface 37 | auto *hybridJointInterface = robot_hw->get(); 38 | 39 | initJointAngles_.resize(initState.size()); 40 | // Note: The order of joint angles below should match the order during training. 41 | std::string jointNames; 42 | for (size_t i = 0; i < jointNames_.size(); i++) { 43 | hybridJointHandles_.push_back(hybridJointInterface->getHandle(jointNames_[i])); 44 | initJointAngles_(i) = initState[jointNames_[i]]; 45 | jointNames += jointNames_[i]; 46 | if (i != jointNames_.size() -1) { 47 | jointNames += ", "; 48 | } 49 | } 50 | 51 | ROS_INFO("hybridJointHandles jointNames: [%s]", jointNames.c_str()); 52 | 53 | defaultJointAngles_.resize(hybridJointHandles_.size()); 54 | 55 | imuSensorHandles_ = robot_hw->get()->getHandle("limx_imu"); 56 | 57 | auto *contactInterface = robot_hw->get(); 58 | 59 | // Subscribe to command velocity topic. 60 | cmdVelSub_ = nh_.subscribe("/cmd_vel", 1, &ControllerBase::cmdVelCallback, this); 61 | return true; 62 | } 63 | 64 | } // namespace robot_controller 65 | 66 | PLUGINLIB_EXPORT_CLASS(robot_controller::ControllerBase, controller_interface::ControllerBase) 67 | -------------------------------------------------------------------------------- /robot_controllers/src/PointfootController.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #include "robot_controllers/PointfootController.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace robot_controller { 13 | 14 | // Initialize the controller 15 | bool PointfootController::init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh) { 16 | return ControllerBase::init(robot_hw, nh); 17 | } 18 | 19 | // Perform initialization when the controller starts 20 | void PointfootController::starting(const ros::Time &time) { 21 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 22 | ROS_INFO_STREAM("starting hybridJointHandle: " << hybridJointHandles_[i].getPosition()); 23 | defaultJointAngles_[i] = hybridJointHandles_[i].getPosition(); 24 | } 25 | 26 | standPercent_ += 1 / (standDuration_ * loopFrequency_); 27 | 28 | loopCount_ = 0; 29 | 30 | mode_ = Mode::STAND; 31 | } 32 | 33 | // Update function called periodically 34 | void PointfootController::update(const ros::Time &time, const ros::Duration &period) { 35 | switch (mode_) { 36 | case Mode::STAND: 37 | handleStandMode(); 38 | break; 39 | case Mode::WALK: 40 | handleWalkMode(); 41 | break; 42 | } 43 | 44 | loopCount_++; 45 | } 46 | 47 | // Handle walking mode 48 | void PointfootController::handleWalkMode() { 49 | TicToc dida; 50 | // Compute observation & actions 51 | if (robotCfg_.controlCfg.decimation == 0) { 52 | ROS_ERROR("Error robotCfg_.controlCfg.decimation"); 53 | return; 54 | } 55 | if (loopCount_ % robotCfg_.controlCfg.decimation == 0) { 56 | computeObservation(); 57 | computeEncoder(); 58 | computeActions(); 59 | 60 | // Limit action range 61 | scalar_t actionMin = -robotCfg_.rlCfg.clipActions; 62 | scalar_t actionMax = robotCfg_.rlCfg.clipActions; 63 | std::transform(actions_.begin(), actions_.end(), actions_.begin(), 64 | [actionMin, actionMax](scalar_t x) { return std::max(actionMin, std::min(actionMax, x)); }); 65 | } 66 | 67 | // Set action 68 | vector_t jointPos(hybridJointHandles_.size()), jointVel(hybridJointHandles_.size()); 69 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 70 | jointPos(i) = hybridJointHandles_[i].getPosition(); 71 | jointVel(i) = hybridJointHandles_[i].getVelocity(); 72 | } 73 | // in wheel mode, wheel joint actions differ from others 74 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 75 | scalar_t actionMin = 76 | jointPos(i) - initJointAngles_(i, 0) + 77 | (robotCfg_.controlCfg.damping * jointVel(i) - robotCfg_.controlCfg.user_torque_limit) / robotCfg_.controlCfg.stiffness; 78 | scalar_t actionMax = 79 | jointPos(i) - initJointAngles_(i, 0) + 80 | (robotCfg_.controlCfg.damping * jointVel(i) + robotCfg_.controlCfg.user_torque_limit) / robotCfg_.controlCfg.stiffness; 81 | actions_[i] = std::max(actionMin / robotCfg_.controlCfg.action_scale_pos, 82 | std::min(actionMax / robotCfg_.controlCfg.action_scale_pos, (scalar_t)actions_[i])); 83 | scalar_t pos_des = actions_[i] * robotCfg_.controlCfg.action_scale_pos + initJointAngles_(i, 0); 84 | hybridJointHandles_[i].setCommand(pos_des, 0, robotCfg_.controlCfg.stiffness, robotCfg_.controlCfg.damping, 85 | 0, 2); 86 | lastActions_(i, 0) = actions_[i]; 87 | } 88 | } 89 | 90 | // Handle standing mode 91 | void PointfootController::handleStandMode() { 92 | if (standPercent_ < 1) { 93 | for (int j = 0; j < hybridJointHandles_.size(); j++) { 94 | scalar_t pos_des = defaultJointAngles_[j] * (1 - standPercent_) + initJointAngles_[j] * standPercent_; 95 | hybridJointHandles_[j].setCommand(pos_des, 0, robotCfg_.controlCfg.stiffness, 96 | robotCfg_.controlCfg.damping, 0, 2); 97 | } 98 | standPercent_ += 1 / (standDuration_ * loopFrequency_); 99 | } else { 100 | mode_ = Mode::WALK; 101 | } 102 | } 103 | 104 | bool PointfootController::loadModel() { 105 | // Load ONNX models for policy, encoder, and gait generator. 106 | 107 | std::string policyModelPath; 108 | if (!nh_.getParam("/policyFile", policyModelPath)) { 109 | ROS_ERROR("Failed to retrieve policy path from the parameter server!"); 110 | return false; 111 | } 112 | 113 | std::string encoderModelPath; 114 | if (!nh_.getParam("/encoderFile", encoderModelPath)) { 115 | ROS_ERROR("Failed to retrieve encoder path from the parameter server!"); 116 | return false; 117 | } 118 | 119 | // Create ONNX environment 120 | onnxEnvPrt_.reset(new Ort::Env(ORT_LOGGING_LEVEL_WARNING, "PointFootOnnxController")); 121 | 122 | // Create session options 123 | Ort::SessionOptions sessionOptions; 124 | sessionOptions.SetIntraOpNumThreads(1); 125 | sessionOptions.SetInterOpNumThreads(1); 126 | 127 | Ort::AllocatorWithDefaultOptions allocator; 128 | 129 | // Policy session 130 | ROS_INFO("Loading policy from: %s", policyModelPath.c_str()); 131 | policySessionPtr_ = std::make_unique(*onnxEnvPrt_, policyModelPath.c_str(), sessionOptions); 132 | policyInputNames_.clear(); 133 | policyOutputNames_.clear(); 134 | policyInputShapes_.clear(); 135 | policyOutputShapes_.clear(); 136 | for (size_t i = 0; i < policySessionPtr_->GetInputCount(); i++) { 137 | policyInputNames_.push_back(policySessionPtr_->GetInputName(i, allocator)); 138 | policyInputShapes_.push_back(policySessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 139 | ROS_INFO("GetInputName: %s", policySessionPtr_->GetInputName(i, allocator)); 140 | std::vector shape = policySessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 141 | std::string shapeString; 142 | for (size_t j = 0; j < shape.size(); ++j) { 143 | shapeString += std::to_string(shape[j]); 144 | if (j != shape.size() - 1) { 145 | shapeString += ", "; 146 | } 147 | } 148 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 149 | } 150 | for (size_t i = 0; i < policySessionPtr_->GetOutputCount(); i++) { 151 | policyOutputNames_.push_back(policySessionPtr_->GetOutputName(i, allocator)); 152 | ROS_INFO("GetOutputName: %s", policySessionPtr_->GetOutputName(i, allocator)); 153 | policyOutputShapes_.push_back(policySessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 154 | std::vector shape = policySessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 155 | std::string shapeString; 156 | for (size_t j = 0; j < shape.size(); ++j) { 157 | shapeString += std::to_string(shape[j]); 158 | if (j != shape.size() - 1) { 159 | shapeString += ", "; 160 | } 161 | } 162 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 163 | } 164 | 165 | // Encoder session 166 | ROS_INFO("Loading encoder from: %s", encoderModelPath.c_str()); 167 | encoderSessionPtr_ = std::make_unique(*onnxEnvPrt_, encoderModelPath.c_str(), sessionOptions); 168 | encoderInputNames_.clear(); 169 | encoderOutputNames_.clear(); 170 | encoderInputShapes_.clear(); 171 | encoderOutputShapes_.clear(); 172 | for (size_t i = 0; i < encoderSessionPtr_->GetInputCount(); i++) { 173 | encoderInputNames_.push_back(encoderSessionPtr_->GetInputName(i, allocator)); 174 | encoderInputShapes_.push_back(encoderSessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 175 | ROS_INFO("GetInputName: %s", encoderSessionPtr_->GetInputName(i, allocator)); 176 | std::vector shape = encoderSessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 177 | std::string shapeString; 178 | for (size_t j = 0; j < shape.size(); ++j) { 179 | shapeString += std::to_string(shape[j]); 180 | if (j != shape.size() - 1) { 181 | shapeString += ", "; 182 | } 183 | } 184 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 185 | } 186 | for (size_t i = 0; i < encoderSessionPtr_->GetOutputCount(); i++) { 187 | encoderOutputNames_.push_back(encoderSessionPtr_->GetOutputName(i, allocator)); 188 | ROS_INFO("GetOutputName: %s", encoderSessionPtr_->GetOutputName(i, allocator)); 189 | encoderOutputShapes_.push_back(encoderSessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 190 | std::vector shape = encoderSessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 191 | std::string shapeString; 192 | for (size_t j = 0; j < shape.size(); ++j) { 193 | shapeString += std::to_string(shape[j]); 194 | if (j != shape.size() - 1) { 195 | shapeString += ", "; 196 | } 197 | } 198 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 199 | } 200 | 201 | ROS_INFO("Successfully loaded ONNX models!"); 202 | return true; 203 | } 204 | 205 | // Loads the reinforcement learning configuration. 206 | bool PointfootController::loadRLCfg() { 207 | auto &initState = robotCfg_.initState; 208 | BipedRobotCfg::ControlCfg &controlCfg = robotCfg_.controlCfg; 209 | BipedRobotCfg::RlCfg::ObsScales &obsScales = robotCfg_.rlCfg.obsScales; 210 | 211 | try { 212 | // Load parameters from ROS parameter server. 213 | int error = 0; 214 | error += static_cast(!nh_.getParam("/PointfootCfg/joint_names", jointNames_)); 215 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/abad_L_Joint", initState["abad_L_Joint"])); 216 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/hip_L_Joint", initState["hip_L_Joint"])); 217 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/knee_L_Joint", initState["knee_L_Joint"])); 218 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/abad_R_Joint", initState["abad_R_Joint"])); 219 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/hip_R_Joint", initState["hip_R_Joint"])); 220 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/knee_R_Joint", initState["knee_R_Joint"])); 221 | error += static_cast(!nh_.getParam("/PointfootCfg/stand_mode/stand_duration", standDuration_)); 222 | error += static_cast(!nh_.getParam("/robot_hw/loop_frequency", loopFrequency_)); 223 | error += static_cast(!nh_.getParam("/PointfootCfg/control/stiffness", controlCfg.stiffness)); 224 | error += static_cast(!nh_.getParam("/PointfootCfg/control/damping", controlCfg.damping)); 225 | error += static_cast(!nh_.getParam("/PointfootCfg/control/action_scale_pos", controlCfg.action_scale_pos)); 226 | error += static_cast(!nh_.getParam("/PointfootCfg/control/decimation", controlCfg.decimation)); 227 | error += static_cast(!nh_.getParam("/PointfootCfg/control/user_torque_limit", controlCfg.user_torque_limit)); 228 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/clip_scales/clip_observations", robotCfg_.rlCfg.clipObs)); 229 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/clip_scales/clip_actions", robotCfg_.rlCfg.clipActions)); 230 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/lin_vel", obsScales.linVel)); 231 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/ang_vel", obsScales.angVel)); 232 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/dof_pos", obsScales.dofPos)); 233 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/dof_vel", obsScales.dofVel)); 234 | error += static_cast(!nh_.getParam("/PointfootCfg/size/actions_size", actionsSize_)); 235 | error += static_cast(!nh_.getParam("/PointfootCfg/size/observations_size", observationSize_)); 236 | error += static_cast(!nh_.getParam("/PointfootCfg/size/obs_history_length", obsHistoryLength_)); 237 | error += static_cast(!nh_.getParam("/PointfootCfg/size/encoder_output_size", encoderOutputSize_)); 238 | 239 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/yaw", imu_orientation_offset[0])); 240 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/pitch", imu_orientation_offset[1])); 241 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/roll", imu_orientation_offset[2])); 242 | 243 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/lin_vel_x", robotCfg_.userCmdCfg.linVel_x)); 244 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/lin_vel_y", robotCfg_.userCmdCfg.linVel_y)); 245 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/ang_vel_yaw", robotCfg_.userCmdCfg.angVel_yaw)); 246 | 247 | if (error) { 248 | ROS_ERROR("Load parameters from ROS parameter server error!!!"); 249 | } 250 | if (standDuration_ <= 0.5) { 251 | ROS_ERROR("standDuration_ must be larger than 0.5!!!"); 252 | } 253 | 254 | encoderInputSize_ = obsHistoryLength_ * observationSize_; 255 | robotCfg_.print(); 256 | 257 | // Resize vectors. 258 | actions_.resize(actionsSize_); 259 | observations_.resize(observationSize_); 260 | proprioHistoryVector_.resize(observationSize_ * obsHistoryLength_); 261 | encoderOut_.resize(encoderOutputSize_); 262 | lastActions_.resize(actionsSize_); 263 | 264 | // Initialize vectors. 265 | lastActions_.setZero(); 266 | commands_.setZero(); 267 | scaled_commands_.setZero(); 268 | baseLinVel_.setZero(); 269 | basePosition_.setZero(); 270 | } catch (const std::exception &e) { 271 | // Error handling. 272 | ROS_ERROR("Error in the PointfootCfg: %s", e.what()); 273 | return false; 274 | } 275 | 276 | return true; 277 | } 278 | 279 | void PointfootController::computeObservation() { 280 | // Get IMU orientation 281 | Eigen::Quaterniond q_wi; 282 | for (size_t i = 0; i < 4; ++i) { 283 | q_wi.coeffs()(i) = imuSensorHandles_.getOrientation()[i]; 284 | } 285 | // Convert quaternion to ZYX Euler angles and calculate inverse rotation matrix 286 | vector3_t zyx = quatToZyx(q_wi); 287 | matrix_t inverseRot = getRotationMatrixFromZyxEulerAngles(zyx).inverse(); 288 | 289 | // Define gravity vector and project it to the body frame 290 | vector3_t gravityVector(0, 0, -1); 291 | vector3_t projectedGravity(inverseRot * gravityVector); 292 | 293 | // Get base angular velocity and apply orientation offset 294 | vector3_t baseAngVel(imuSensorHandles_.getAngularVelocity()[0], imuSensorHandles_.getAngularVelocity()[1], 295 | imuSensorHandles_.getAngularVelocity()[2]); 296 | vector3_t _zyx(imu_orientation_offset[0], imu_orientation_offset[1], imu_orientation_offset[2]); 297 | matrix_t rot = getRotationMatrixFromZyxEulerAngles(_zyx); 298 | baseAngVel = rot * baseAngVel; 299 | projectedGravity = rot * projectedGravity; 300 | 301 | // Get initial state of joints 302 | auto &initState = robotCfg_.initState; 303 | vector_t jointPos(initState.size()); 304 | vector_t jointVel(initState.size()); 305 | for (size_t i = 0; i < hybridJointHandles_.size(); ++i) { 306 | jointPos(i) = hybridJointHandles_[i].getPosition(); 307 | jointVel(i) = hybridJointHandles_[i].getVelocity(); 308 | } 309 | 310 | vector_t gait(4); 311 | gait << 2.0, 0.5, 0.5, 0.1; // trot 312 | gait_index_ += 0.02 * gait(0); 313 | if (gait_index_ > 1.0) 314 | { 315 | gait_index_ = 0.0; 316 | } 317 | vector_t gait_clock(2); 318 | gait_clock << sin(gait_index_ * 2 * M_PI), cos(gait_index_ * 2 * M_PI); 319 | 320 | vector_t actions(lastActions_); 321 | 322 | // Define command scaler and observation vector 323 | matrix_t commandScaler = Eigen::DiagonalMatrix(robotCfg_.userCmdCfg.linVel_x, 324 | robotCfg_.userCmdCfg.linVel_y, 325 | robotCfg_.userCmdCfg.angVel_yaw); 326 | 327 | vector_t obs(observationSize_); 328 | vector3_t scaled_commands = commandScaler * commands_; 329 | 330 | // Populate observation vector 331 | vector_t jointPos_value = (jointPos - initJointAngles_) * robotCfg_.rlCfg.obsScales.dofPos; 332 | 333 | obs << baseAngVel * robotCfg_.rlCfg.obsScales.angVel, 334 | projectedGravity, 335 | jointPos_value, 336 | jointVel * robotCfg_.rlCfg.obsScales.dofVel, 337 | actions, 338 | gait_clock, 339 | gait; 340 | 341 | if (isfirstRecObs_) 342 | { 343 | int64_t inputSize = std::accumulate(encoderInputShapes_[0].begin(), encoderInputShapes_[0].end(), 344 | static_cast(1), std::multiplies()); 345 | proprioHistoryBuffer_.resize(inputSize); 346 | for (size_t i = 0; i < obsHistoryLength_; i++) 347 | { 348 | proprioHistoryBuffer_.segment(i * observationSize_, 349 | observationSize_) = obs.cast(); 350 | } 351 | isfirstRecObs_ = false; 352 | } 353 | proprioHistoryBuffer_.head(proprioHistoryBuffer_.size() - observationSize_) = proprioHistoryBuffer_.tail( 354 | proprioHistoryBuffer_.size() - observationSize_); 355 | proprioHistoryBuffer_.tail(observationSize_) = obs.cast(); 356 | 357 | // Update observation, scaled commands, and proprioceptive history vector 358 | for (size_t i = 0; i < obs.size(); i++) { 359 | observations_[i] = static_cast(obs(i)); 360 | } 361 | for (size_t i = 0; i < scaled_commands_.size(); i++) { 362 | scaled_commands_[i] = static_cast(scaled_commands(i)); 363 | } 364 | for (size_t i = 0; i < proprioHistoryBuffer_.size(); i++) 365 | { 366 | proprioHistoryVector_[i] = static_cast(proprioHistoryBuffer_(i)); 367 | } 368 | 369 | // Limit observation range 370 | scalar_t obsMin = -robotCfg_.rlCfg.clipObs; 371 | scalar_t obsMax = robotCfg_.rlCfg.clipObs; 372 | std::transform(observations_.begin(), observations_.end(), observations_.begin(), 373 | [obsMin, obsMax](scalar_t x) 374 | { return std::max(obsMin, std::min(obsMax, x)); }); 375 | } 376 | 377 | void PointfootController::computeEncoder() { 378 | Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, 379 | OrtMemType::OrtMemTypeDefault); 380 | std::vector inputValues; 381 | inputValues.push_back(Ort::Value::CreateTensor(memoryInfo, proprioHistoryBuffer_.data(), 382 | proprioHistoryBuffer_.size(), 383 | encoderInputShapes_[0].data(), 384 | encoderInputShapes_[0].size())); 385 | 386 | Ort::RunOptions runOptions; 387 | std::vector outputValues = 388 | encoderSessionPtr_->Run(runOptions, encoderInputNames_.data(), inputValues.data(), 1, 389 | encoderOutputNames_.data(), 1); 390 | for (int i = 0; i < encoderOutputSize_; i++) 391 | { 392 | encoderOut_[i] = *(outputValues[0].GetTensorMutableData() + i); 393 | } 394 | 395 | } 396 | 397 | // Computes actions using the policy model. 398 | void PointfootController::computeActions() { 399 | // Create input tensor object 400 | Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, 401 | OrtMemType::OrtMemTypeDefault); 402 | std::vector inputValues; 403 | std::vector combined_obs; 404 | for (const auto &item : encoderOut_) 405 | { 406 | combined_obs.push_back(item); 407 | } 408 | for (const auto &item : observations_) { 409 | combined_obs.push_back(item); 410 | } 411 | for (int i = 0; i < scaled_commands_.size(); i++) 412 | { 413 | combined_obs.push_back(scaled_commands_[i]); 414 | } 415 | 416 | inputValues.push_back( 417 | Ort::Value::CreateTensor(memoryInfo, combined_obs.data(), combined_obs.size(), 418 | policyInputShapes_[0].data(), policyInputShapes_[0].size())); 419 | // Run inference 420 | Ort::RunOptions runOptions; 421 | std::vector outputValues = policySessionPtr_->Run(runOptions, policyInputNames_.data(), 422 | inputValues.data(), 1, policyOutputNames_.data(), 423 | 1); 424 | 425 | for (size_t i = 0; i < actionsSize_; i++) { 426 | actions_[i] = *(outputValues[0].GetTensorMutableData() + i); 427 | } 428 | } 429 | 430 | void PointfootController::cmdVelCallback(const geometry_msgs::TwistConstPtr &msg) { 431 | // Update the commands with the linear and angular velocities from the Twist message. 432 | 433 | // Set linear x velocity. 434 | commands_(0) = (msg->linear.x > 1.0 ? 1.0 : (msg->linear.x < -1.0 ? -1.0 : msg->linear.x)); 435 | 436 | // Set linear y velocity. 437 | commands_(1) = (msg->linear.y > 1.0 ? 1.0 : (msg->linear.y < -1.0 ? -1.0 : msg->linear.y)); 438 | 439 | // Set angular z velocity. 440 | commands_(2) = (msg->angular.z > 1.0 ? 1.0 : (msg->angular.z < -1.0 ? -1.0 : msg->angular.z)); 441 | } 442 | 443 | } // namespace 444 | 445 | // Export the class as a plugin. 446 | PLUGINLIB_EXPORT_CLASS(robot_controller::PointfootController, controller_interface::ControllerBase) 447 | -------------------------------------------------------------------------------- /robot_controllers/src/SolefootController.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #include "robot_controllers/SolefootController.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace robot_controller { 13 | 14 | // Initialize the controller 15 | bool SolefootController::init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh) { 16 | return ControllerBase::init(robot_hw, nh); 17 | } 18 | 19 | // Perform initialization when the controller starts 20 | void SolefootController::starting(const ros::Time &time) { 21 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 22 | ROS_INFO_STREAM("starting hybridJointHandle: " << hybridJointHandles_[i].getPosition()); 23 | defaultJointAngles_[i] = hybridJointHandles_[i].getPosition(); 24 | } 25 | 26 | standPercent_ += 1 / (standDuration_ * loopFrequency_); 27 | 28 | loopCount_ = 0; 29 | 30 | mode_ = Mode::STAND; 31 | } 32 | 33 | // Update function called periodically 34 | void SolefootController::update(const ros::Time &time, const ros::Duration &period) { 35 | switch (mode_) { 36 | case Mode::STAND: 37 | handleStandMode(); 38 | break; 39 | case Mode::WALK: 40 | handleWalkMode(); 41 | break; 42 | } 43 | 44 | loopCount_++; 45 | } 46 | 47 | // Handle walking mode 48 | void SolefootController::handleWalkMode() { 49 | TicToc dida; 50 | // Compute observation & actions 51 | if (robotCfg_.controlCfg.decimation == 0) { 52 | ROS_ERROR("Error robotCfg_.controlCfg.decimation"); 53 | return; 54 | } 55 | if (loopCount_ % robotCfg_.controlCfg.decimation == 0) { 56 | computeObservation(); 57 | computeEncoder(); 58 | computeActions(); 59 | 60 | // Limit action range 61 | scalar_t actionMin = -robotCfg_.rlCfg.clipActions; 62 | scalar_t actionMax = robotCfg_.rlCfg.clipActions; 63 | std::transform(actions_.begin(), actions_.end(), actions_.begin(), 64 | [actionMin, actionMax](scalar_t x) { return std::max(actionMin, std::min(actionMax, x)); }); 65 | } 66 | 67 | // Set action 68 | vector_t jointPos(hybridJointHandles_.size()), jointVel(hybridJointHandles_.size()); 69 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 70 | jointPos(i) = hybridJointHandles_[i].getPosition(); 71 | jointVel(i) = hybridJointHandles_[i].getVelocity(); 72 | } 73 | // in wheel mode, wheel joint actions differ from others 74 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 75 | if ((i + 1) % 4 != 0) { 76 | scalar_t actionMin = 77 | jointPos(i) - initJointAngles_(i, 0) + 78 | (robotCfg_.controlCfg.damping * jointVel(i) - robotCfg_.controlCfg.user_torque_limit) / robotCfg_.controlCfg.stiffness; 79 | scalar_t actionMax = 80 | jointPos(i) - initJointAngles_(i, 0) + 81 | (robotCfg_.controlCfg.damping * jointVel(i) + robotCfg_.controlCfg.user_torque_limit) / robotCfg_.controlCfg.stiffness; 82 | actions_[i] = std::max(actionMin / robotCfg_.controlCfg.action_scale_pos, 83 | std::min(actionMax / robotCfg_.controlCfg.action_scale_pos, (scalar_t)actions_[i])); 84 | scalar_t pos_des = actions_[i] * robotCfg_.controlCfg.action_scale_pos + initJointAngles_(i, 0); 85 | hybridJointHandles_[i].setCommand(pos_des, 0, robotCfg_.controlCfg.stiffness, robotCfg_.controlCfg.damping, 86 | 0, 2); 87 | lastActions_(i, 0) = actions_[i]; 88 | } else { 89 | scalar_t actionMin = (jointVel(i) - ankleJointTorqueLimit_ / ankleJointDamping_); 90 | scalar_t actionMax = (jointVel(i) + ankleJointTorqueLimit_ / ankleJointDamping_); 91 | lastActions_(i, 0) = actions_[i]; 92 | actions_[i] = std::max(actionMin / ankleJointDamping_, 93 | std::min(actionMax / ankleJointDamping_, (scalar_t) actions_[i])); 94 | scalar_t velocity_des = actions_[i] * ankleJointDamping_; 95 | hybridJointHandles_[i].setCommand(0, velocity_des, 0, ankleJointDamping_, 0, 0); 96 | } 97 | } 98 | } 99 | 100 | // Handle standing mode 101 | void SolefootController::handleStandMode() { 102 | if (standPercent_ < 1) { 103 | for (int j = 0; j < hybridJointHandles_.size(); j++) { 104 | scalar_t pos_des = defaultJointAngles_[j] * (1 - standPercent_) + initJointAngles_[j] * standPercent_; 105 | hybridJointHandles_[j].setCommand(pos_des, 0, robotCfg_.controlCfg.stiffness, 106 | robotCfg_.controlCfg.damping, 0, 2); 107 | } 108 | standPercent_ += 1 / (standDuration_ * loopFrequency_); 109 | } else { 110 | mode_ = Mode::WALK; 111 | } 112 | } 113 | 114 | bool SolefootController::loadModel() { 115 | // Load ONNX models for policy, encoder, and gait generator. 116 | 117 | std::string policyModelPath; 118 | if (!nh_.getParam("/policyFile", policyModelPath)) { 119 | ROS_ERROR("Failed to retrieve policy path from the parameter server!"); 120 | return false; 121 | } 122 | 123 | std::string encoderModelPath; 124 | if (!nh_.getParam("/encoderFile", encoderModelPath)) { 125 | ROS_ERROR("Failed to retrieve encoder path from the parameter server!"); 126 | return false; 127 | } 128 | 129 | // Create ONNX environment 130 | onnxEnvPrt_.reset(new Ort::Env(ORT_LOGGING_LEVEL_WARNING, "PointFootOnnxController")); 131 | 132 | // Create session options 133 | Ort::SessionOptions sessionOptions; 134 | sessionOptions.SetIntraOpNumThreads(1); 135 | sessionOptions.SetInterOpNumThreads(1); 136 | 137 | Ort::AllocatorWithDefaultOptions allocator; 138 | 139 | // Policy session 140 | ROS_INFO("Loading policy from: %s", policyModelPath.c_str()); 141 | policySessionPtr_ = std::make_unique(*onnxEnvPrt_, policyModelPath.c_str(), sessionOptions); 142 | policyInputNames_.clear(); 143 | policyOutputNames_.clear(); 144 | policyInputShapes_.clear(); 145 | policyOutputShapes_.clear(); 146 | for (size_t i = 0; i < policySessionPtr_->GetInputCount(); i++) { 147 | policyInputNames_.push_back(policySessionPtr_->GetInputName(i, allocator)); 148 | policyInputShapes_.push_back(policySessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 149 | ROS_INFO("GetInputName: %s", policySessionPtr_->GetInputName(i, allocator)); 150 | std::vector shape = policySessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 151 | std::string shapeString; 152 | for (size_t j = 0; j < shape.size(); ++j) { 153 | shapeString += std::to_string(shape[j]); 154 | if (j != shape.size() - 1) { 155 | shapeString += ", "; 156 | } 157 | } 158 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 159 | } 160 | for (size_t i = 0; i < policySessionPtr_->GetOutputCount(); i++) { 161 | policyOutputNames_.push_back(policySessionPtr_->GetOutputName(i, allocator)); 162 | ROS_INFO("GetOutputName: %s", policySessionPtr_->GetOutputName(i, allocator)); 163 | policyOutputShapes_.push_back(policySessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 164 | std::vector shape = policySessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 165 | std::string shapeString; 166 | for (size_t j = 0; j < shape.size(); ++j) { 167 | shapeString += std::to_string(shape[j]); 168 | if (j != shape.size() - 1) { 169 | shapeString += ", "; 170 | } 171 | } 172 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 173 | } 174 | 175 | // Encoder session 176 | ROS_INFO("Loading encoder from: %s", encoderModelPath.c_str()); 177 | encoderSessionPtr_ = std::make_unique(*onnxEnvPrt_, encoderModelPath.c_str(), sessionOptions); 178 | encoderInputNames_.clear(); 179 | encoderOutputNames_.clear(); 180 | encoderInputShapes_.clear(); 181 | encoderOutputShapes_.clear(); 182 | for (size_t i = 0; i < encoderSessionPtr_->GetInputCount(); i++) { 183 | encoderInputNames_.push_back(encoderSessionPtr_->GetInputName(i, allocator)); 184 | encoderInputShapes_.push_back(encoderSessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 185 | ROS_INFO("GetInputName: %s", encoderSessionPtr_->GetInputName(i, allocator)); 186 | std::vector shape = encoderSessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 187 | std::string shapeString; 188 | for (size_t j = 0; j < shape.size(); ++j) { 189 | shapeString += std::to_string(shape[j]); 190 | if (j != shape.size() - 1) { 191 | shapeString += ", "; 192 | } 193 | } 194 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 195 | } 196 | for (size_t i = 0; i < encoderSessionPtr_->GetOutputCount(); i++) { 197 | encoderOutputNames_.push_back(encoderSessionPtr_->GetOutputName(i, allocator)); 198 | ROS_INFO("GetOutputName: %s", encoderSessionPtr_->GetOutputName(i, allocator)); 199 | encoderOutputShapes_.push_back(encoderSessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 200 | std::vector shape = encoderSessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 201 | std::string shapeString; 202 | for (size_t j = 0; j < shape.size(); ++j) { 203 | shapeString += std::to_string(shape[j]); 204 | if (j != shape.size() - 1) { 205 | shapeString += ", "; 206 | } 207 | } 208 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 209 | } 210 | 211 | ROS_INFO("Successfully loaded ONNX models!"); 212 | return true; 213 | } 214 | 215 | // Loads the reinforcement learning configuration. 216 | bool SolefootController::loadRLCfg() { 217 | auto &initState = robotCfg_.initState; 218 | SoleBipedRobotCfg::ControlCfg &controlCfg = robotCfg_.controlCfg; 219 | SoleBipedRobotCfg::RlCfg::ObsScales &obsScales = robotCfg_.rlCfg.obsScales; 220 | 221 | try { 222 | // Load parameters from ROS parameter server. 223 | int error = 0; 224 | error += static_cast(!nh_.getParam("/PointfootCfg/joint_names", jointNames_)); 225 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/abad_L_Joint", initState["abad_L_Joint"])); 226 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/hip_L_Joint", initState["hip_L_Joint"])); 227 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/knee_L_Joint", initState["knee_L_Joint"])); 228 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/abad_R_Joint", initState["abad_R_Joint"])); 229 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/hip_R_Joint", initState["hip_R_Joint"])); 230 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/knee_R_Joint", initState["knee_R_Joint"])); 231 | error += static_cast(!nh_.getParam("/PointfootCfg/stand_mode/stand_duration", standDuration_)); 232 | error += static_cast(!nh_.getParam("/robot_hw/loop_frequency", loopFrequency_)); 233 | error += static_cast(!nh_.getParam("/PointfootCfg/control/stiffness", controlCfg.stiffness)); 234 | error += static_cast(!nh_.getParam("/PointfootCfg/control/damping", controlCfg.damping)); 235 | error += static_cast(!nh_.getParam("/PointfootCfg/control/action_scale_pos", controlCfg.action_scale_pos)); 236 | error += static_cast(!nh_.getParam("/PointfootCfg/control/decimation", controlCfg.decimation)); 237 | error += static_cast(!nh_.getParam("/PointfootCfg/control/user_torque_limit", controlCfg.user_torque_limit)); 238 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/clip_scales/clip_observations", robotCfg_.rlCfg.clipObs)); 239 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/clip_scales/clip_actions", robotCfg_.rlCfg.clipActions)); 240 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/lin_vel", obsScales.linVel)); 241 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/ang_vel", obsScales.angVel)); 242 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/dof_pos", obsScales.dofPos)); 243 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/dof_vel", obsScales.dofVel)); 244 | error += static_cast(!nh_.getParam("/PointfootCfg/size/actions_size", actionsSize_)); 245 | error += static_cast(!nh_.getParam("/PointfootCfg/size/observations_size", observationSize_)); 246 | error += static_cast(!nh_.getParam("/PointfootCfg/size/obs_history_length", obsHistoryLength_)); 247 | error += static_cast(!nh_.getParam("/PointfootCfg/size/encoder_output_size", encoderOutputSize_)); 248 | error += static_cast(!nh_.getParam("/PointfootCfg/size/commands_size", commandSizeSolefoot_)); 249 | 250 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/yaw", imu_orientation_offset[0])); 251 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/pitch", imu_orientation_offset[1])); 252 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/roll", imu_orientation_offset[2])); 253 | 254 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/lin_vel_x", robotCfg_.userCmdCfg.linVel_x)); 255 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/lin_vel_y", robotCfg_.userCmdCfg.linVel_y)); 256 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/ang_vel_yaw", robotCfg_.userCmdCfg.angVel_yaw)); 257 | 258 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/ankle_L_Joint", initState["ankle_L_Joint"])); 259 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/ankle_R_Joint", initState["ankle_R_Joint"])); 260 | error += static_cast(!nh_.getParam("/PointfootCfg/control/ankle_joint_damping", ankleJointDamping_)); 261 | error += static_cast(!nh_.getParam("/PointfootCfg/control/ankle_joint_torque_limit", ankleJointTorqueLimit_)); 262 | 263 | error += static_cast(!nh_.getParam("/PointfootCfg/gait/frequencies", robotCfg_.gaitCfg.frequencies)); 264 | error += static_cast(!nh_.getParam("/PointfootCfg/gait/swing_height", robotCfg_.gaitCfg.swing_height)); 265 | 266 | if (error) { 267 | ROS_ERROR("Load parameters from ROS parameter server error!!!"); 268 | } 269 | if (standDuration_ <= 0.5) { 270 | ROS_ERROR("standDuration_ must be larger than 0.5!!!"); 271 | } 272 | 273 | encoderInputSize_ = obsHistoryLength_ * observationSize_; 274 | robotCfg_.print(); 275 | 276 | // Resize vectors. 277 | actions_.resize(actionsSize_); 278 | observations_.resize(observationSize_); 279 | proprioHistoryVector_.resize(observationSize_ * obsHistoryLength_); 280 | encoderOut_.resize(encoderOutputSize_); 281 | lastActions_.resize(actionsSize_); 282 | commandSolefoot_.resize(commandSizeSolefoot_); 283 | scaledCommandSolefoot_.resize(commandSizeSolefoot_); 284 | 285 | // Initialize vectors. 286 | lastActions_.setZero(); 287 | commandSolefoot_.setZero(); 288 | scaledCommandSolefoot_.setZero(); 289 | baseLinVel_.setZero(); 290 | basePosition_.setZero(); 291 | } catch (const std::exception &e) { 292 | // Error handling. 293 | ROS_ERROR("Error in the PointfootCfg: %s", e.what()); 294 | return false; 295 | } 296 | 297 | return true; 298 | } 299 | 300 | void SolefootController::computeObservation() { 301 | // Get IMU orientation 302 | Eigen::Quaterniond q_wi; 303 | for (size_t i = 0; i < 4; ++i) { 304 | q_wi.coeffs()(i) = imuSensorHandles_.getOrientation()[i]; 305 | } 306 | // Convert quaternion to ZYX Euler angles and calculate inverse rotation matrix 307 | vector3_t zyx = quatToZyx(q_wi); 308 | matrix_t inverseRot = getRotationMatrixFromZyxEulerAngles(zyx).inverse(); 309 | 310 | // Define gravity vector and project it to the body frame 311 | vector3_t gravityVector(0, 0, -1); 312 | vector3_t projectedGravity(inverseRot * gravityVector); 313 | 314 | // Get base angular velocity and apply orientation offset 315 | vector3_t baseAngVel(imuSensorHandles_.getAngularVelocity()[0], imuSensorHandles_.getAngularVelocity()[1], 316 | imuSensorHandles_.getAngularVelocity()[2]); 317 | vector3_t _zyx(imu_orientation_offset[0], imu_orientation_offset[1], imu_orientation_offset[2]); 318 | matrix_t rot = getRotationMatrixFromZyxEulerAngles(_zyx); 319 | baseAngVel = rot * baseAngVel; 320 | projectedGravity = rot * projectedGravity; 321 | 322 | // Get initial state of joints 323 | auto &initState = robotCfg_.initState; 324 | vector_t jointPos(initState.size()); 325 | vector_t jointVel(initState.size()); 326 | for (size_t i = 0; i < hybridJointHandles_.size(); ++i) { 327 | jointPos(i) = hybridJointHandles_[i].getPosition(); 328 | jointVel(i) = hybridJointHandles_[i].getVelocity(); 329 | } 330 | 331 | vector_t gait(4); 332 | gait << robotCfg_.gaitCfg.frequencies, 0.5, 0.5, robotCfg_.gaitCfg.swing_height; // trot 333 | gait_index_ += 0.02 * gait(0); 334 | if (gait_index_ > 1.0) 335 | { 336 | gait_index_ = 0.0; 337 | } 338 | vector_t gait_clock(2); 339 | gait_clock << sin(gait_index_ * 2 * M_PI), cos(gait_index_ * 2 * M_PI); 340 | 341 | vector_t actions(lastActions_); 342 | 343 | // Define command scaler and observation vector 344 | vector_t commandScalerVal(commandSizeSolefoot_); 345 | commandScalerVal << robotCfg_.userCmdCfg.linVel_x, robotCfg_.userCmdCfg.linVel_y, robotCfg_.userCmdCfg.angVel_yaw, 1.0, 1.0; 346 | // matrix_t commandScaler = Eigen::DiagonalMatrix(robotCfg_.userCmdCfg.linVel_x, 347 | // robotCfg_.userCmdCfg.linVel_y, 348 | // robotCfg_.userCmdCfg.angVel_yaw); 349 | vector_t scaled_commands(commandSizeSolefoot_); 350 | for (int i = 0; i < commandSizeSolefoot_; i++) { 351 | scaled_commands(i) = commandSolefoot_(i) * commandScalerVal(i); 352 | } 353 | vector_t obs(observationSize_); 354 | // vector_t scaled_commands = commandScaler * commandSolefoot_; 355 | // Populate observation vector 356 | vector_t jointPos_value = (jointPos - initJointAngles_) * robotCfg_.rlCfg.obsScales.dofPos; 357 | vector_t jointPos_input = jointPos_value; 358 | 359 | obs << baseAngVel * robotCfg_.rlCfg.obsScales.angVel, 360 | projectedGravity, 361 | jointPos_input, 362 | jointVel * robotCfg_.rlCfg.obsScales.dofVel, 363 | actions, 364 | gait_clock, 365 | gait; 366 | if (isfirstRecObs_) 367 | { 368 | int64_t inputSize = std::accumulate(encoderInputShapes_[0].begin(), encoderInputShapes_[0].end(), 369 | static_cast(1), std::multiplies()); 370 | proprioHistoryBuffer_.resize(inputSize); 371 | for (size_t i = 0; i < obsHistoryLength_; i++) 372 | { 373 | proprioHistoryBuffer_.segment(i * observationSize_, 374 | observationSize_) = obs.cast(); 375 | } 376 | isfirstRecObs_ = false; 377 | } 378 | proprioHistoryBuffer_.head(proprioHistoryBuffer_.size() - observationSize_) = proprioHistoryBuffer_.tail( 379 | proprioHistoryBuffer_.size() - observationSize_); 380 | proprioHistoryBuffer_.tail(observationSize_) = obs.cast(); 381 | // Update observation, scaled commands, and proprioceptive history vector 382 | for (size_t i = 0; i < obs.size(); i++) { 383 | observations_[i] = static_cast(obs(i)); 384 | } 385 | for (size_t i = 0; i < scaledCommandSolefoot_.size(); i++) { 386 | scaledCommandSolefoot_[i] = static_cast(scaled_commands(i)); 387 | } 388 | for (size_t i = 0; i < proprioHistoryBuffer_.size(); i++) 389 | { 390 | proprioHistoryVector_[i] = static_cast(proprioHistoryBuffer_(i)); 391 | } 392 | 393 | // Limit observation range 394 | scalar_t obsMin = -robotCfg_.rlCfg.clipObs; 395 | scalar_t obsMax = robotCfg_.rlCfg.clipObs; 396 | std::transform(observations_.begin(), observations_.end(), observations_.begin(), 397 | [obsMin, obsMax](scalar_t x) 398 | { return std::max(obsMin, std::min(obsMax, x)); }); 399 | } 400 | 401 | void SolefootController::computeEncoder() { 402 | Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, 403 | OrtMemType::OrtMemTypeDefault); 404 | std::vector inputValues; 405 | inputValues.push_back(Ort::Value::CreateTensor(memoryInfo, proprioHistoryBuffer_.data(), 406 | proprioHistoryBuffer_.size(), 407 | encoderInputShapes_[0].data(), 408 | encoderInputShapes_[0].size())); 409 | 410 | Ort::RunOptions runOptions; 411 | std::vector outputValues = 412 | encoderSessionPtr_->Run(runOptions, encoderInputNames_.data(), inputValues.data(), 1, 413 | encoderOutputNames_.data(), 1); 414 | for (int i = 0; i < encoderOutputSize_; i++) 415 | { 416 | encoderOut_[i] = *(outputValues[0].GetTensorMutableData() + i); 417 | } 418 | 419 | } 420 | 421 | // Computes actions using the policy model. 422 | void SolefootController::computeActions() { 423 | // Create input tensor object 424 | Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, 425 | OrtMemType::OrtMemTypeDefault); 426 | std::vector inputValues; 427 | std::vector combined_obs; 428 | for (const auto &item : encoderOut_) 429 | { 430 | combined_obs.push_back(item); 431 | } 432 | for (const auto &item : observations_) { 433 | combined_obs.push_back(item); 434 | } 435 | for (int i = 0; i < scaledCommandSolefoot_.size(); i++) 436 | { 437 | combined_obs.push_back(scaledCommandSolefoot_[i]); 438 | } 439 | 440 | inputValues.push_back( 441 | Ort::Value::CreateTensor(memoryInfo, combined_obs.data(), combined_obs.size(), 442 | policyInputShapes_[0].data(), policyInputShapes_[0].size())); 443 | // Run inference 444 | Ort::RunOptions runOptions; 445 | std::vector outputValues = policySessionPtr_->Run(runOptions, policyInputNames_.data(), 446 | inputValues.data(), 1, policyOutputNames_.data(), 447 | 1); 448 | 449 | for (size_t i = 0; i < actionsSize_; i++) { 450 | actions_[i] = *(outputValues[0].GetTensorMutableData() + i); 451 | } 452 | } 453 | 454 | // vector_t SolefootController::handleGait() { 455 | // vector_t gait(4); 456 | // gait << robotCfg_.gaitCfg.frequencies, 0.5, 0.5, robotCfg_.gaitCfg.swing_height; 457 | // return gait; 458 | // } 459 | 460 | // vector_t SolefootController::handleGaitClock(vector_t &gait) { 461 | // vector_t gait_clock(2); 462 | // gait_index_ += 0.02 * gait(0); 463 | // gait_index_ = gait_index_ > 1.0 ? 0.0 : gait_index_; 464 | // gait_clock << sin(gait_index_ * 2 * M_PI), cos(gait_index_ * 2 * M_PI); 465 | // return gait_clock; 466 | // } 467 | 468 | void SolefootController::cmdVelCallback(const geometry_msgs::TwistConstPtr &msg) { 469 | // Update the commands with the linear and angular velocities from the Twist message. 470 | 471 | // Set linear x velocity. 472 | commandSolefoot_(0) = (msg->linear.x > 1.0 ? 1.0 : (msg->linear.x < -1.0 ? -1.0 : msg->linear.x)); 473 | 474 | // Set linear y velocity. 475 | commandSolefoot_(1) = (msg->linear.y > 1.0 ? 1.0 : (msg->linear.y < -1.0 ? -1.0 : msg->linear.y)); 476 | 477 | // Set angular z velocity. 478 | commandSolefoot_(2) = (msg->angular.z > 1.0 ? 1.0 : (msg->angular.z < -1.0 ? -1.0 : msg->angular.z)); 479 | commandSolefoot_(3) = 0.0; 480 | commandSolefoot_(4) = 0.0; 481 | } 482 | 483 | } // namespace 484 | 485 | // Export the class as a plugin. 486 | PLUGINLIB_EXPORT_CLASS(robot_controller::SolefootController, controller_interface::ControllerBase) 487 | -------------------------------------------------------------------------------- /robot_controllers/src/WheelfootController.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #include "robot_controllers/WheelfootController.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace robot_controller { 13 | 14 | // Initialize the controller 15 | bool WheelfootController::init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &nh) { 16 | return ControllerBase::init(robot_hw, nh); 17 | } 18 | 19 | // Perform initialization when the controller starts 20 | void WheelfootController::starting(const ros::Time &time) { 21 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 22 | ROS_INFO_STREAM("starting hybridJointHandle: " << hybridJointHandles_[i].getPosition()); 23 | defaultJointAngles_[i] = hybridJointHandles_[i].getPosition(); 24 | } 25 | 26 | standPercent_ += 1 / (standDuration_ * loopFrequency_); 27 | 28 | loopCount_ = 0; 29 | 30 | mode_ = Mode::STAND; 31 | } 32 | 33 | // Update function called periodically 34 | void WheelfootController::update(const ros::Time &time, const ros::Duration &period) { 35 | switch (mode_) { 36 | case Mode::STAND: 37 | initJointAngles_(1, 0) = -0.9; 38 | initJointAngles_(5, 0) = 0.9; 39 | handleStandMode(); 40 | break; 41 | case Mode::WALK: 42 | initJointAngles_(1, 0) = -0.0; 43 | initJointAngles_(5, 0) = 0.0; 44 | handleWalkMode(); 45 | break; 46 | } 47 | 48 | loopCount_++; 49 | } 50 | 51 | // Handle walking mode 52 | void WheelfootController::handleWalkMode() { 53 | TicToc dida; 54 | // Compute observation & actions 55 | if (robotCfg_.controlCfg.decimation == 0) { 56 | ROS_ERROR("Error robotCfg_.controlCfg.decimation"); 57 | return; 58 | } 59 | if (loopCount_ % robotCfg_.controlCfg.decimation == 0) { 60 | computeObservation(); 61 | computeEncoder(); 62 | computeActions(); 63 | 64 | // Limit action range 65 | scalar_t actionMin = -robotCfg_.rlCfg.clipActions; 66 | scalar_t actionMax = robotCfg_.rlCfg.clipActions; 67 | std::transform(actions_.begin(), actions_.end(), actions_.begin(), 68 | [actionMin, actionMax](scalar_t x) { return std::max(actionMin, std::min(actionMax, x)); }); 69 | } 70 | 71 | // Set action 72 | vector_t jointPos(hybridJointHandles_.size()), jointVel(hybridJointHandles_.size()); 73 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 74 | jointPos(i) = hybridJointHandles_[i].getPosition(); 75 | jointVel(i) = hybridJointHandles_[i].getVelocity(); 76 | } 77 | // in wheel mode, wheel joint actions differ from others 78 | for (size_t i = 0; i < hybridJointHandles_.size(); i++) { 79 | if ((i + 1) % 4 != 0) { 80 | scalar_t actionMin = 81 | jointPos(i) - initJointAngles_(i, 0) + 82 | (robotCfg_.controlCfg.damping * jointVel(i) - robotCfg_.controlCfg.user_torque_limit) / robotCfg_.controlCfg.stiffness; 83 | scalar_t actionMax = 84 | jointPos(i) - initJointAngles_(i, 0) + 85 | (robotCfg_.controlCfg.damping * jointVel(i) + robotCfg_.controlCfg.user_torque_limit) / robotCfg_.controlCfg.stiffness; 86 | actions_[i] = std::max(actionMin / robotCfg_.controlCfg.action_scale_pos, 87 | std::min(actionMax / robotCfg_.controlCfg.action_scale_pos, (scalar_t)actions_[i])); 88 | scalar_t pos_des = actions_[i] * robotCfg_.controlCfg.action_scale_pos + initJointAngles_(i, 0); 89 | hybridJointHandles_[i].setCommand(pos_des, 0, robotCfg_.controlCfg.stiffness, robotCfg_.controlCfg.damping, 90 | 0, 2); 91 | lastActions_(i, 0) = actions_[i]; 92 | } else { 93 | scalar_t actionMin = (jointVel(i) - wheelJointTorqueLimit_ / wheelJointDamping_); 94 | scalar_t actionMax = (jointVel(i) + wheelJointTorqueLimit_ / wheelJointDamping_); 95 | lastActions_(i, 0) = actions_[i]; 96 | actions_[i] = std::max(actionMin / wheelJointDamping_, 97 | std::min(actionMax / wheelJointDamping_, (scalar_t) actions_[i])); 98 | scalar_t velocity_des = actions_[i] * wheelJointDamping_; 99 | hybridJointHandles_[i].setCommand(0, velocity_des, 0, wheelJointDamping_, 0, 0); 100 | } 101 | } 102 | } 103 | 104 | // Handle standing mode 105 | void WheelfootController::handleStandMode() { 106 | if (standPercent_ < 1) { 107 | for (int j = 0; j < hybridJointHandles_.size(); j++) { 108 | if ((j + 1) % 4 != 0) { 109 | scalar_t pos_des = defaultJointAngles_[j] * (1 - standPercent_) + initJointAngles_[j] * standPercent_; 110 | hybridJointHandles_[j].setCommand(pos_des, 0, robotCfg_.controlCfg.stiffness, 111 | robotCfg_.controlCfg.damping, 0, 2); 112 | } 113 | else { 114 | hybridJointHandles_[j].setCommand(0, 0.0, 0, wheelJointDamping_, 0, 0); 115 | } 116 | } 117 | standPercent_ += 3 / (standDuration_ * loopFrequency_); 118 | } else { 119 | mode_ = Mode::WALK; 120 | } 121 | } 122 | 123 | bool WheelfootController::loadModel() { 124 | // Load ONNX models for policy, encoder, and gait generator. 125 | 126 | std::string policyModelPath; 127 | if (!nh_.getParam("/policyFile", policyModelPath)) { 128 | ROS_ERROR("Failed to retrieve policy path from the parameter server!"); 129 | return false; 130 | } 131 | 132 | std::string encoderModelPath; 133 | if (!nh_.getParam("/encoderFile", encoderModelPath)) { 134 | ROS_ERROR("Failed to retrieve encoder path from the parameter server!"); 135 | return false; 136 | } 137 | 138 | // Create ONNX environment 139 | onnxEnvPrt_.reset(new Ort::Env(ORT_LOGGING_LEVEL_WARNING, "PointFootOnnxController")); 140 | 141 | // Create session options 142 | Ort::SessionOptions sessionOptions; 143 | sessionOptions.SetIntraOpNumThreads(1); 144 | sessionOptions.SetInterOpNumThreads(1); 145 | 146 | Ort::AllocatorWithDefaultOptions allocator; 147 | 148 | // Policy session 149 | ROS_INFO("Loading policy from: %s", policyModelPath.c_str()); 150 | policySessionPtr_ = std::make_unique(*onnxEnvPrt_, policyModelPath.c_str(), sessionOptions); 151 | policyInputNames_.clear(); 152 | policyOutputNames_.clear(); 153 | policyInputShapes_.clear(); 154 | policyOutputShapes_.clear(); 155 | for (size_t i = 0; i < policySessionPtr_->GetInputCount(); i++) { 156 | policyInputNames_.push_back(policySessionPtr_->GetInputName(i, allocator)); 157 | policyInputShapes_.push_back(policySessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 158 | ROS_INFO("GetInputName: %s", policySessionPtr_->GetInputName(i, allocator)); 159 | std::vector shape = policySessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 160 | std::string shapeString; 161 | for (size_t j = 0; j < shape.size(); ++j) { 162 | shapeString += std::to_string(shape[j]); 163 | if (j != shape.size() - 1) { 164 | shapeString += ", "; 165 | } 166 | } 167 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 168 | } 169 | for (size_t i = 0; i < policySessionPtr_->GetOutputCount(); i++) { 170 | policyOutputNames_.push_back(policySessionPtr_->GetOutputName(i, allocator)); 171 | ROS_INFO("GetOutputName: %s", policySessionPtr_->GetOutputName(i, allocator)); 172 | policyOutputShapes_.push_back(policySessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 173 | std::vector shape = policySessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 174 | std::string shapeString; 175 | for (size_t j = 0; j < shape.size(); ++j) { 176 | shapeString += std::to_string(shape[j]); 177 | if (j != shape.size() - 1) { 178 | shapeString += ", "; 179 | } 180 | } 181 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 182 | } 183 | 184 | // Encoder session 185 | ROS_INFO("Loading encoder from: %s", encoderModelPath.c_str()); 186 | encoderSessionPtr_ = std::make_unique(*onnxEnvPrt_, encoderModelPath.c_str(), sessionOptions); 187 | encoderInputNames_.clear(); 188 | encoderOutputNames_.clear(); 189 | encoderInputShapes_.clear(); 190 | encoderOutputShapes_.clear(); 191 | for (size_t i = 0; i < encoderSessionPtr_->GetInputCount(); i++) { 192 | encoderInputNames_.push_back(encoderSessionPtr_->GetInputName(i, allocator)); 193 | encoderInputShapes_.push_back(encoderSessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 194 | ROS_INFO("GetInputName: %s", encoderSessionPtr_->GetInputName(i, allocator)); 195 | std::vector shape = encoderSessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 196 | std::string shapeString; 197 | for (size_t j = 0; j < shape.size(); ++j) { 198 | shapeString += std::to_string(shape[j]); 199 | if (j != shape.size() - 1) { 200 | shapeString += ", "; 201 | } 202 | } 203 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 204 | } 205 | for (size_t i = 0; i < encoderSessionPtr_->GetOutputCount(); i++) { 206 | encoderOutputNames_.push_back(encoderSessionPtr_->GetOutputName(i, allocator)); 207 | ROS_INFO("GetOutputName: %s", encoderSessionPtr_->GetOutputName(i, allocator)); 208 | encoderOutputShapes_.push_back(encoderSessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); 209 | std::vector shape = encoderSessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape(); 210 | std::string shapeString; 211 | for (size_t j = 0; j < shape.size(); ++j) { 212 | shapeString += std::to_string(shape[j]); 213 | if (j != shape.size() - 1) { 214 | shapeString += ", "; 215 | } 216 | } 217 | ROS_INFO("Shape: [%s]", shapeString.c_str()); 218 | } 219 | 220 | ROS_INFO("Successfully loaded ONNX models!"); 221 | return true; 222 | } 223 | 224 | // Loads the reinforcement learning configuration. 225 | bool WheelfootController::loadRLCfg() { 226 | auto &initState = robotCfg_.initState; 227 | WheelBipedRobotCfg::ControlCfg &controlCfg = robotCfg_.controlCfg; 228 | WheelBipedRobotCfg::RlCfg::ObsScales &obsScales = robotCfg_.rlCfg.obsScales; 229 | 230 | try { 231 | // Load parameters from ROS parameter server. 232 | int error = 0; 233 | error += static_cast(!nh_.getParam("/PointfootCfg/joint_names", jointNames_)); 234 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/abad_L_Joint", initState["abad_L_Joint"])); 235 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/hip_L_Joint", initState["hip_L_Joint"])); 236 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/knee_L_Joint", initState["knee_L_Joint"])); 237 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/abad_R_Joint", initState["abad_R_Joint"])); 238 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/hip_R_Joint", initState["hip_R_Joint"])); 239 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/knee_R_Joint", initState["knee_R_Joint"])); 240 | error += static_cast(!nh_.getParam("/PointfootCfg/stand_mode/stand_duration", standDuration_)); 241 | error += static_cast(!nh_.getParam("/robot_hw/loop_frequency", loopFrequency_)); 242 | error += static_cast(!nh_.getParam("/PointfootCfg/control/stiffness", controlCfg.stiffness)); 243 | error += static_cast(!nh_.getParam("/PointfootCfg/control/damping", controlCfg.damping)); 244 | error += static_cast(!nh_.getParam("/PointfootCfg/control/action_scale_pos", controlCfg.action_scale_pos)); 245 | error += static_cast(!nh_.getParam("/PointfootCfg/control/decimation", controlCfg.decimation)); 246 | error += static_cast(!nh_.getParam("/PointfootCfg/control/user_torque_limit", controlCfg.user_torque_limit)); 247 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/clip_scales/clip_observations", robotCfg_.rlCfg.clipObs)); 248 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/clip_scales/clip_actions", robotCfg_.rlCfg.clipActions)); 249 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/lin_vel", obsScales.linVel)); 250 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/ang_vel", obsScales.angVel)); 251 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/dof_pos", obsScales.dofPos)); 252 | error += static_cast(!nh_.getParam("/PointfootCfg/normalization/obs_scales/dof_vel", obsScales.dofVel)); 253 | error += static_cast(!nh_.getParam("/PointfootCfg/size/actions_size", actionsSize_)); 254 | error += static_cast(!nh_.getParam("/PointfootCfg/size/observations_size", observationSize_)); 255 | error += static_cast(!nh_.getParam("/PointfootCfg/size/obs_history_length", obsHistoryLength_)); 256 | error += static_cast(!nh_.getParam("/PointfootCfg/size/encoder_output_size", encoderOutputSize_)); 257 | 258 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/yaw", imu_orientation_offset[0])); 259 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/pitch", imu_orientation_offset[1])); 260 | error += static_cast(!nh_.getParam("/PointfootCfg/imu_orientation_offset/roll", imu_orientation_offset[2])); 261 | 262 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/lin_vel_x", robotCfg_.userCmdCfg.linVel_x)); 263 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/lin_vel_y", robotCfg_.userCmdCfg.linVel_y)); 264 | error += static_cast(!nh_.getParam("/PointfootCfg/user_cmd_scales/ang_vel_yaw", robotCfg_.userCmdCfg.angVel_yaw)); 265 | 266 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/wheel_R_Joint", initState["wheel_R_Joint"])); 267 | error += static_cast(!nh_.getParam("/PointfootCfg/init_state/default_joint_angle/wheel_L_Joint", initState["wheel_L_Joint"])); 268 | error += static_cast(!nh_.getParam("/PointfootCfg/control/wheel_joint_damping", wheelJointDamping_)); 269 | error += static_cast(!nh_.getParam("/PointfootCfg/control/wheel_joint_torque_limit", wheelJointTorqueLimit_)); 270 | error += static_cast(!nh_.getParam("/PointfootCfg/size/jointpos_idxs", jointPosIdxs_)); 271 | 272 | if (error) { 273 | ROS_ERROR("Load parameters from ROS parameter server error!!!"); 274 | } 275 | if (standDuration_ <= 0.5) { 276 | ROS_ERROR("standDuration_ must be larger than 0.5!!!"); 277 | } 278 | 279 | encoderInputSize_ = obsHistoryLength_ * observationSize_; 280 | robotCfg_.print(); 281 | 282 | // Resize vectors. 283 | actions_.resize(actionsSize_); 284 | observations_.resize(observationSize_); 285 | proprioHistoryVector_.resize(observationSize_ * obsHistoryLength_); 286 | encoderOut_.resize(encoderOutputSize_); 287 | lastActions_.resize(actionsSize_); 288 | 289 | // Initialize vectors. 290 | lastActions_.setZero(); 291 | commands_.setZero(); 292 | scaled_commands_.setZero(); 293 | baseLinVel_.setZero(); 294 | basePosition_.setZero(); 295 | } catch (const std::exception &e) { 296 | // Error handling. 297 | ROS_ERROR("Error in the PointfootCfg: %s", e.what()); 298 | return false; 299 | } 300 | 301 | return true; 302 | } 303 | 304 | void WheelfootController::computeObservation() { 305 | // Get IMU orientation 306 | Eigen::Quaterniond q_wi; 307 | for (size_t i = 0; i < 4; ++i) { 308 | q_wi.coeffs()(i) = imuSensorHandles_.getOrientation()[i]; 309 | } 310 | // Convert quaternion to ZYX Euler angles and calculate inverse rotation matrix 311 | vector3_t zyx = quatToZyx(q_wi); 312 | matrix_t inverseRot = getRotationMatrixFromZyxEulerAngles(zyx).inverse(); 313 | 314 | // Define gravity vector and project it to the body frame 315 | vector3_t gravityVector(0, 0, -1); 316 | vector3_t projectedGravity(inverseRot * gravityVector); 317 | 318 | // Get base angular velocity and apply orientation offset 319 | vector3_t baseAngVel(imuSensorHandles_.getAngularVelocity()[0], imuSensorHandles_.getAngularVelocity()[1], 320 | imuSensorHandles_.getAngularVelocity()[2]); 321 | vector3_t _zyx(imu_orientation_offset[0], imu_orientation_offset[1], imu_orientation_offset[2]); 322 | matrix_t rot = getRotationMatrixFromZyxEulerAngles(_zyx); 323 | baseAngVel = rot * baseAngVel; 324 | projectedGravity = rot * projectedGravity; 325 | 326 | // Get initial state of joints 327 | auto &initState = robotCfg_.initState; 328 | vector_t jointPos(initState.size()); 329 | vector_t jointVel(initState.size()); 330 | for (size_t i = 0; i < hybridJointHandles_.size(); ++i) { 331 | jointPos(i) = hybridJointHandles_[i].getPosition(); 332 | jointVel(i) = hybridJointHandles_[i].getVelocity(); 333 | } 334 | 335 | vector_t actions(lastActions_); 336 | 337 | // Define command scaler and observation vector 338 | matrix_t commandScaler = Eigen::DiagonalMatrix(robotCfg_.userCmdCfg.linVel_x, 339 | robotCfg_.userCmdCfg.linVel_y, 340 | robotCfg_.userCmdCfg.angVel_yaw); 341 | 342 | vector_t obs(observationSize_); 343 | vector3_t scaled_commands = commandScaler * commands_; 344 | // Populate observation vector 345 | vector_t jointPos_value = (jointPos - initJointAngles_) * robotCfg_.rlCfg.obsScales.dofPos; 346 | vector_t jointPos_input; 347 | jointPos_input.resize(jointPosIdxs_.size()); 348 | for (int i = 0; i < jointPosIdxs_.size(); i++){ 349 | jointPos_input(i) = jointPos_value(jointPosIdxs_[i]); 350 | } 351 | 352 | obs << baseAngVel * robotCfg_.rlCfg.obsScales.angVel, 353 | projectedGravity, 354 | jointPos_input, 355 | jointVel * robotCfg_.rlCfg.obsScales.dofVel, 356 | actions; 357 | // remove scaled_commands; 358 | 359 | if (isfirstRecObs_) 360 | { 361 | int64_t inputSize = std::accumulate(encoderInputShapes_[0].begin(), encoderInputShapes_[0].end(), 362 | static_cast(1), std::multiplies()); 363 | proprioHistoryBuffer_.resize(inputSize); 364 | for (size_t i = 0; i < obsHistoryLength_; i++) 365 | { 366 | proprioHistoryBuffer_.segment(i * observationSize_, 367 | observationSize_) = obs.cast(); 368 | } 369 | isfirstRecObs_ = false; 370 | } 371 | proprioHistoryBuffer_.head(proprioHistoryBuffer_.size() - observationSize_) = proprioHistoryBuffer_.tail( 372 | proprioHistoryBuffer_.size() - observationSize_); 373 | proprioHistoryBuffer_.tail(observationSize_) = obs.cast(); 374 | 375 | // Update observation, scaled commands, and proprioceptive history vector 376 | for (size_t i = 0; i < obs.size(); i++) { 377 | observations_[i] = static_cast(obs(i)); 378 | } 379 | for (size_t i = 0; i < scaled_commands_.size(); i++) { 380 | scaled_commands_[i] = static_cast(scaled_commands(i)); 381 | } 382 | for (size_t i = 0; i < proprioHistoryBuffer_.size(); i++) 383 | { 384 | proprioHistoryVector_[i] = static_cast(proprioHistoryBuffer_(i)); 385 | } 386 | 387 | // Limit observation range 388 | scalar_t obsMin = -robotCfg_.rlCfg.clipObs; 389 | scalar_t obsMax = robotCfg_.rlCfg.clipObs; 390 | std::transform(observations_.begin(), observations_.end(), observations_.begin(), 391 | [obsMin, obsMax](scalar_t x) 392 | { return std::max(obsMin, std::min(obsMax, x)); }); 393 | } 394 | 395 | void WheelfootController::computeEncoder() { 396 | Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, 397 | OrtMemType::OrtMemTypeDefault); 398 | std::vector inputValues; 399 | inputValues.push_back(Ort::Value::CreateTensor(memoryInfo, proprioHistoryBuffer_.data(), 400 | proprioHistoryBuffer_.size(), 401 | encoderInputShapes_[0].data(), 402 | encoderInputShapes_[0].size())); 403 | 404 | Ort::RunOptions runOptions; 405 | std::vector outputValues = 406 | encoderSessionPtr_->Run(runOptions, encoderInputNames_.data(), inputValues.data(), 1, 407 | encoderOutputNames_.data(), 1); 408 | for (int i = 0; i < encoderOutputSize_; i++) 409 | { 410 | encoderOut_[i] = *(outputValues[0].GetTensorMutableData() + i); 411 | } 412 | 413 | } 414 | 415 | // Computes actions using the policy model. 416 | void WheelfootController::computeActions() { 417 | // Create input tensor object 418 | Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, 419 | OrtMemType::OrtMemTypeDefault); 420 | std::vector inputValues; 421 | std::vector combined_obs; 422 | for (const auto &item : encoderOut_) 423 | { 424 | combined_obs.push_back(item); 425 | } 426 | for (const auto &item : observations_) { 427 | combined_obs.push_back(item); 428 | } 429 | for (int i = 0; i < scaled_commands_.size(); i++) 430 | { 431 | combined_obs.push_back(scaled_commands_[i]); 432 | } 433 | 434 | inputValues.push_back( 435 | Ort::Value::CreateTensor(memoryInfo, combined_obs.data(), combined_obs.size(), 436 | policyInputShapes_[0].data(), policyInputShapes_[0].size())); 437 | // Run inference 438 | Ort::RunOptions runOptions; 439 | std::vector outputValues = policySessionPtr_->Run(runOptions, policyInputNames_.data(), 440 | inputValues.data(), 1, policyOutputNames_.data(), 441 | 1); 442 | 443 | for (size_t i = 0; i < actionsSize_; i++) { 444 | actions_[i] = *(outputValues[0].GetTensorMutableData() + i); 445 | } 446 | } 447 | 448 | void WheelfootController::cmdVelCallback(const geometry_msgs::TwistConstPtr &msg) { 449 | // Update the commands with the linear and angular velocities from the Twist message. 450 | 451 | // Set linear x velocity. 452 | commands_(0) = (msg->linear.x > 1.0 ? 1.0 : (msg->linear.x < -1.0 ? -1.0 : msg->linear.x)); 453 | 454 | // Set linear y velocity. 455 | commands_(1) = (msg->linear.y > 1.0 ? 1.0 : (msg->linear.y < -1.0 ? -1.0 : msg->linear.y)); 456 | 457 | // Set angular z velocity. 458 | commands_(2) = (msg->angular.z > 1.0 ? 1.0 : (msg->angular.z < -1.0 ? -1.0 : msg->angular.z)); 459 | } 460 | 461 | } // namespace 462 | 463 | // Export the class as a plugin. 464 | PLUGINLIB_EXPORT_CLASS(robot_controller::WheelfootController, controller_interface::ControllerBase) 465 | -------------------------------------------------------------------------------- /robot_hw/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright information 2 | # 3 | # © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | # Set the minimum required CMake version 6 | cmake_minimum_required(VERSION 3.10) 7 | 8 | project(robot_hw) 9 | 10 | # Enable generation of compile commands database (used by some code editors) 11 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 12 | 13 | ## Find catkin macros and libraries 14 | # Find and specify required catkin packages and components 15 | find_package(catkin REQUIRED 16 | COMPONENTS 17 | roscpp 18 | pointfoot_sdk_lowlevel 19 | robot_common 20 | robot_description 21 | hardware_interface 22 | controller_manager 23 | urdf 24 | ) 25 | 26 | # Specify catkin-specific package configuration 27 | catkin_package( 28 | INCLUDE_DIRS 29 | include 30 | LIBRARIES 31 | ${PROJECT_NAME} 32 | CATKIN_DEPENDS 33 | roscpp 34 | pointfoot_sdk_lowlevel 35 | robot_common 36 | robot_description 37 | hardware_interface 38 | controller_manager 39 | urdf 40 | ) 41 | 42 | find_package(urdfdom REQUIRED) 43 | 44 | # Include directories for header files 45 | include_directories( 46 | include 47 | ${CATKIN_DEVEL_PREFIX}/include 48 | ${EIGEN3_INCLUDE_DIRS} 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | 52 | link_directories(${CATKIN_DEVEL_PREFIX}/lib) 53 | 54 | add_executable(pointfoot_node 55 | ${PROJECT_SOURCE_DIR}/src/RobotHW.cpp 56 | ${PROJECT_SOURCE_DIR}/src/RobotHWLoop.cpp 57 | ${PROJECT_SOURCE_DIR}/src/PointfootHW.cpp 58 | ${PROJECT_SOURCE_DIR}/src/pointfootNode.cpp 59 | ) 60 | 61 | target_link_libraries(pointfoot_node 62 | ${catkin_LIBRARIES} # Link with catkin libraries 63 | ${urdfdom_LIBRARIES} 64 | pointfoot_sdk_lowlevel 65 | pthread 66 | ) 67 | 68 | # Mark executables and/or libraries for installation 69 | install(TARGETS pointfoot_node 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | ) 74 | 75 | # Mark cpp header files for installation 76 | install(DIRECTORY include/. DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 77 | 78 | # Mark resource files for installation 79 | install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 80 | -------------------------------------------------------------------------------- /robot_hw/config/joystick.yaml: -------------------------------------------------------------------------------- 1 | joystick_axes: 2 | left_horizon: 0 3 | left_vertical: 1 4 | right_horizon: 2 5 | right_vertical: 3 6 | 7 | joystick_buttons: 8 | A: 0 9 | B: 1 10 | X: 2 11 | Y: 3 12 | L1: 4 13 | R2: 5 14 | L2: 6 15 | R1: 7 16 | SELECT: 8 17 | START: 9 18 | U: 12 19 | D: 13 20 | L: 14 21 | R: 15 22 | MENU: 16 23 | BACK: 17 24 | -------------------------------------------------------------------------------- /robot_hw/config/joystick_sim.yaml: -------------------------------------------------------------------------------- 1 | teleop: 2 | walk: 3 | type: topic 4 | message_type: geometry_msgs/Twist 5 | topic_name: cmd_vel 6 | deadman_buttons: [ 4 ] 7 | axis_mappings: 8 | - axis: 0 9 | target: angular.z 10 | - axis: 1 11 | target: linear.x 12 | - axis: 3 13 | target: linear.y 14 | -------------------------------------------------------------------------------- /robot_hw/config/robot_hw.yaml: -------------------------------------------------------------------------------- 1 | robot_hw: 2 | loop_frequency: 500 3 | cycle_time_error_threshold: 0.002 4 | power_limit: 6 5 | contact_threshold: 40 6 | 7 | -------------------------------------------------------------------------------- /robot_hw/include/robot_hw/PointfootHW.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_POINTFOOT_HW_H_ 6 | #define _LIMX_POINTFOOT_HW_H_ 7 | 8 | #include 9 | #include 10 | #include "controller_manager_msgs/SwitchController.h" 11 | #include "controller_manager_msgs/ListControllers.h" 12 | #include "realtime_tools/realtime_buffer.h" 13 | #include "geometry_msgs/Twist.h" 14 | #include "sensor_msgs/Joy.h" 15 | #include "limxsdk/pointfoot.h" 16 | 17 | namespace hw { 18 | 19 | const std::vector CONTACT_SENSOR_NAMES = {"L_FOOT", "R_FOOT"}; 20 | 21 | class PointfootHW : public hw::RobotHW { 22 | public: 23 | PointfootHW() = default; 24 | 25 | /** 26 | * \brief Get necessary parameters from the parameter server and initialize hardware_interface. 27 | * 28 | * @param root_nh Root node-handle of a ROS node. 29 | * @param robot_hw_nh Node-handle for robot hardware. 30 | * @return True when initialization is successful, False when failed. 31 | */ 32 | bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) override; 33 | 34 | /** 35 | * \brief Communicate with hardware to get data and status of the robot. 36 | * 37 | * @param time Current time 38 | * @param period Current time - last time 39 | */ 40 | void read(const ros::Time &time, const ros::Duration &period) override; 41 | 42 | /** 43 | * \brief Communicate with hardware to publish commands to the robot. 44 | * 45 | * @param time Current time 46 | * @param period Current time - last time 47 | */ 48 | void write(const ros::Time &time, const ros::Duration &period) override; 49 | 50 | bool loadUrdf(ros::NodeHandle &nh) override; 51 | 52 | bool startBipedController(); 53 | 54 | bool stopBipedController(); 55 | 56 | private: 57 | bool setupJoints(); 58 | 59 | bool setupImu(); 60 | 61 | bool setupContactSensor(ros::NodeHandle &nh); 62 | 63 | std::vector jointData_; // Vector to store motor data for each joint. 64 | ImuData imuData_{}; 65 | 66 | realtime_tools::RealtimeBuffer robotstate_buffer_; 67 | realtime_tools::RealtimeBuffer imudata_buffer_; 68 | limxsdk::RobotCmd robotCmd_; 69 | 70 | bool contactState_[2]{}; // NOLINT(modernize-avoid-c-arrays) 71 | int contactThreshold_{40}; 72 | 73 | int calibration_state_{-1}; 74 | ros::Publisher cmd_vel_pub_; 75 | std::map joystick_btn_map_; 76 | std::map joystick_axes_map_; 77 | ros::ServiceClient switch_controllers_client_; 78 | ros::ServiceClient list_controllers_client_; 79 | 80 | limxsdk::PointFoot *robot_; 81 | 82 | std::string controller_name_; 83 | 84 | std::string robot_type_; // Type of the robot (e.g., point foot, wheel foot, sole foot) 85 | bool is_point_foot_{false}; // Indicates if the robot has a point foot configuration 86 | bool is_wheel_foot_{false}; // Indicates if the robot has a wheel foot configuration 87 | bool is_sole_foot_{false}; // Indicates if the robot has a sole foot configuration 88 | }; 89 | 90 | } // namespace hw 91 | 92 | #endif //_LIMX_POINTFOOT_HW_H_ 93 | -------------------------------------------------------------------------------- /robot_hw/include/robot_hw/RobotData.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_ROBOT_DATA_H_ 6 | #define _LIMX_ROBOT_DATA_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace hw { 13 | 14 | struct MotorData { 15 | MotorData() { 16 | // Initialize command variables 17 | pos_ = vel_ = tau_ = 0.0; 18 | posDes_ = velDes_ = kp_ = kd_ = tau_ff_ = 0.0; 19 | mode_ = 0; 20 | } 21 | // State variables 22 | double pos_, vel_, tau_; 23 | 24 | // Command variables 25 | double posDes_, velDes_, kp_, kd_, tau_ff_; 26 | uint8_t mode_; 27 | }; 28 | 29 | struct ImuData { 30 | ImuData() { 31 | // Initialize orientation 32 | for (std::size_t i = 0; i < 4; i++) { 33 | ori_[i] = 0.0; 34 | } 35 | // Initialize orientation covariance 36 | for (std::size_t i = 0; i < 9; i++) { 37 | oriCov_[i] = 0.0; 38 | } 39 | // Initialize angular velocity 40 | for (std::size_t i = 0; i < 3; i++) { 41 | angularVel_[i] = 0.0; 42 | } 43 | // Initialize angular velocity covariance 44 | for (std::size_t i = 0; i < 9; i++) { 45 | angularVelCov_[i] = 0.0; 46 | } 47 | // Initialize linear acceleration 48 | for (std::size_t i = 0; i < 3; i++) { 49 | linearAcc_[i] = 0.0; 50 | } 51 | // Initialize linear acceleration covariance 52 | for (std::size_t i = 0; i < 9; i++) { 53 | linearAccCov_[i] = 0.0; 54 | } 55 | } 56 | 57 | // Orientation 58 | double ori_[4]; // NOLINT(modernize-avoid-c-arrays) 59 | double oriCov_[9]; // NOLINT(modernize-avoid-c-arrays) 60 | 61 | // Angular velocity 62 | double angularVel_[3]; // NOLINT(modernize-avoid-c-arrays) 63 | double angularVelCov_[9]; // NOLINT(modernize-avoid-c-arrays) 64 | 65 | // Linear acceleration 66 | double linearAcc_[3]; // NOLINT(modernize-avoid-c-arrays) 67 | double linearAccCov_[9]; // NOLINT(modernize-avoid-c-arrays) 68 | }; 69 | 70 | } // namespace hw 71 | 72 | #endif // _LIMX_ROBOT_DATA_H_ 73 | -------------------------------------------------------------------------------- /robot_hw/include/robot_hw/RobotHW.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_ROBOT_HW_H_ 6 | #define _LIMX_ROBOT_HW_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace hw { 23 | 24 | class RobotHW : public hardware_interface::RobotHW { 25 | public: 26 | RobotHW() = default; 27 | 28 | /** 29 | * \brief Initializes the robot hardware. 30 | * 31 | * Initializes the robot hardware interface by setting up joint state, IMU sensor, hybrid joint, and contact sensor interfaces. 32 | * 33 | * @param root_nh Root node-handle of a ROS node. 34 | * @param robot_hw_nh Node-handle for robot hardware. 35 | * @return True if initialization is successful, false otherwise. 36 | */ 37 | bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) override; 38 | 39 | /** 40 | * \brief Retrieves the URDF model of the robot. 41 | * 42 | * Returns the URDF model of the robot. 43 | * 44 | * @return Shared pointer to the URDF model. 45 | */ 46 | virtual std::shared_ptr getUrdfModel() { return urdfModel_; } 47 | 48 | /** 49 | * \brief Loads the URDF model of the robot. 50 | * 51 | * Loads the URDF model of the robot from the ROS parameter server. 52 | * 53 | * @param nh Node-handle for loading URDF. 54 | * @return True if URDF loading is successful, false otherwise. 55 | */ 56 | virtual bool loadUrdf(ros::NodeHandle& nh) { return false; } 57 | 58 | protected: 59 | // Interface 60 | hardware_interface::JointStateInterface jointStateInterface_; // Interface for joint state data. 61 | hardware_interface::ImuSensorInterface imuSensorInterface_; // Interface for IMU sensor data. 62 | robot_common::HybridJointInterface hybridJointInterface_; // Interface for hybrid joint data. 63 | robot_common::ContactSensorInterface contactSensorInterface_; // Interface for contact sensor data. 64 | std::shared_ptr urdfModel_; // Shared pointer to URDF model. 65 | }; 66 | 67 | } // namespace hw 68 | 69 | #endif // _LIMX_ROBOT_HW_H_ 70 | -------------------------------------------------------------------------------- /robot_hw/include/robot_hw/RobotHWLoop.h: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #ifndef _LIMX_ROBOT_HW_LOOP_H_ 6 | #define _LIMX_ROBOT_HW_LOOP_H_ 7 | 8 | #include "robot_hw/RobotHW.h" 9 | #include 10 | #include 11 | #include 12 | 13 | namespace hw { 14 | 15 | class RobotHWLoop { // NOLINT(cppcoreguidelines-special-member-functions) 16 | using Clock = std::chrono::high_resolution_clock; 17 | using Duration = std::chrono::duration; 18 | 19 | public: 20 | /** \brief Create controller manager. 21 | * 22 | * @param nh Node-handle of a ROS node. 23 | * @param hardware_interface A pointer which point to hardware_interface. 24 | */ 25 | RobotHWLoop(ros::NodeHandle& nh, ros::NodeHandle& robot_hw_nh, std::shared_ptr hardware_interface); 26 | 27 | ~RobotHWLoop(); 28 | 29 | /** \brief Timed method that reads current hardware's state, runs the controller code once and sends the new commands 30 | * to the hardware. 31 | * 32 | * Timed method that reads current hardware's state, runs the controller code once and sends the new commands to the 33 | * hardware. 34 | * 35 | */ 36 | void Update(); 37 | 38 | void StartControlLoop(ros::NodeHandle& nh); 39 | 40 | protected: 41 | double cycleTimeErrorThreshold_, loopHz_; 42 | std::thread loopThread_; 43 | std::atomic_bool loopRunning_; 44 | ros::Duration elapsedTime_; 45 | Clock::time_point lastTime_; 46 | std::shared_ptr controllerManager_; 47 | std::shared_ptr hardwareInterface_; 48 | std::string robot_type_; // Type of the robot (e.g., point foot, wheel foot, sole foot) 49 | bool is_point_foot_{false}; // Indicates if the robot has a point foot configuration 50 | bool is_wheel_foot_{false}; // Indicates if the robot has a wheel foot configuration 51 | bool is_sole_foot_{false}; // Indicates if the robot has a sole foot configuration 52 | }; 53 | 54 | } // namespace hw 55 | 56 | #endif // _LIMX_ROBOT_HW_LOOP_H_ -------------------------------------------------------------------------------- /robot_hw/launch/pointfoot_controller_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /robot_hw/launch/pointfoot_hw.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /robot_hw/launch/pointfoot_hw_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /robot_hw/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_hw 4 | 0.0.1 5 | The robot_hw packages. 6 | max 7 | TODO 8 | max 9 | catkin 10 | roscpp 11 | controller_interface 12 | angles 13 | pointfoot_sdk_lowlevel 14 | robot_common 15 | robot_description 16 | hardware_interface 17 | controller_manager 18 | urdf 19 | 20 | -------------------------------------------------------------------------------- /robot_hw/src/PointfootHW.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | /* 6 | * This file contains the implementation of the PointfootHW class, which is a hardware interface 7 | * for controlling a legged robot with point foot contacts. It utilizes ROS (Robot Operating System) 8 | * for communication and control. 9 | */ 10 | 11 | #include "robot_hw/PointfootHW.h" 12 | 13 | namespace hw { 14 | static const std::string CONTROLLER_NAME = "/controllers/pointfoot_controller"; 15 | 16 | // Method to start the biped controller 17 | bool PointfootHW::startBipedController() { 18 | controller_manager_msgs::ListControllers list_controllers; 19 | if (!list_controllers_client_.call(list_controllers)) { 20 | ROS_ERROR("Failed to call list controllers service."); 21 | return false; 22 | } 23 | 24 | for (const auto& controller : list_controllers.response.controller) { 25 | if (controller.name == controller_name_ && controller.state == "running") { 26 | ROS_WARN("Controller %s is already running, skipping start.", controller.name.c_str()); 27 | return false; 28 | } 29 | } 30 | 31 | // Creating a message to switch controllers 32 | controller_manager_msgs::SwitchController sw; 33 | sw.request.start_controllers.push_back(controller_name_); 34 | sw.request.start_asap = false; 35 | sw.request.strictness = controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT; 36 | sw.request.timeout = ros::Duration(3.0).toSec(); 37 | 38 | // Calling the controller_manager service to switch controllers 39 | if (switch_controllers_client_.call(sw.request, sw.response)) { 40 | if (sw.response.ok) { 41 | ROS_INFO("Start controller %s successfully.", sw.request.start_controllers[0].c_str()); 42 | return true; 43 | } else { 44 | ROS_WARN("Start controller %s failed.", sw.request.start_controllers[0].c_str()); 45 | } 46 | } else { 47 | ROS_WARN("Failed to start controller %s.", sw.request.start_controllers[0].c_str()); 48 | } 49 | return false; 50 | } 51 | 52 | // Method to stop the biped controller 53 | bool PointfootHW::stopBipedController() { 54 | // Creating a message to switch controllers 55 | controller_manager_msgs::SwitchController sw; 56 | sw.request.stop_controllers.push_back(controller_name_); 57 | sw.request.start_asap = false; 58 | sw.request.strictness = controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT; 59 | sw.request.timeout = ros::Duration(3.0).toSec(); 60 | 61 | // Calling the controller_manager service to switch controllers 62 | if (switch_controllers_client_.call(sw.request, sw.response)) { 63 | if (sw.response.ok) { 64 | ROS_INFO("Stop controller %s successfully.", sw.request.stop_controllers[0].c_str()); 65 | } else { 66 | ROS_WARN("Stop controller %s failed.", sw.request.stop_controllers[0].c_str()); 67 | } 68 | } else { 69 | ROS_WARN("Failed to stop controller %s.", sw.request.stop_controllers[0].c_str()); 70 | } 71 | 72 | for (int i = 0; i < robot_->getMotorNumber(); ++i) { 73 | robotCmd_.q[i] = jointData_[i].posDes_ = 0.0; 74 | robotCmd_.dq[i] = jointData_[i].velDes_ = 0.0; 75 | robotCmd_.Kp[i] = jointData_[i].kp_ = 0.0; 76 | robotCmd_.tau[i] = jointData_[i].tau_ff_ = 0.0; 77 | robotCmd_.Kd[i] = jointData_[i].kd_ = 1.0; 78 | } 79 | robot_->publishRobotCmd(robotCmd_); 80 | 81 | std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast(1.0 * 1e9))); 82 | 83 | return true; 84 | } 85 | 86 | // Method to initialize the hardware interface 87 | bool PointfootHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { 88 | // Initializing the legged robot instance 89 | robot_ = limxsdk::PointFoot::getInstance(); 90 | 91 | const char* value = ::getenv("ROBOT_TYPE"); 92 | if (value && strlen(value) > 0) { 93 | robot_type_ = std::string(value); 94 | } else { 95 | ROS_ERROR("Error: Please set the ROBOT_TYPE using 'export ROBOT_TYPE='."); 96 | abort(); 97 | } 98 | // Determine the specific robot configuration based on the robot type 99 | is_point_foot_ = (robot_type_.find("PF") != std::string::npos); 100 | is_wheel_foot_ = (robot_type_.find("WF") != std::string::npos); 101 | is_sole_foot_ = (robot_type_.find("SF") != std::string::npos); 102 | 103 | if (is_point_foot_) 104 | { 105 | controller_name_ = "/controllers/pointfoot_controller"; 106 | } 107 | if (is_wheel_foot_) 108 | { 109 | controller_name_ = "/controllers/wheelfoot_controller"; 110 | } 111 | if (is_sole_foot_) 112 | { 113 | controller_name_ = "/controllers/solefoot_controller"; 114 | } 115 | 116 | // Initializing the RobotHW base class 117 | if (!RobotHW::init(root_nh, robot_hw_nh)) { 118 | return false; 119 | } 120 | 121 | // Resize the jointData_ vector to match the number of motors in the robot 122 | jointData_.resize(robot_->getMotorNumber()); 123 | 124 | // Initializing robot command instance, state buffer and imu buffer 125 | robotCmd_ = limxsdk::RobotCmd(robot_->getMotorNumber()); 126 | robotstate_buffer_.writeFromNonRT(limxsdk::RobotState(robot_->getMotorNumber())); 127 | imudata_buffer_.writeFromNonRT(limxsdk::ImuData()); 128 | 129 | // Subscribing to robot state 130 | robot_->subscribeRobotState([this](const limxsdk::RobotStateConstPtr& msg) { 131 | robotstate_buffer_.writeFromNonRT(*msg); 132 | }); 133 | 134 | // Subscribing to robot imu 135 | robot_->subscribeImuData([this](const limxsdk::ImuDataConstPtr& msg) { 136 | imudata_buffer_.writeFromNonRT(*msg); 137 | }); 138 | 139 | // Retrieving joystick configuration parameters 140 | root_nh.getParam("/joystick_buttons", joystick_btn_map_); 141 | for (auto button: joystick_btn_map_) { 142 | ROS_INFO_STREAM("Joystick button: [" << button.first << ", " << button.second << "]"); 143 | } 144 | 145 | root_nh.getParam("/joystick_axes", joystick_axes_map_); 146 | for (auto axes: joystick_axes_map_) { 147 | ROS_INFO_STREAM("Joystick axes: [" << axes.first << ", " << axes.second << "]"); 148 | } 149 | 150 | // When deploying on the real machine, this part receives data from the robot controller and processes it. 151 | // You can customize it according to your needs. 152 | robot_->subscribeSensorJoy([this](const limxsdk::SensorJoyConstPtr& msg) { 153 | // Logic for starting biped controller 154 | if (calibration_state_ == 0 && joystick_btn_map_.count("L1") > 0 && joystick_btn_map_.count("Y") > 0) { 155 | if (msg->buttons[joystick_btn_map_["L1"]] == 1 && msg->buttons[joystick_btn_map_["Y"]] == 1) { 156 | startBipedController(); 157 | } 158 | } 159 | 160 | // Logic for stopping biped controller 161 | if (joystick_btn_map_.count("L1") > 0 && joystick_btn_map_.count("X") > 0) { 162 | if (msg->buttons[joystick_btn_map_["L1"]] == 1 && msg->buttons[joystick_btn_map_["X"]] == 1) { 163 | ROS_FATAL("L1 + X stopping controller!"); 164 | stopBipedController(); 165 | abort(); 166 | } 167 | } 168 | 169 | // Publishing cmd_vel based on joystick input 170 | if (joystick_axes_map_.count("left_horizon") > 0 && joystick_axes_map_.count("left_vertical") > 0 171 | && joystick_axes_map_.count("right_horizon") > 0 && joystick_axes_map_.count("right_vertical") > 0) { 172 | static ros::Time lastpub; 173 | ros::Time now = ros::Time::now(); 174 | if (fabs(now.toSec() - lastpub.toSec()) >= (1.0 / 30)) { 175 | geometry_msgs::Twist twist; 176 | twist.linear.x = msg->axes[joystick_axes_map_["left_vertical"]] * 0.5; 177 | twist.linear.y = msg->axes[joystick_axes_map_["left_horizon"]] * 0.5; 178 | twist.angular.z = msg->axes[joystick_axes_map_["right_horizon"]] * 0.5; 179 | cmd_vel_pub_.publish(twist); 180 | lastpub = now; 181 | } 182 | } 183 | }); 184 | 185 | /* 186 | * Subscribing to diagnostic values for calibration state 187 | */ 188 | robot_->subscribeDiagnosticValue([&](const limxsdk::DiagnosticValueConstPtr& msg) { 189 | // Check if the diagnostic message pertains to calibration 190 | if (msg->name == "calibration") { 191 | ROS_WARN("Calibration state: %d, msg: %s", msg->code, msg->message.c_str()); 192 | calibration_state_ = msg->code; 193 | } 194 | 195 | // Check if the diagnostic message pertains to EtherCAT 196 | if (msg->name == "ethercat" && msg->level == limxsdk::DiagnosticValue::ERROR) { 197 | ROS_FATAL("Ethercat code: %d, msg: %s", msg->code, msg->message.c_str()); 198 | stopBipedController(); 199 | abort(); 200 | } 201 | 202 | // Check if the diagnostic message pertains to IMU 203 | if (msg->name == "imu" && msg->level == limxsdk::DiagnosticValue::ERROR) { 204 | ROS_FATAL("IMU code: %d, msg: %s", msg->code, msg->message.c_str()); 205 | stopBipedController(); 206 | abort(); 207 | } 208 | }); 209 | 210 | // Advertising cmd_vel topic for publishing twist commands 211 | cmd_vel_pub_ = root_nh.advertise("/cmd_vel", 10); 212 | 213 | // Initializing ROS service clients for controller 214 | switch_controllers_client_ = robot_hw_nh.serviceClient("/pointfoot_hw/controller_manager/switch_controller"); 215 | list_controllers_client_ = robot_hw_nh.serviceClient("/pointfoot_hw/controller_manager/list_controllers"); 216 | 217 | // Setting up joints, IMU, and contact sensors 218 | setupJoints(); 219 | setupImu(); 220 | setupContactSensor(robot_hw_nh); 221 | 222 | return true; 223 | } 224 | 225 | // Method to read data from hardware 226 | void PointfootHW::read(const ros::Time& /*time*/, const ros::Duration& /*period*/) { 227 | // Reading robot state 228 | limxsdk::RobotState robotstate = *robotstate_buffer_.readFromRT(); 229 | for (int i = 0; i < robot_->getMotorNumber(); ++i) { 230 | jointData_[i].pos_ = robotstate.q[i]; 231 | jointData_[i].vel_ = robotstate.dq[i]; 232 | jointData_[i].tau_ = robotstate.tau[i]; 233 | } 234 | // Reading imu data 235 | limxsdk::ImuData imudata = *imudata_buffer_.readFromRT(); 236 | imuData_.ori_[0] = imudata.quat[1]; 237 | imuData_.ori_[1] = imudata.quat[2]; 238 | imuData_.ori_[2] = imudata.quat[3]; 239 | imuData_.ori_[3] = imudata.quat[0]; 240 | imuData_.angularVel_[0] = imudata.gyro[0]; 241 | imuData_.angularVel_[1] = imudata.gyro[1]; 242 | imuData_.angularVel_[2] = imudata.gyro[2]; 243 | imuData_.linearAcc_[0] = imudata.acc[0]; 244 | imuData_.linearAcc_[1] = imudata.acc[1]; 245 | imuData_.linearAcc_[2] = imudata.acc[2]; 246 | } 247 | 248 | // Method to write commands to hardware 249 | void PointfootHW::write(const ros::Time& /*time*/, const ros::Duration& /*period*/) { 250 | // Writing commands to robot 251 | for (int i = 0; i < robot_->getMotorNumber(); ++i) { 252 | robotCmd_.q[i] = static_cast(jointData_[i].posDes_); 253 | robotCmd_.dq[i] = static_cast(jointData_[i].velDes_); 254 | robotCmd_.Kp[i] = static_cast(jointData_[i].kp_); 255 | robotCmd_.Kd[i] = static_cast(jointData_[i].kd_); 256 | robotCmd_.tau[i] = static_cast(jointData_[i].tau_ff_); 257 | robotCmd_.mode[i] = static_cast(jointData_[i].mode_); 258 | } 259 | 260 | // Publishing robot commands 261 | if (calibration_state_ == 0) { 262 | robot_->publishRobotCmd(robotCmd_); 263 | } 264 | } 265 | 266 | // Method to setup joints based on URDF 267 | bool PointfootHW::setupJoints() { 268 | for (const auto& joint : urdfModel_->joints_) { 269 | int leg_index; 270 | int joint_index; 271 | if (joint.first.find("L_") != std::string::npos) 272 | leg_index = 0; 273 | else if (joint.first.find("R_") != std::string::npos) 274 | leg_index = 1; 275 | else 276 | continue; 277 | 278 | if (joint.first.find("abad") != std::string::npos) 279 | joint_index = 0; 280 | else if (joint.first.find("hip") != std::string::npos) 281 | joint_index = 1; 282 | else if (joint.first.find("knee") != std::string::npos) 283 | joint_index = 2; 284 | else if (joint.first.find("wheel") != std::string::npos) 285 | joint_index = 3; 286 | else if (joint.first.find("ankle") != std::string::npos) 287 | joint_index = 3; 288 | else 289 | continue; 290 | 291 | int index = leg_index * robot_->getMotorNumber() / 2 + joint_index; 292 | hardware_interface::JointStateHandle state_handle(joint.first, &jointData_[index].pos_, &jointData_[index].vel_, 293 | &jointData_[index].tau_); 294 | jointStateInterface_.registerHandle(state_handle); 295 | hybridJointInterface_.registerHandle(robot_common::HybridJointHandle(state_handle, &jointData_[index].posDes_, 296 | &jointData_[index].velDes_, 297 | &jointData_[index].kp_, &jointData_[index].kd_, 298 | &jointData_[index].tau_ff_, &jointData_[index].mode_)); 299 | } 300 | 301 | return true; 302 | } 303 | 304 | // Method to setup IMU sensor 305 | bool PointfootHW::setupImu() { 306 | imuSensorInterface_.registerHandle(hardware_interface::ImuSensorHandle("limx_imu", "limx_imu", imuData_.ori_, 307 | imuData_.oriCov_, imuData_.angularVel_, 308 | imuData_.angularVelCov_, imuData_.linearAcc_, 309 | imuData_.linearAccCov_)); 310 | return true; 311 | } 312 | 313 | // Method to setup contact sensors 314 | bool PointfootHW::setupContactSensor(ros::NodeHandle& nh) { 315 | nh.getParam("/robot_hw/contact_threshold", contactThreshold_); 316 | for (size_t i = 0; i < CONTACT_SENSOR_NAMES.size(); ++i) { 317 | contactSensorInterface_.registerHandle(robot_common::ContactSensorHandle(CONTACT_SENSOR_NAMES[i], &contactState_[i])); 318 | } 319 | return true; 320 | } 321 | 322 | // Method to load URDF model 323 | bool PointfootHW::loadUrdf(ros::NodeHandle& nh) { 324 | std::string urdfString; 325 | if (urdfModel_ == nullptr) { 326 | urdfModel_ = std::make_shared(); 327 | } 328 | // Getting the URDF parameter from the parameter server 329 | nh.getParam("robot_description", urdfString); 330 | return !urdfString.empty() && urdfModel_->initString(urdfString); 331 | } 332 | 333 | } // namespace hw 334 | -------------------------------------------------------------------------------- /robot_hw/src/RobotHW.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #include "robot_hw/RobotHW.h" 6 | 7 | namespace hw { 8 | // Initialize the RobotHW 9 | bool RobotHW::init(ros::NodeHandle& root_nh, ros::NodeHandle & /*robot_hw_nh*/) { 10 | // Load URDF for the robot 11 | if (!loadUrdf(root_nh)) { 12 | ROS_ERROR("Error occurred while setting up URDF"); 13 | return false; 14 | } 15 | 16 | // Register hardware interfaces 17 | registerInterface(&jointStateInterface_); 18 | registerInterface(&hybridJointInterface_); 19 | registerInterface(&imuSensorInterface_); 20 | registerInterface(&contactSensorInterface_); 21 | 22 | return true; 23 | } 24 | } // namespace hw 25 | 26 | -------------------------------------------------------------------------------- /robot_hw/src/RobotHWLoop.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #include 6 | #include "robot_hw/RobotHWLoop.h" 7 | 8 | namespace hw { 9 | // Constructor for the RobotHWLoop class 10 | RobotHWLoop::RobotHWLoop(ros::NodeHandle &nh, ros::NodeHandle &robot_hw_nh, std::shared_ptr hardware_interface) 11 | : hardwareInterface_(std::move(hardware_interface)), loopRunning_(true) { 12 | controllerManager_.reset(new controller_manager::ControllerManager(hardwareInterface_.get(), robot_hw_nh)); 13 | } 14 | 15 | void RobotHWLoop::StartControlLoop(ros::NodeHandle& nh) { 16 | 17 | std::string controller_name; 18 | 19 | const char* value = ::getenv("ROBOT_TYPE"); 20 | if (value && strlen(value) > 0) { 21 | robot_type_ = std::string(value); 22 | } else { 23 | ROS_ERROR("Error: Please set the ROBOT_TYPE using 'export ROBOT_TYPE='."); 24 | abort(); 25 | } 26 | 27 | // Determine the specific robot configuration based on the robot type 28 | is_point_foot_ = (robot_type_.find("PF") != std::string::npos); 29 | is_wheel_foot_ = (robot_type_.find("WF") != std::string::npos); 30 | is_sole_foot_ = (robot_type_.find("SF") != std::string::npos); 31 | 32 | if (is_point_foot_) 33 | { 34 | controller_name = "/controllers/pointfoot_controller"; 35 | } 36 | if (is_wheel_foot_) 37 | { 38 | controller_name = "/controllers/wheelfoot_controller"; 39 | } 40 | if (is_sole_foot_) 41 | { 42 | controller_name = "/controllers/solefoot_controller"; 43 | } 44 | 45 | std::cerr << "controller_name = " << controller_name << std::endl; 46 | controllerManager_->loadController(controller_name); 47 | 48 | // Load ros params 49 | int error = 0; 50 | 51 | loopHz_ = nh.param("/robot_hw/loop_frequency", 500); 52 | cycleTimeErrorThreshold_ = nh.param("/robot_hw/cycle_time_error_threshold", 0.002); 53 | 54 | ROS_INFO("Load param:\nloop_frequency: %f, \ncycleTimeErrorThreshold: %f", 55 | loopHz_, cycleTimeErrorThreshold_); 56 | 57 | // Get current time for use with first update 58 | lastTime_ = Clock::now(); 59 | 60 | // Setup loop thread 61 | loopThread_ = std::thread([&]() { 62 | while (loopRunning_) { 63 | Update(); 64 | } 65 | }); 66 | } 67 | 68 | // Update function for the hardware loop 69 | void RobotHWLoop::Update() { 70 | const auto currentTime = Clock::now(); 71 | // Compute desired duration rounded to clock decimation 72 | const Duration desiredDuration(1.0 / loopHz_); 73 | 74 | // Get change in time 75 | Duration time_span = std::chrono::duration_cast(currentTime - lastTime_); 76 | elapsedTime_ = ros::Duration(time_span.count()); 77 | lastTime_ = currentTime; 78 | 79 | // Check cycle time for excess delay 80 | const double cycle_time_error = (elapsedTime_ - ros::Duration(desiredDuration.count())).toSec(); 81 | if (cycle_time_error > cycleTimeErrorThreshold_) 82 | { 83 | ROS_WARN("Cycle time exceeded error threshold by: %fs, cycle time: %fs, threshold: %fs", 84 | cycle_time_error - cycleTimeErrorThreshold_, elapsedTime_.toSec(), cycleTimeErrorThreshold_); 85 | } 86 | 87 | // Input 88 | // Get the hardware's state 89 | hardwareInterface_->read(ros::Time::now(), elapsedTime_); 90 | 91 | // Control 92 | // Let the controller compute the new command (via the controller manager) 93 | controllerManager_->update(ros::Time::now(), elapsedTime_); 94 | 95 | // Output 96 | // Send the new command to hardware 97 | hardwareInterface_->write(ros::Time::now(), elapsedTime_); 98 | 99 | // Sleep 100 | const auto sleepTill = currentTime + std::chrono::duration_cast(desiredDuration); 101 | std::this_thread::sleep_until(sleepTill); 102 | } 103 | 104 | // Destructor for the RobotHWLoop class 105 | RobotHWLoop::~RobotHWLoop() { 106 | loopRunning_ = false; 107 | if (loopThread_.joinable()) { 108 | loopThread_.join(); 109 | } 110 | } 111 | } // namespace hw 112 | -------------------------------------------------------------------------------- /robot_hw/src/pointfootNode.cpp: -------------------------------------------------------------------------------- 1 | // Copyright information 2 | // 3 | // © [2024] LimX Dynamics Technology Co., Ltd. All rights reserved. 4 | 5 | #include "ros/ros.h" 6 | #include "robot_hw/RobotHWLoop.h" 7 | #include "robot_hw/PointfootHW.h" 8 | #include "limxsdk/pointfoot.h" 9 | 10 | // Main function 11 | int main(int argc, char **argv) { 12 | ros::init(argc, argv, "pointfoot_hw"); 13 | 14 | std::string robot_ip = "127.0.0.1"; // Default robot IP address 15 | if (argc > 1) { 16 | robot_ip = argv[1]; // Use command-line argument as robot IP address if provided 17 | } 18 | 19 | // Initialize PointFoot instance with robot IP 20 | if (!limxsdk::PointFoot::getInstance()->init(robot_ip)) { 21 | ROS_ERROR("Failed to connect to the robot: %s", robot_ip.c_str()); 22 | abort(); 23 | } else { 24 | ROS_WARN("Connect to the robot: %s", robot_ip.c_str()); 25 | } 26 | 27 | try { 28 | ros::NodeHandle nh; 29 | ros::NodeHandle robot_hw_nh("~"); 30 | 31 | // Create and initialize PointfootHW instance 32 | std::shared_ptr hw = std::make_shared(); 33 | hw->init(nh, robot_hw_nh); 34 | 35 | // Create and initialize RobotHWLoop instance 36 | hw::RobotHWLoop loop(nh, robot_hw_nh, hw); 37 | loop.StartControlLoop(robot_hw_nh); 38 | 39 | // Check if Gazebo should be used 40 | bool use_gazebo; 41 | if (!nh.getParam("use_gazebo", use_gazebo)) { 42 | ROS_WARN("Failed to get param 'use_gazebo', defaulting to false"); 43 | use_gazebo = false; 44 | } 45 | 46 | // If Gazebo is being used, start the Biped controller in a detached thread 47 | if (use_gazebo) { 48 | std::thread controller_thread([hw]() { 49 | std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast(3.0 * 1e9))); 50 | hw->startBipedController(); 51 | }); 52 | controller_thread.detach(); 53 | } 54 | 55 | // Start ROS event loop 56 | ros::spin(); 57 | } catch (const std::exception &e) { 58 | ROS_ERROR("Error in the hardware interface: %s", e.what()); 59 | return 1; 60 | } 61 | 62 | return 0; 63 | } 64 | --------------------------------------------------------------------------------