├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── controllers ├── PointfootController.py ├── SolefootController.py ├── WheelfootController.py ├── __init__.py └── model │ ├── 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 ├── doc └── simulator.gif └── main.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pointfoot-sdk-lowlevel"] 2 | path = pointfoot-sdk-lowlevel 3 | url = https://github.com/limxdynamics/pointfoot-sdk-lowlevel.git 4 | -------------------------------------------------------------------------------- /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 | # rl-deploy-with-python 2 | 3 | 4 | 5 | ## 1. 运行仿真 6 | 7 | - 打开一个 Bash 终端。 8 | 9 | - 下载 MuJoCo 仿真器代码： 10 | 11 | ``` 12 | git clone --recurse https://github.com/limxdynamics/pointfoot-mujoco-sim.git 13 | ``` 14 | 15 | - 安装运动控制开发库（如果尚未安装）： 16 | 17 | - Linux x86_64 环境 18 | 19 | ``` 20 | pip install pointfoot-mujoco-sim/pointfoot-sdk-lowlevel/python3/amd64/limxsdk-*-py3-none-any.whl 21 | ``` 22 | 23 | - Linux aarch64 环境 24 | 25 | ``` 26 | pip install pointfoot-mujoco-sim/pointfoot-sdk-lowlevel/python3/aarch64/limxsdk-*-py3-none-any.whl 27 | ``` 28 | 29 | - 设置机器人类型 30 | 31 | - 通过 Shell 命令 `tree -L 1 pointfoot-mujoco-sim/robot-description/pointfoot` 列出可用的机器人类型： 32 | 33 | ``` 34 | limx@limx:~$ tree -L 1 pointfoot-mujoco-sim/robot-description/pointfoot 35 | pointfoot-mujoco-sim/robot-description/pointfoot 36 | ├── PF_P441A 37 | ├── PF_P441B 38 | ├── PF_P441C 39 | └── PF_P441C2 40 | 41 | ``` 42 | 43 | - 以`PF_P441C`（请根据实际机器人类型进行替换）为例，设置机器人型号类型： 44 | 45 | ``` 46 | echo 'export ROBOT_TYPE=PF_P441C' >> ~/.bashrc && source ~/.bashrc 47 | ``` 48 | 49 | - 运行 MuJoCo 仿真器： 50 | 51 | ``` 52 | python pointfoot-mujoco-sim/simulator.py 53 | ``` 54 | 55 | 56 | 57 | ## 2. 运行控制算法 58 | 59 | - 打开一个 Bash 终端。 60 | 61 | - 下载控制算法代码： 62 | 63 | ``` 64 | git clone --recurse https://github.com/limxdynamics/rl-deploy-with-python.git 65 | ``` 66 | 67 | - 安装运动控制开发库（如果尚未安装）： 68 | 69 | - Linux x86_64 环境 70 | 71 | ``` 72 | pip install rl-deploy-with-python/pointfoot-sdk-lowlevel/python3/amd64/limxsdk-*-py3-none-any.whl 73 | ``` 74 | 75 | - Linux aarch64 环境 76 | 77 | ``` 78 | pip install rl-deploy-with-python/pointfoot-sdk-lowlevel/python3/aarch64/limxsdk-*-py3-none-any.whl 79 | ``` 80 | 81 | - 设置机器人类型 82 | 83 | - 通过 Shell 命令 `tree -L 1 rl-deploy-with-python/controllers/model` 列出可用的机器人类型： 84 | 85 | ``` 86 | limx@limx:~$ tree -L 1 rl-deploy-with-python/controllers/model 87 | rl-deploy-with-python/controllers/model 88 | ├── PF_P441A 89 | ├── PF_P441B 90 | ├── PF_P441C 91 | ├── PF_P441C2 92 | ├── PF_TRON1A 93 | ├── SF_TRON1A 94 | └── WF_TRON1A 95 | 96 | ``` 97 | 98 | - 以`PF_P441C`（请根据实际机器人类型进行替换）为例，设置机器人型号类型： 99 | 100 | ``` 101 | echo 'export ROBOT_TYPE=PF_P441C' >> ~/.bashrc && source ~/.bashrc 102 | ``` 103 | 104 | - 运行控制算法： 105 | 106 | ``` 107 | python rl-deploy-with-python/main.py 108 | ``` 109 | 110 | ## 3. 虚拟遥控器 111 | 112 | - 打开一个 Bash 终端。 113 | 114 | - 运行 robot-joystick： 115 | 116 | ``` 117 | ./pointfoot-mujoco-sim/robot-joystick/robot-joystick 118 | ``` 119 | 120 | ## 4. 效果展示 121 | ![](doc/simulator.gif) 122 | 123 | -------------------------------------------------------------------------------- /controllers/PointfootController.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import copy 4 | import numpy as np 5 | import yaml 6 | import time 7 | import onnxruntime as ort 8 | from scipy.spatial.transform import Rotation as R 9 | from functools import partial 10 | import limxsdk 11 | import limxsdk.robot.Rate as Rate 12 | import limxsdk.robot.Robot as Robot 13 | import limxsdk.robot.RobotType as RobotType 14 | import limxsdk.datatypes as datatypes 15 | 16 | class PointfootController: 17 | def __init__(self, model_dir, robot, robot_type, start_controller): 18 | # Initialize robot and type information 19 | self.robot = robot 20 | self.robot_type = robot_type 21 | 22 | # Load configuration and model file paths based on robot type 23 | self.config_file = f'{model_dir}/{self.robot_type}/params.yaml' 24 | self.model_policy = f'{model_dir}/{self.robot_type}/policy/policy.onnx' 25 | self.model_encoder = f'{model_dir}/{self.robot_type}/policy/encoder.onnx' 26 | 27 | # Load configuration settings from the YAML file 28 | self.load_config(self.config_file) 29 | 30 | # Load the ONNX model and set up input and output names 31 | self.policy_session = ort.InferenceSession(self.model_policy) 32 | self.policy_input_names = [self.policy_session.get_inputs()[i].name for i in range(self.policy_session.get_inputs().__len__())] 33 | self.policy_output_names = [self.policy_session.get_outputs()[i].name for i in range(self.policy_session.get_outputs().__len__())] 34 | self.policy_input_shapes = [self.policy_session.get_inputs()[i].shape for i in range(self.policy_session.get_inputs().__len__())] 35 | self.policy_output_shapes = [self.policy_session.get_outputs()[i].shape for i in range(self.policy_session.get_outputs().__len__())] 36 | 37 | self.encoder_session = ort.InferenceSession(self.model_encoder) 38 | self.encoder_input_names = [self.encoder_session.get_inputs()[i].name for i in range(self.encoder_session.get_inputs().__len__())] 39 | self.encoder_output_names = [self.encoder_session.get_outputs()[i].name for i in range(self.encoder_session.get_outputs().__len__())] 40 | self.encoder_input_shapes = [self.encoder_session.get_inputs()[i].shape for i in range(self.encoder_session.get_inputs().__len__())] 41 | self.encoder_output_shapes = [self.encoder_session.get_outputs()[i].shape for i in range(self.encoder_session.get_outputs().__len__())] 42 | 43 | # Prepare robot command structure with default values for mode, q, dq, tau, Kp, Kd 44 | self.robot_cmd = datatypes.RobotCmd() 45 | self.robot_cmd.mode = [0. for x in range(0, self.joint_num)] 46 | self.robot_cmd.q = [0. for x in range(0, self.joint_num)] 47 | self.robot_cmd.dq = [0. for x in range(0, self.joint_num)] 48 | self.robot_cmd.tau = [0. for x in range(0, self.joint_num)] 49 | self.robot_cmd.Kp = [self.control_cfg['stiffness'] for x in range(0, self.joint_num)] 50 | self.robot_cmd.Kd = [self.control_cfg['damping'] for x in range(0, self.joint_num)] 51 | 52 | # Prepare robot state structure 53 | self.robot_state = datatypes.RobotState() 54 | self.robot_state.tau = [0. for x in range(0, self.joint_num)] 55 | self.robot_state.q = [0. for x in range(0, self.joint_num)] 56 | self.robot_state.dq = [0. for x in range(0, self.joint_num)] 57 | self.robot_state_tmp = copy.deepcopy(self.robot_state) 58 | 59 | # Initialize IMU (Inertial Measurement Unit) data structure 60 | self.imu_data = datatypes.ImuData() 61 | self.imu_data.quat[0] = 0 62 | self.imu_data.quat[1] = 0 63 | self.imu_data.quat[2] = 0 64 | self.imu_data.quat[3] = 1 65 | self.imu_data_tmp = copy.deepcopy(self.imu_data) 66 | 67 | # Set up a callback to receive updated robot state data 68 | self.robot_state_callback_partial = partial(self.robot_state_callback) 69 | self.robot.subscribeRobotState(self.robot_state_callback_partial) 70 | 71 | # Set up a callback to receive updated IMU data 72 | self.imu_data_callback_partial = partial(self.imu_data_callback) 73 | self.robot.subscribeImuData(self.imu_data_callback_partial) 74 | 75 | # Set up a callback to receive updated SensorJoy 76 | self.sensor_joy_callback_partial = partial(self.sensor_joy_callback) 77 | self.robot.subscribeSensorJoy(self.sensor_joy_callback_partial) 78 | 79 | # Set up a callback to receive diagnostic data 80 | self.robot_diagnostic_callback_partial = partial(self.robot_diagnostic_callback) 81 | self.robot.subscribeDiagnosticValue(self.robot_diagnostic_callback_partial) 82 | 83 | # Initialize the calibration state to -1, indicating no calibration has occurred. 84 | self.calibration_state = -1 85 | 86 | # Flag to start the controller 87 | self.start_controller = start_controller 88 | 89 | # Gait index 90 | self.gait_index = 0 91 | 92 | # Flag indicating first received observation 93 | self.is_first_rec_obs = True 94 | 95 | # Load the configuration from a YAML file 96 | def load_config(self, config_file): 97 | with open(config_file, 'r') as f: 98 | config = yaml.safe_load(f) 99 | 100 | # Assign configuration parameters to controller variables 101 | self.joint_names = config['PointfootCfg']['joint_names'] 102 | self.init_state = config['PointfootCfg']['init_state']['default_joint_angle'] 103 | self.stand_duration = config['PointfootCfg']['stand_mode']['stand_duration'] 104 | self.control_cfg = config['PointfootCfg']['control'] 105 | self.rl_cfg = config['PointfootCfg']['normalization'] 106 | self.obs_scales = config['PointfootCfg']['normalization']['obs_scales'] 107 | self.actions_size = config['PointfootCfg']['size']['actions_size'] 108 | self.commands_size = config['PointfootCfg']['size']['commands_size'] 109 | self.observations_size = config['PointfootCfg']['size']['observations_size'] 110 | self.obs_history_length = config['PointfootCfg']['size']['obs_history_length'] 111 | self.encoder_output_size = config['PointfootCfg']['size']['encoder_output_size'] 112 | self.imu_orientation_offset = np.array(list(config['PointfootCfg']['imu_orientation_offset'].values())) 113 | self.user_cmd_cfg = config['PointfootCfg']['user_cmd_scales'] 114 | self.loop_frequency = config['PointfootCfg']['loop_frequency'] 115 | self.encoder_input_size = self.obs_history_length * self.observations_size 116 | 117 | # Initialize variables for actions, observations, and commands 118 | self.proprio_history_vector = np.zeros(self.obs_history_length * self.observations_size) 119 | self.encoder_out = np.zeros(self.encoder_output_size) 120 | self.actions = np.zeros(self.actions_size) 121 | self.observations = np.zeros(self.observations_size) 122 | self.last_actions = np.zeros(self.actions_size) 123 | self.commands = np.zeros(self.commands_size) # command to the robot (e.g., velocity, rotation) 124 | self.scaled_commands = np.zeros(self.commands_size) 125 | self.base_lin_vel = np.zeros(3) # base linear velocity 126 | self.base_position = np.zeros(3) # robot base position 127 | self.loop_count = 0 # loop iteration count 128 | self.stand_percent = 0 # percentage of time the robot has spent in stand mode 129 | self.policy_session = None # ONNX model session for policy inference 130 | self.joint_num = len(self.joint_names) # number of joints 131 | 132 | # Initialize joint angles based on the initial configuration 133 | self.init_joint_angles = np.zeros(len(self.joint_names)) 134 | for i in range(len(self.joint_names)): 135 | self.init_joint_angles[i] = self.init_state[self.joint_names[i]] 136 | 137 | # Set initial mode to "STAND" 138 | self.mode = "STAND" 139 | 140 | # Main control loop 141 | def run(self): 142 | # Wait until the controller is started 143 | while not self.start_controller: 144 | time.sleep(1) 145 | 146 | # Initialize default joint angles for standing 147 | self.default_joint_angles = np.array([0.0] * len(self.joint_names)) 148 | self.stand_percent += 1 / (self.stand_duration * self.loop_frequency) 149 | self.mode = "STAND" 150 | self.loop_count = 0 151 | 152 | # Set the loop rate based on the frequency in the configuration 153 | rate = Rate(self.loop_frequency) 154 | while self.start_controller: 155 | self.update() 156 | rate.sleep() 157 | 158 | # Reset robot command values to ensure a safe stop when exiting the loop 159 | self.robot_cmd.q = [0. for x in range(0, self.joint_num)] 160 | self.robot_cmd.dq = [0. for x in range(0, self.joint_num)] 161 | self.robot_cmd.tau = [0. for x in range(0, self.joint_num)] 162 | self.robot_cmd.Kp = [0. for x in range(0, self.joint_num)] 163 | self.robot_cmd.Kd = [1.0 for x in range(0, self.joint_num)] 164 | self.robot.publishRobotCmd(self.robot_cmd) 165 | time.sleep(1) 166 | 167 | # Handle the stand mode for smoothly transitioning the robot into standing 168 | def handle_stand_mode(self): 169 | if self.stand_percent < 1: 170 | for j in range(len(self.joint_names)): 171 | # Interpolate between initial and default joint angles during stand mode 172 | pos_des = self.default_joint_angles[j] * (1 - self.stand_percent) + self.init_state[self.joint_names[j]] * self.stand_percent 173 | self.set_joint_command(j, pos_des, 0, 0, self.control_cfg['stiffness'], self.control_cfg['damping']) 174 | # Increment the stand percentage over time 175 | self.stand_percent += 1 / (self.stand_duration * self.loop_frequency) 176 | else: 177 | # Switch to walk mode after standing 178 | self.mode = "WALK" 179 | 180 | # Handle the walk mode where the robot moves based on computed actions 181 | def handle_walk_mode(self): 182 | # Update the temporary robot state and IMU data 183 | self.robot_state_tmp = copy.deepcopy(self.robot_state) 184 | self.imu_data_tmp = copy.deepcopy(self.imu_data) 185 | 186 | # Execute actions every 'decimation' iterations 187 | if self.loop_count % self.control_cfg['decimation'] == 0: 188 | self.compute_observation() 189 | self.compute_encoder() 190 | self.compute_actions() 191 | # Clip the actions within predefined limits 192 | action_min = -self.rl_cfg['clip_scales']['clip_actions'] 193 | action_max = self.rl_cfg['clip_scales']['clip_actions'] 194 | self.actions = np.clip(self.actions, action_min, action_max) 195 | 196 | # Iterate over the joints and set commands based on actions 197 | joint_pos = np.array(self.robot_state_tmp.q) 198 | joint_vel = np.array(self.robot_state_tmp.dq) 199 | 200 | for i in range(len(joint_pos)): 201 | # Compute the limits for the action based on joint position and velocity 202 | action_min = (joint_pos[i] - self.init_joint_angles[i] + 203 | (self.control_cfg['damping'] * joint_vel[i] - self.control_cfg['user_torque_limit']) / 204 | self.control_cfg['stiffness']) 205 | action_max = (joint_pos[i] - self.init_joint_angles[i] + 206 | (self.control_cfg['damping'] * joint_vel[i] + self.control_cfg['user_torque_limit']) / 207 | self.control_cfg['stiffness']) 208 | 209 | # Clip action within limits 210 | self.actions[i] = max(action_min / self.control_cfg['action_scale_pos'], 211 | min(action_max / self.control_cfg['action_scale_pos'], self.actions[i])) 212 | 213 | # Compute the desired joint position and set it 214 | pos_des = self.actions[i] * self.control_cfg['action_scale_pos'] + self.init_joint_angles[i] 215 | self.set_joint_command(i, pos_des, 0, 0, self.control_cfg['stiffness'], self.control_cfg['damping']) 216 | 217 | # Save the last action for reference 218 | self.last_actions[i] = self.actions[i] 219 | 220 | def compute_observation(self): 221 | # Convert IMU orientation from quaternion to Euler angles (ZYX convention) 222 | imu_orientation = np.array(self.imu_data_tmp.quat) 223 | q_wi = R.from_quat(imu_orientation).as_euler('zyx') # Quaternion to Euler ZYX conversion 224 | inverse_rot = R.from_euler('zyx', q_wi).inv().as_matrix() # Get the inverse rotation matrix 225 | 226 | # Project the gravity vector (pointing downwards) into the body frame 227 | gravity_vector = np.array([0, 0, -1]) # Gravity in world frame (z-axis down) 228 | projected_gravity = np.dot(inverse_rot, gravity_vector) # Transform gravity into body frame 229 | 230 | # Retrieve base angular velocity from the IMU data 231 | base_ang_vel = np.array(self.imu_data_tmp.gyro) 232 | # Apply IMU orientation offset correction (using Euler angles) 233 | rot = R.from_euler('zyx', self.imu_orientation_offset).as_matrix() # Rotation matrix for offset correction 234 | base_ang_vel = np.dot(rot, base_ang_vel) # Apply correction to angular velocity 235 | projected_gravity = np.dot(rot, projected_gravity) # Apply correction to projected gravity 236 | 237 | # Retrieve joint positions and velocities from the robot state 238 | joint_positions = np.array(self.robot_state_tmp.q) 239 | joint_velocities = np.array(self.robot_state_tmp.dq) 240 | 241 | gait = np.array([2.0, 0.5, 0.5, 0.1]) 242 | self.gait_index += 0.02 * gait[0] 243 | if self.gait_index > 1.0: 244 | self.gait_index = 0.0 245 | gait_clock = np.array([np.sin(self.gait_index * 2 * np.pi), np.cos(self.gait_index * 2 * np.pi)]) 246 | 247 | # Retrieve the last actions that were applied to the robot 248 | actions = np.array(self.last_actions) 249 | 250 | # Create a command scaler matrix for linear and angular velocities 251 | command_scaler = np.diag([ 252 | self.user_cmd_cfg['lin_vel_x'], # Scale factor for linear velocity in x direction 253 | self.user_cmd_cfg['lin_vel_y'], # Scale factor for linear velocity in y direction 254 | self.user_cmd_cfg['ang_vel_yaw'] # Scale factor for yaw (angular velocity) 255 | ]) 256 | 257 | # Apply scaling to the command inputs (velocity commands) 258 | self.scaled_commands = np.dot(command_scaler, self.commands) 259 | 260 | # Populate observation vector 261 | joint_pos_input = (joint_positions - self.init_joint_angles) * self.obs_scales['dof_pos'] 262 | 263 | # Create the observation vector by concatenating various state variables: 264 | # - Base angular velocity (scaled) 265 | # - Projected gravity vector 266 | # - Joint positions (difference from initial angles, scaled) 267 | # - Joint velocities (scaled) 268 | # - Last actions applied to the robot 269 | # - gait_clock: A clock signal related to the gait of the robot. 270 | # - gait: Information about the current gait of the robot. 271 | obs = np.concatenate([ 272 | base_ang_vel * self.obs_scales['ang_vel'], # Scaled base angular velocity 273 | projected_gravity, # Projected gravity vector in body frame 274 | joint_pos_input, # Scaled joint positions 275 | joint_velocities * self.obs_scales['dof_vel'], # Scaled joint velocities 276 | actions, # Last actions taken by the robot 277 | gait_clock, # A clock signal related to the robot's gait 278 | gait # Information about the current gait pattern of the robot 279 | ]) 280 | 281 | # Check if this is the first recorded observation 282 | if self.is_first_rec_obs: 283 | # Calculate the total size of the encoder input 284 | input_size = np.prod(self.encoder_input_shapes[0]) 285 | 286 | # Initialize the proprioceptive history buffer with zeros 287 | self.proprio_history_buffer = np.zeros(input_size) 288 | 289 | # Fill the proprioceptive history buffer with the current observation for the entire history length 290 | for i in range(self.obs_history_length): 291 | self.proprio_history_buffer[i * self.observations_size:(i + 1) * self.observations_size] = obs 292 | 293 | # Update the flag to indicate that the first observation has been processed 294 | self.is_first_rec_obs = False 295 | 296 | # Shift the existing proprioceptive history buffer to the left 297 | self.proprio_history_buffer[:-self.observations_size] = self.proprio_history_buffer[self.observations_size:] 298 | 299 | # Add the current observation to the end of the proprioceptive history buffer 300 | self.proprio_history_buffer[-self.observations_size:] = obs 301 | 302 | # Convert the proprioceptive history buffer to a numpy array 303 | self.proprio_history_vector = np.array(self.proprio_history_buffer) 304 | 305 | # Clip the observation values to within the specified limits for stability 306 | self.observations = np.clip( 307 | obs, 308 | -self.rl_cfg['clip_scales']['clip_observations'], # Lower limit for clipping 309 | self.rl_cfg['clip_scales']['clip_observations'] # Upper limit for clipping 310 | ) 311 | 312 | def compute_actions(self): 313 | """ 314 | Computes the actions based on the current observations using the policy session. 315 | """ 316 | # Concatenate observations into a single tensor and convert to float32 317 | input_tensor = np.concatenate([self.encoder_out, self.observations, self.scaled_commands], axis=0) 318 | input_tensor = input_tensor.astype(np.float32) 319 | 320 | # Create a dictionary of inputs for the policy session 321 | inputs = {self.policy_input_names[0]: input_tensor} 322 | 323 | # Run the policy session and get the output 324 | output = self.policy_session.run(self.policy_output_names, inputs) 325 | 326 | # Flatten the output and store it as actions 327 | self.actions = np.array(output).flatten() 328 | 329 | def compute_encoder(self): 330 | """ 331 | Computes the encoder output based on the proprioceptive history buffer. 332 | 333 | This method first concatenates the proprioceptive history buffer into a single input tensor. 334 | Then it converts the input tensor to the float32 data type. After that, it creates a dictionary 335 | of inputs for the encoder session and runs the encoder session to get the output. Finally, 336 | it flattens the output and stores it as the encoder output. 337 | """ 338 | # Concatenate the proprioceptive history buffer into a single tensor and convert to float32 339 | input_tensor = np.concatenate([self.proprio_history_buffer], axis=0) 340 | input_tensor = input_tensor.astype(np.float32) 341 | 342 | # Create a dictionary of inputs for the encoder session 343 | inputs = {self.encoder_input_names[0]: input_tensor} 344 | 345 | # Run the encoder session and get the output 346 | output = self.encoder_session.run(self.encoder_output_names, inputs) 347 | 348 | # Flatten the output and store it as the encoder output 349 | self.encoder_out = np.array(output).flatten() 350 | 351 | def set_joint_command(self, joint_index, q, dq, tau, kp, kd): 352 | """ 353 | Sends a command to configure the state of a specific joint. 354 | This method updates the joint's desired position, velocity, torque, and control gains. 355 | Replace this implementation with the actual communication logic for your hardware. 356 | 357 | Parameters: 358 | joint_index (int): The index of the joint to be controlled. 359 | q (float): The desired joint position, typically in radians or degrees. 360 | dq (float): The desired joint velocity, typically in radians/second or degrees/second. 361 | tau (float): The desired joint torque, typically in Newton-meters (Nm). 362 | kp (float): The proportional gain for position control. 363 | kd (float): The derivative gain for velocity control. 364 | """ 365 | self.robot_cmd.q[joint_index] = q 366 | self.robot_cmd.dq[joint_index] = dq 367 | self.robot_cmd.tau[joint_index] = tau 368 | self.robot_cmd.Kp[joint_index] = kp 369 | self.robot_cmd.Kd[joint_index] = kd 370 | 371 | def update(self): 372 | """ 373 | Updates the robot's state based on the current mode and publishes the robot command. 374 | """ 375 | if self.mode == "STAND": 376 | self.handle_stand_mode() 377 | elif self.mode == "WALK": 378 | self.handle_walk_mode() 379 | 380 | # Increment the loop count 381 | self.loop_count += 1 382 | 383 | # Publish the robot command 384 | self.robot.publishRobotCmd(self.robot_cmd) 385 | 386 | # Callback function for receiving robot command data 387 | def robot_state_callback(self, robot_state: datatypes.RobotState): 388 | """ 389 | Callback function to update the robot state from incoming data. 390 | 391 | Parameters: 392 | robot_state (datatypes.RobotState): The current state of the robot. 393 | """ 394 | self.robot_state = robot_state 395 | 396 | # Callback function for receiving imu data 397 | def imu_data_callback(self, imu_data: datatypes.ImuData): 398 | """ 399 | Callback function to update IMU data from incoming data. 400 | 401 | Parameters: 402 | imu_data (datatypes.ImuData): The IMU data containing stamp, acceleration, gyro, and quaternion. 403 | """ 404 | self.imu_data.stamp = imu_data.stamp 405 | self.imu_data.acc = imu_data.acc 406 | self.imu_data.gyro = imu_data.gyro 407 | 408 | # Rotate quaternion values 409 | self.imu_data.quat[0] = imu_data.quat[1] 410 | self.imu_data.quat[1] = imu_data.quat[2] 411 | self.imu_data.quat[2] = imu_data.quat[3] 412 | self.imu_data.quat[3] = imu_data.quat[0] 413 | 414 | # Callback function for receiving sensor joy data 415 | def sensor_joy_callback(self, sensor_joy: datatypes.SensorJoy): 416 | # Check if the robot is in the calibration state and both L1 (button index 4) and Y (button index 3) buttons are pressed. 417 | if not self.start_controller and self.calibration_state == 0 and sensor_joy.buttons[4] == 1 and sensor_joy.buttons[3] == 1: 418 | print(f"L1 + Y: start_controller...") 419 | self.start_controller = True 420 | 421 | # Check if both L1 (button index 4) and X (button index 2) are pressed to stop the controller 422 | if self.start_controller and sensor_joy.buttons[4] == 1 and sensor_joy.buttons[2] == 1: 423 | print(f"L1 + X: stop_controller...") 424 | self.start_controller = False 425 | 426 | linear_x = sensor_joy.axes[1] 427 | linear_y = sensor_joy.axes[0] 428 | angular_z = sensor_joy.axes[2] 429 | 430 | linear_x = 1.0 if linear_x > 1.0 else (-1.0 if linear_x < -1.0 else linear_x) 431 | linear_y = 1.0 if linear_y > 1.0 else (-1.0 if linear_y < -1.0 else linear_y) 432 | angular_z = 1.0 if angular_z > 1.0 else (-1.0 if angular_z < -1.0 else angular_z) 433 | 434 | self.commands[0] = linear_x * 0.5 435 | self.commands[1] = linear_y * 0.5 436 | self.commands[2] = angular_z * 0.5 437 | 438 | # Callback function for receiving diagnostic data 439 | def robot_diagnostic_callback(self, diagnostic_value: datatypes.DiagnosticValue): 440 | # Check if the received diagnostic data is related to calibration. 441 | if diagnostic_value.name == "calibration": 442 | print(f"Calibration state: {diagnostic_value.code}") 443 | self.calibration_state = diagnostic_value.code 444 | -------------------------------------------------------------------------------- /controllers/SolefootController.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import copy 4 | import numpy as np 5 | import yaml 6 | import time 7 | import onnxruntime as ort 8 | from scipy.spatial.transform import Rotation as R 9 | from functools import partial 10 | import limxsdk 11 | import limxsdk.robot.Rate as Rate 12 | import limxsdk.robot.Robot as Robot 13 | import limxsdk.robot.RobotType as RobotType 14 | import limxsdk.datatypes as datatypes 15 | 16 | class SolefootController: 17 | def __init__(self, model_dir, robot, robot_type, start_controller): 18 | # Initialize robot and type information 19 | self.robot = robot 20 | self.robot_type = robot_type 21 | 22 | # Load configuration and model file paths based on robot type 23 | self.config_file = f'{model_dir}/{self.robot_type}/params.yaml' 24 | self.model_policy = f'{model_dir}/{self.robot_type}/policy/policy.onnx' 25 | self.model_encoder = f'{model_dir}/{self.robot_type}/policy/encoder.onnx' 26 | 27 | # Load configuration settings from the YAML file 28 | self.load_config(self.config_file) 29 | 30 | # Load the ONNX model and set up input and output names 31 | self.policy_session = ort.InferenceSession(self.model_policy) 32 | self.policy_input_names = [self.policy_session.get_inputs()[i].name for i in range(self.policy_session.get_inputs().__len__())] 33 | self.policy_output_names = [self.policy_session.get_outputs()[i].name for i in range(self.policy_session.get_outputs().__len__())] 34 | self.policy_input_shapes = [self.policy_session.get_inputs()[i].shape for i in range(self.policy_session.get_inputs().__len__())] 35 | self.policy_output_shapes = [self.policy_session.get_outputs()[i].shape for i in range(self.policy_session.get_outputs().__len__())] 36 | 37 | self.encoder_session = ort.InferenceSession(self.model_encoder) 38 | self.encoder_input_names = [self.encoder_session.get_inputs()[i].name for i in range(self.encoder_session.get_inputs().__len__())] 39 | self.encoder_output_names = [self.encoder_session.get_outputs()[i].name for i in range(self.encoder_session.get_outputs().__len__())] 40 | self.encoder_input_shapes = [self.encoder_session.get_inputs()[i].shape for i in range(self.encoder_session.get_inputs().__len__())] 41 | self.encoder_output_shapes = [self.encoder_session.get_outputs()[i].shape for i in range(self.encoder_session.get_outputs().__len__())] 42 | 43 | # Prepare robot command structure with default values for mode, q, dq, tau, Kp, Kd 44 | self.robot_cmd = datatypes.RobotCmd() 45 | self.robot_cmd.mode = [0. for x in range(0, self.joint_num)] 46 | self.robot_cmd.q = [0. for x in range(0, self.joint_num)] 47 | self.robot_cmd.dq = [0. for x in range(0, self.joint_num)] 48 | self.robot_cmd.tau = [0. for x in range(0, self.joint_num)] 49 | self.robot_cmd.Kp = [self.control_cfg['stiffness'] for x in range(0, self.joint_num)] 50 | self.robot_cmd.Kd = [self.control_cfg['damping'] for x in range(0, self.joint_num)] 51 | 52 | # Prepare robot state structure 53 | self.robot_state = datatypes.RobotState() 54 | self.robot_state.tau = [0. for x in range(0, self.joint_num)] 55 | self.robot_state.q = [0. for x in range(0, self.joint_num)] 56 | self.robot_state.dq = [0. for x in range(0, self.joint_num)] 57 | self.robot_state_tmp = copy.deepcopy(self.robot_state) 58 | 59 | # Initialize IMU (Inertial Measurement Unit) data structure 60 | self.imu_data = datatypes.ImuData() 61 | self.imu_data.quat[0] = 0 62 | self.imu_data.quat[1] = 0 63 | self.imu_data.quat[2] = 0 64 | self.imu_data.quat[3] = 1 65 | self.imu_data_tmp = copy.deepcopy(self.imu_data) 66 | 67 | # Set up a callback to receive updated robot state data 68 | self.robot_state_callback_partial = partial(self.robot_state_callback) 69 | self.robot.subscribeRobotState(self.robot_state_callback_partial) 70 | 71 | # Set up a callback to receive updated IMU data 72 | self.imu_data_callback_partial = partial(self.imu_data_callback) 73 | self.robot.subscribeImuData(self.imu_data_callback_partial) 74 | 75 | # Set up a callback to receive updated SensorJoy 76 | self.sensor_joy_callback_partial = partial(self.sensor_joy_callback) 77 | self.robot.subscribeSensorJoy(self.sensor_joy_callback_partial) 78 | 79 | # Set up a callback to receive diagnostic data 80 | self.robot_diagnostic_callback_partial = partial(self.robot_diagnostic_callback) 81 | self.robot.subscribeDiagnosticValue(self.robot_diagnostic_callback_partial) 82 | 83 | # Initialize the calibration state to -1, indicating no calibration has occurred. 84 | self.calibration_state = -1 85 | 86 | # Flag to start the controller 87 | self.start_controller = start_controller 88 | 89 | # Gait index 90 | self.gait_index = 0 91 | 92 | # Flag indicating first received observation 93 | self.is_first_rec_obs = True 94 | 95 | # Load the configuration from a YAML file 96 | def load_config(self, config_file): 97 | with open(config_file, 'r') as f: 98 | config = yaml.safe_load(f) 99 | 100 | # Assign configuration parameters to controller variables 101 | self.joint_names = config['PointfootCfg']['joint_names'] 102 | self.init_state = config['PointfootCfg']['init_state']['default_joint_angle'] 103 | self.stand_duration = config['PointfootCfg']['stand_mode']['stand_duration'] 104 | self.control_cfg = config['PointfootCfg']['control'] 105 | self.rl_cfg = config['PointfootCfg']['normalization'] 106 | self.obs_scales = config['PointfootCfg']['normalization']['obs_scales'] 107 | self.actions_size = config['PointfootCfg']['size']['actions_size'] 108 | self.commands_size = config['PointfootCfg']['size']['commands_size'] 109 | self.observations_size = config['PointfootCfg']['size']['observations_size'] 110 | self.obs_history_length = config['PointfootCfg']['size']['obs_history_length'] 111 | self.encoder_output_size = config['PointfootCfg']['size']['encoder_output_size'] 112 | self.imu_orientation_offset = np.array(list(config['PointfootCfg']['imu_orientation_offset'].values())) 113 | self.user_cmd_cfg = config['PointfootCfg']['user_cmd_scales'] 114 | self.loop_frequency = config['PointfootCfg']['loop_frequency'] 115 | self.encoder_input_size = self.obs_history_length * self.observations_size 116 | 117 | # Initialize variables for actions, observations, and commands 118 | self.proprio_history_vector = np.zeros(self.obs_history_length * self.observations_size) 119 | self.encoder_out = np.zeros(self.encoder_output_size) 120 | self.actions = np.zeros(self.actions_size) 121 | self.observations = np.zeros(self.observations_size) 122 | self.last_actions = np.zeros(self.actions_size) 123 | self.commands = np.zeros(self.commands_size) # command to the robot (e.g., velocity, rotation) 124 | self.scaled_commands = np.zeros(self.commands_size) 125 | self.base_lin_vel = np.zeros(3) # base linear velocity 126 | self.base_position = np.zeros(3) # robot base position 127 | self.loop_count = 0 # loop iteration count 128 | self.stand_percent = 0 # percentage of time the robot has spent in stand mode 129 | self.policy_session = None # ONNX model session for policy inference 130 | self.joint_num = len(self.joint_names) # number of joints 131 | 132 | self.ankle_joint_damping = config['PointfootCfg']['control']['ankle_joint_damping'] 133 | self.ankle_joint_torque_limit = config['PointfootCfg']['control']['ankle_joint_torque_limit'] 134 | 135 | self.gait_frequencies = config['PointfootCfg']['gait']['frequencies'] 136 | self.gait_swing_height = config['PointfootCfg']['gait']['swing_height'] 137 | 138 | # Initialize joint angles based on the initial configuration 139 | self.init_joint_angles = np.zeros(len(self.joint_names)) 140 | for i in range(len(self.joint_names)): 141 | self.init_joint_angles[i] = self.init_state[self.joint_names[i]] 142 | 143 | # Set initial mode to "STAND" 144 | self.mode = "STAND" 145 | 146 | # Main control loop 147 | def run(self): 148 | # Wait until the controller is started 149 | while not self.start_controller: 150 | time.sleep(1) 151 | 152 | # Initialize default joint angles for standing 153 | self.default_joint_angles = np.array([0.0] * len(self.joint_names)) 154 | self.stand_percent += 1 / (self.stand_duration * self.loop_frequency) 155 | self.mode = "STAND" 156 | self.loop_count = 0 157 | 158 | # Set the loop rate based on the frequency in the configuration 159 | rate = Rate(self.loop_frequency) 160 | while self.start_controller: 161 | self.update() 162 | rate.sleep() 163 | 164 | # Reset robot command values to ensure a safe stop when exiting the loop 165 | self.robot_cmd.q = [0. for x in range(0, self.joint_num)] 166 | self.robot_cmd.dq = [0. for x in range(0, self.joint_num)] 167 | self.robot_cmd.tau = [0. for x in range(0, self.joint_num)] 168 | self.robot_cmd.Kp = [0. for x in range(0, self.joint_num)] 169 | self.robot_cmd.Kd = [1.0 for x in range(0, self.joint_num)] 170 | self.robot.publishRobotCmd(self.robot_cmd) 171 | time.sleep(1) 172 | 173 | # Handle the stand mode for smoothly transitioning the robot into standing 174 | def handle_stand_mode(self): 175 | if self.stand_percent < 1: 176 | for j in range(len(self.joint_names)): 177 | # Interpolate between initial and default joint angles during stand mode 178 | pos_des = self.default_joint_angles[j] * (1 - self.stand_percent) + self.init_state[self.joint_names[j]] * self.stand_percent 179 | self.set_joint_command(j, pos_des, 0, 0, self.control_cfg['stiffness'], self.control_cfg['damping']) 180 | # Increment the stand percentage over time 181 | self.stand_percent += 1 / (self.stand_duration * self.loop_frequency) 182 | else: 183 | # Switch to walk mode after standing 184 | self.mode = "WALK" 185 | 186 | # Handle the walk mode where the robot moves based on computed actions 187 | def handle_walk_mode(self): 188 | # Update the temporary robot state and IMU data 189 | self.robot_state_tmp = copy.deepcopy(self.robot_state) 190 | self.imu_data_tmp = copy.deepcopy(self.imu_data) 191 | 192 | # Execute actions every 'decimation' iterations 193 | if self.loop_count % self.control_cfg['decimation'] == 0: 194 | self.compute_observation() 195 | self.compute_encoder() 196 | self.compute_actions() 197 | # Clip the actions within predefined limits 198 | action_min = -self.rl_cfg['clip_scales']['clip_actions'] 199 | action_max = self.rl_cfg['clip_scales']['clip_actions'] 200 | self.actions = np.clip(self.actions, action_min, action_max) 201 | 202 | # Iterate over the joints and set commands based on actions 203 | joint_pos = np.array(self.robot_state_tmp.q) 204 | joint_vel = np.array(self.robot_state_tmp.dq) 205 | 206 | for i in range(len(joint_pos)): 207 | if (i + 1) % 4 != 0: 208 | # Compute the limits for the action based on joint position and velocity 209 | action_min = (joint_pos[i] - self.init_joint_angles[i] + 210 | (self.control_cfg['damping'] * joint_vel[i] - self.control_cfg['user_torque_limit']) / 211 | self.control_cfg['stiffness']) 212 | action_max = (joint_pos[i] - self.init_joint_angles[i] + 213 | (self.control_cfg['damping'] * joint_vel[i] + self.control_cfg['user_torque_limit']) / 214 | self.control_cfg['stiffness']) 215 | 216 | # Clip action within limits 217 | self.actions[i] = max(action_min / self.control_cfg['action_scale_pos'], 218 | min(action_max / self.control_cfg['action_scale_pos'], self.actions[i])) 219 | 220 | # Compute the desired joint position and set it 221 | pos_des = self.actions[i] * self.control_cfg['action_scale_pos'] + self.init_joint_angles[i] 222 | self.set_joint_command(i, pos_des, 0, 0, self.control_cfg['stiffness'], self.control_cfg['damping']) 223 | 224 | # Save the last action for reference 225 | self.last_actions[i] = self.actions[i] 226 | else: 227 | action_min = joint_vel[i] - self.ankle_joint_torque_limit / self.ankle_joint_damping 228 | action_max = joint_vel[i] + self.ankle_joint_torque_limit / self.ankle_joint_damping 229 | self.last_actions[i] = self.actions[i] 230 | self.actions[i] = max(action_min / self.ankle_joint_damping, 231 | min(action_max / self.ankle_joint_damping, self.actions[i])) 232 | velocity_des = self.actions[i] * self.ankle_joint_damping 233 | self.set_joint_command(i, 0, velocity_des, 0, 0, self.ankle_joint_damping) 234 | 235 | def compute_observation(self): 236 | # Convert IMU orientation from quaternion to Euler angles (ZYX convention) 237 | imu_orientation = np.array(self.imu_data_tmp.quat) 238 | q_wi = R.from_quat(imu_orientation).as_euler('zyx') # Quaternion to Euler ZYX conversion 239 | inverse_rot = R.from_euler('zyx', q_wi).inv().as_matrix() # Get the inverse rotation matrix 240 | 241 | # Project the gravity vector (pointing downwards) into the body frame 242 | gravity_vector = np.array([0, 0, -1]) # Gravity in world frame (z-axis down) 243 | projected_gravity = np.dot(inverse_rot, gravity_vector) # Transform gravity into body frame 244 | 245 | # Retrieve base angular velocity from the IMU data 246 | base_ang_vel = np.array(self.imu_data_tmp.gyro) 247 | # Apply IMU orientation offset correction (using Euler angles) 248 | rot = R.from_euler('zyx', self.imu_orientation_offset).as_matrix() # Rotation matrix for offset correction 249 | base_ang_vel = np.dot(rot, base_ang_vel) # Apply correction to angular velocity 250 | projected_gravity = np.dot(rot, projected_gravity) # Apply correction to projected gravity 251 | 252 | # Retrieve joint positions and velocities from the robot state 253 | joint_positions = np.array(self.robot_state_tmp.q) 254 | joint_velocities = np.array(self.robot_state_tmp.dq) 255 | 256 | gait = np.array([self.gait_frequencies, 0.5, 0.5, self.gait_swing_height]) 257 | self.gait_index += 0.02 * gait[0] 258 | if self.gait_index > 1.0: 259 | self.gait_index = 0.0 260 | gait_clock = np.array([np.sin(self.gait_index * 2 * np.pi), np.cos(self.gait_index * 2 * np.pi)]) 261 | 262 | # Retrieve the last actions that were applied to the robot 263 | actions = np.array(self.last_actions) 264 | 265 | # Create a command scaler matrix for linear and angular velocities 266 | command_scaler = np.diag([ 267 | self.user_cmd_cfg['lin_vel_x'], # Scale factor for linear velocity in x direction 268 | self.user_cmd_cfg['lin_vel_y'], # Scale factor for linear velocity in y direction 269 | self.user_cmd_cfg['ang_vel_yaw'], # Scale factor for yaw (angular velocity) 270 | 1.0, 1.0 271 | ]) 272 | 273 | # Apply scaling to the command inputs (velocity commands) 274 | self.scaled_commands = np.dot(command_scaler, self.commands) 275 | 276 | # Populate observation vector 277 | joint_pos_input = (joint_positions - self.init_joint_angles) * self.obs_scales['dof_pos'] 278 | 279 | # Create the observation vector by concatenating various state variables: 280 | # - Base angular velocity (scaled) 281 | # - Projected gravity vector 282 | # - Joint positions (difference from initial angles, scaled) 283 | # - Joint velocities (scaled) 284 | # - Last actions applied to the robot 285 | # - gait_clock: A clock signal related to the gait of the robot. 286 | # - gait: Information about the current gait of the robot. 287 | obs = np.concatenate([ 288 | base_ang_vel * self.obs_scales['ang_vel'], # Scaled base angular velocity 289 | projected_gravity, # Projected gravity vector in body frame 290 | joint_pos_input, # Scaled joint positions 291 | joint_velocities * self.obs_scales['dof_vel'], # Scaled joint velocities 292 | actions, # Last actions taken by the robot 293 | gait_clock, # A clock signal related to the robot's gait 294 | gait # Information about the current gait pattern of the robot 295 | ]) 296 | 297 | # Check if this is the first recorded observation 298 | if self.is_first_rec_obs: 299 | # Calculate the total size of the encoder input 300 | input_size = np.prod(self.encoder_input_shapes[0]) 301 | 302 | # Initialize the proprioceptive history buffer with zeros 303 | self.proprio_history_buffer = np.zeros(input_size) 304 | 305 | # Fill the proprioceptive history buffer with the current observation for the entire history length 306 | for i in range(self.obs_history_length): 307 | self.proprio_history_buffer[i * self.observations_size:(i + 1) * self.observations_size] = obs 308 | 309 | # Update the flag to indicate that the first observation has been processed 310 | self.is_first_rec_obs = False 311 | 312 | # Shift the existing proprioceptive history buffer to the left 313 | self.proprio_history_buffer[:-self.observations_size] = self.proprio_history_buffer[self.observations_size:] 314 | 315 | # Add the current observation to the end of the proprioceptive history buffer 316 | self.proprio_history_buffer[-self.observations_size:] = obs 317 | 318 | # Convert the proprioceptive history buffer to a numpy array 319 | self.proprio_history_vector = np.array(self.proprio_history_buffer) 320 | 321 | # Clip the observation values to within the specified limits for stability 322 | self.observations = np.clip( 323 | obs, 324 | -self.rl_cfg['clip_scales']['clip_observations'], # Lower limit for clipping 325 | self.rl_cfg['clip_scales']['clip_observations'] # Upper limit for clipping 326 | ) 327 | 328 | def compute_actions(self): 329 | """ 330 | Computes the actions based on the current observations using the policy session. 331 | """ 332 | # Concatenate observations into a single tensor and convert to float32 333 | input_tensor = np.concatenate([self.encoder_out, self.observations, self.scaled_commands], axis=0) 334 | input_tensor = input_tensor.astype(np.float32) 335 | 336 | # Create a dictionary of inputs for the policy session 337 | inputs = {self.policy_input_names[0]: input_tensor} 338 | 339 | # Run the policy session and get the output 340 | output = self.policy_session.run(self.policy_output_names, inputs) 341 | 342 | # Flatten the output and store it as actions 343 | self.actions = np.array(output).flatten() 344 | 345 | def compute_encoder(self): 346 | """ 347 | Computes the encoder output based on the proprioceptive history buffer. 348 | 349 | This method first concatenates the proprioceptive history buffer into a single input tensor. 350 | Then it converts the input tensor to the float32 data type. After that, it creates a dictionary 351 | of inputs for the encoder session and runs the encoder session to get the output. Finally, 352 | it flattens the output and stores it as the encoder output. 353 | """ 354 | # Concatenate the proprioceptive history buffer into a single tensor and convert to float32 355 | input_tensor = np.concatenate([self.proprio_history_buffer], axis=0) 356 | input_tensor = input_tensor.astype(np.float32) 357 | 358 | # Create a dictionary of inputs for the encoder session 359 | inputs = {self.encoder_input_names[0]: input_tensor} 360 | 361 | # Run the encoder session and get the output 362 | output = self.encoder_session.run(self.encoder_output_names, inputs) 363 | 364 | # Flatten the output and store it as the encoder output 365 | self.encoder_out = np.array(output).flatten() 366 | 367 | def set_joint_command(self, joint_index, q, dq, tau, kp, kd): 368 | """ 369 | Sends a command to configure the state of a specific joint. 370 | This method updates the joint's desired position, velocity, torque, and control gains. 371 | Replace this implementation with the actual communication logic for your hardware. 372 | 373 | Parameters: 374 | joint_index (int): The index of the joint to be controlled. 375 | q (float): The desired joint position, typically in radians or degrees. 376 | dq (float): The desired joint velocity, typically in radians/second or degrees/second. 377 | tau (float): The desired joint torque, typically in Newton-meters (Nm). 378 | kp (float): The proportional gain for position control. 379 | kd (float): The derivative gain for velocity control. 380 | """ 381 | self.robot_cmd.q[joint_index] = q 382 | self.robot_cmd.dq[joint_index] = dq 383 | self.robot_cmd.tau[joint_index] = tau 384 | self.robot_cmd.Kp[joint_index] = kp 385 | self.robot_cmd.Kd[joint_index] = kd 386 | 387 | def update(self): 388 | """ 389 | Updates the robot's state based on the current mode and publishes the robot command. 390 | """ 391 | if self.mode == "STAND": 392 | self.handle_stand_mode() 393 | elif self.mode == "WALK": 394 | self.handle_walk_mode() 395 | 396 | # Increment the loop count 397 | self.loop_count += 1 398 | 399 | # Publish the robot command 400 | self.robot.publishRobotCmd(self.robot_cmd) 401 | 402 | # Callback function for receiving robot command data 403 | def robot_state_callback(self, robot_state: datatypes.RobotState): 404 | """ 405 | Callback function to update the robot state from incoming data. 406 | 407 | Parameters: 408 | robot_state (datatypes.RobotState): The current state of the robot. 409 | """ 410 | self.robot_state = robot_state 411 | 412 | # Callback function for receiving imu data 413 | def imu_data_callback(self, imu_data: datatypes.ImuData): 414 | """ 415 | Callback function to update IMU data from incoming data. 416 | 417 | Parameters: 418 | imu_data (datatypes.ImuData): The IMU data containing stamp, acceleration, gyro, and quaternion. 419 | """ 420 | self.imu_data.stamp = imu_data.stamp 421 | self.imu_data.acc = imu_data.acc 422 | self.imu_data.gyro = imu_data.gyro 423 | 424 | # Rotate quaternion values 425 | self.imu_data.quat[0] = imu_data.quat[1] 426 | self.imu_data.quat[1] = imu_data.quat[2] 427 | self.imu_data.quat[2] = imu_data.quat[3] 428 | self.imu_data.quat[3] = imu_data.quat[0] 429 | 430 | # Callback function for receiving sensor joy data 431 | def sensor_joy_callback(self, sensor_joy: datatypes.SensorJoy): 432 | # Check if the robot is in the calibration state and both L1 (button index 4) and Y (button index 3) buttons are pressed. 433 | if not self.start_controller and self.calibration_state == 0 and sensor_joy.buttons[4] == 1 and sensor_joy.buttons[3] == 1: 434 | print(f"L1 + Y: start_controller...") 435 | self.start_controller = True 436 | 437 | # Check if both L1 (button index 4) and X (button index 2) are pressed to stop the controller 438 | if self.start_controller and sensor_joy.buttons[4] == 1 and sensor_joy.buttons[2] == 1: 439 | print(f"L1 + X: stop_controller...") 440 | self.start_controller = False 441 | 442 | linear_x = sensor_joy.axes[1] 443 | linear_y = sensor_joy.axes[0] 444 | angular_z = sensor_joy.axes[2] 445 | 446 | linear_x = 1.0 if linear_x > 1.0 else (-1.0 if linear_x < -1.0 else linear_x) 447 | linear_y = 1.0 if linear_y > 1.0 else (-1.0 if linear_y < -1.0 else linear_y) 448 | angular_z = 1.0 if angular_z > 1.0 else (-1.0 if angular_z < -1.0 else angular_z) 449 | 450 | self.commands[0] = linear_x * 0.5 451 | self.commands[1] = linear_y * 0.5 452 | self.commands[2] = angular_z * 0.5 453 | self.commands[3] = 0.0 454 | self.commands[4] = 0.0 455 | 456 | # Callback function for receiving diagnostic data 457 | def robot_diagnostic_callback(self, diagnostic_value: datatypes.DiagnosticValue): 458 | # Check if the received diagnostic data is related to calibration. 459 | if diagnostic_value.name == "calibration": 460 | print(f"Calibration state: {diagnostic_value.code}") 461 | self.calibration_state = diagnostic_value.code 462 | -------------------------------------------------------------------------------- /controllers/WheelfootController.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import copy 4 | import numpy as np 5 | import yaml 6 | import time 7 | import onnxruntime as ort 8 | from scipy.spatial.transform import Rotation as R 9 | from functools import partial 10 | import limxsdk 11 | import limxsdk.robot.Rate as Rate 12 | import limxsdk.robot.Robot as Robot 13 | import limxsdk.robot.RobotType as RobotType 14 | import limxsdk.datatypes as datatypes 15 | 16 | class WheelfootController: 17 | def __init__(self, model_dir, robot, robot_type, start_controller): 18 | # Initialize robot and type information 19 | self.robot = robot 20 | self.robot_type = robot_type 21 | 22 | # Load configuration and model file paths based on robot type 23 | self.config_file = f'{model_dir}/{self.robot_type}/params.yaml' 24 | self.model_policy = f'{model_dir}/{self.robot_type}/policy/policy.onnx' 25 | self.model_encoder = f'{model_dir}/{self.robot_type}/policy/encoder.onnx' 26 | 27 | # Load configuration settings from the YAML file 28 | self.load_config(self.config_file) 29 | 30 | # Load the ONNX model and set up input and output names 31 | self.policy_session = ort.InferenceSession(self.model_policy) 32 | self.policy_input_names = [self.policy_session.get_inputs()[i].name for i in range(self.policy_session.get_inputs().__len__())] 33 | self.policy_output_names = [self.policy_session.get_outputs()[i].name for i in range(self.policy_session.get_outputs().__len__())] 34 | self.policy_input_shapes = [self.policy_session.get_inputs()[i].shape for i in range(self.policy_session.get_inputs().__len__())] 35 | self.policy_output_shapes = [self.policy_session.get_outputs()[i].shape for i in range(self.policy_session.get_outputs().__len__())] 36 | 37 | self.encoder_session = ort.InferenceSession(self.model_encoder) 38 | self.encoder_input_names = [self.encoder_session.get_inputs()[i].name for i in range(self.encoder_session.get_inputs().__len__())] 39 | self.encoder_output_names = [self.encoder_session.get_outputs()[i].name for i in range(self.encoder_session.get_outputs().__len__())] 40 | self.encoder_input_shapes = [self.encoder_session.get_inputs()[i].shape for i in range(self.encoder_session.get_inputs().__len__())] 41 | self.encoder_output_shapes = [self.encoder_session.get_outputs()[i].shape for i in range(self.encoder_session.get_outputs().__len__())] 42 | 43 | # Prepare robot command structure with default values for mode, q, dq, tau, Kp, Kd 44 | self.robot_cmd = datatypes.RobotCmd() 45 | self.robot_cmd.mode = [0. for x in range(0, self.joint_num)] 46 | self.robot_cmd.q = [0. for x in range(0, self.joint_num)] 47 | self.robot_cmd.dq = [0. for x in range(0, self.joint_num)] 48 | self.robot_cmd.tau = [0. for x in range(0, self.joint_num)] 49 | self.robot_cmd.Kp = [self.control_cfg['stiffness'] for x in range(0, self.joint_num)] 50 | self.robot_cmd.Kd = [self.control_cfg['damping'] for x in range(0, self.joint_num)] 51 | 52 | # Prepare robot state structure 53 | self.robot_state = datatypes.RobotState() 54 | self.robot_state.tau = [0. for x in range(0, self.joint_num)] 55 | self.robot_state.q = [0. for x in range(0, self.joint_num)] 56 | self.robot_state.dq = [0. for x in range(0, self.joint_num)] 57 | self.robot_state_tmp = copy.deepcopy(self.robot_state) 58 | 59 | # Initialize IMU (Inertial Measurement Unit) data structure 60 | self.imu_data = datatypes.ImuData() 61 | self.imu_data.quat[0] = 0 62 | self.imu_data.quat[1] = 0 63 | self.imu_data.quat[2] = 0 64 | self.imu_data.quat[3] = 1 65 | self.imu_data_tmp = copy.deepcopy(self.imu_data) 66 | 67 | # Set up a callback to receive updated robot state data 68 | self.robot_state_callback_partial = partial(self.robot_state_callback) 69 | self.robot.subscribeRobotState(self.robot_state_callback_partial) 70 | 71 | # Set up a callback to receive updated IMU data 72 | self.imu_data_callback_partial = partial(self.imu_data_callback) 73 | self.robot.subscribeImuData(self.imu_data_callback_partial) 74 | 75 | # Set up a callback to receive updated SensorJoy 76 | self.sensor_joy_callback_partial = partial(self.sensor_joy_callback) 77 | self.robot.subscribeSensorJoy(self.sensor_joy_callback_partial) 78 | 79 | # Set up a callback to receive diagnostic data 80 | self.robot_diagnostic_callback_partial = partial(self.robot_diagnostic_callback) 81 | self.robot.subscribeDiagnosticValue(self.robot_diagnostic_callback_partial) 82 | 83 | # Initialize the calibration state to -1, indicating no calibration has occurred. 84 | self.calibration_state = -1 85 | 86 | # Flag to start the controller 87 | self.start_controller = start_controller 88 | 89 | # Gait index 90 | self.gait_index = 0 91 | 92 | # Flag indicating first received observation 93 | self.is_first_rec_obs = True 94 | 95 | # Load the configuration from a YAML file 96 | def load_config(self, config_file): 97 | with open(config_file, 'r') as f: 98 | config = yaml.safe_load(f) 99 | 100 | # Assign configuration parameters to controller variables 101 | self.joint_names = config['PointfootCfg']['joint_names'] 102 | self.init_state = config['PointfootCfg']['init_state']['default_joint_angle'] 103 | self.stand_duration = config['PointfootCfg']['stand_mode']['stand_duration'] 104 | self.control_cfg = config['PointfootCfg']['control'] 105 | self.rl_cfg = config['PointfootCfg']['normalization'] 106 | self.obs_scales = config['PointfootCfg']['normalization']['obs_scales'] 107 | self.actions_size = config['PointfootCfg']['size']['actions_size'] 108 | self.commands_size = config['PointfootCfg']['size']['commands_size'] 109 | self.observations_size = config['PointfootCfg']['size']['observations_size'] 110 | self.obs_history_length = config['PointfootCfg']['size']['obs_history_length'] 111 | self.encoder_output_size = config['PointfootCfg']['size']['encoder_output_size'] 112 | self.imu_orientation_offset = np.array(list(config['PointfootCfg']['imu_orientation_offset'].values())) 113 | self.user_cmd_cfg = config['PointfootCfg']['user_cmd_scales'] 114 | self.loop_frequency = config['PointfootCfg']['loop_frequency'] 115 | self.encoder_input_size = self.obs_history_length * self.observations_size 116 | 117 | # Initialize variables for actions, observations, and commands 118 | self.proprio_history_vector = np.zeros(self.obs_history_length * self.observations_size) 119 | self.encoder_out = np.zeros(self.encoder_output_size) 120 | self.actions = np.zeros(self.actions_size) 121 | self.observations = np.zeros(self.observations_size) 122 | self.last_actions = np.zeros(self.actions_size) 123 | self.commands = np.zeros(self.commands_size) # command to the robot (e.g., velocity, rotation) 124 | self.scaled_commands = np.zeros(self.commands_size) 125 | self.base_lin_vel = np.zeros(3) # base linear velocity 126 | self.base_position = np.zeros(3) # robot base position 127 | self.loop_count = 0 # loop iteration count 128 | self.stand_percent = 0 # percentage of time the robot has spent in stand mode 129 | self.policy_session = None # ONNX model session for policy inference 130 | self.joint_num = len(self.joint_names) # number of joints 131 | 132 | self.joint_pos_idxs = config['PointfootCfg']['size']['jointpos_idxs'] 133 | self.wheel_joint_damping = config['PointfootCfg']['control']['wheel_joint_damping'] 134 | self.wheel_joint_torque_limit = config['PointfootCfg']['control']['wheel_joint_torque_limit'] 135 | 136 | # Initialize joint angles based on the initial configuration 137 | self.init_joint_angles = np.zeros(len(self.joint_names)) 138 | for i in range(len(self.joint_names)): 139 | self.init_joint_angles[i] = self.init_state[self.joint_names[i]] 140 | 141 | # Set initial mode to "STAND" 142 | self.mode = "STAND" 143 | 144 | # Main control loop 145 | def run(self): 146 | # Wait until the controller is started 147 | while not self.start_controller: 148 | time.sleep(1) 149 | 150 | # Initialize default joint angles for standing 151 | self.default_joint_angles = np.array([0.0] * len(self.joint_names)) 152 | self.stand_percent += 1 / (self.stand_duration * self.loop_frequency) 153 | self.mode = "STAND" 154 | self.loop_count = 0 155 | 156 | # Set the loop rate based on the frequency in the configuration 157 | rate = Rate(self.loop_frequency) 158 | while self.start_controller: 159 | self.update() 160 | rate.sleep() 161 | 162 | # Reset robot command values to ensure a safe stop when exiting the loop 163 | self.robot_cmd.q = [0. for x in range(0, self.joint_num)] 164 | self.robot_cmd.dq = [0. for x in range(0, self.joint_num)] 165 | self.robot_cmd.tau = [0. for x in range(0, self.joint_num)] 166 | self.robot_cmd.Kp = [0. for x in range(0, self.joint_num)] 167 | self.robot_cmd.Kd = [1.0 for x in range(0, self.joint_num)] 168 | self.robot.publishRobotCmd(self.robot_cmd) 169 | time.sleep(1) 170 | 171 | # Handle the stand mode for smoothly transitioning the robot into standing 172 | def handle_stand_mode(self): 173 | self.init_state["hip_L_Joint"] = -0.9; 174 | self.init_state["hip_R_Joint"] = 0.9; 175 | if self.stand_percent < 1: 176 | for j in range(len(self.joint_names)): 177 | if (j + 1) % 4 != 0: 178 | # Interpolate between initial and default joint angles during stand mode 179 | pos_des = self.default_joint_angles[j] * (1 - self.stand_percent) + self.init_state[self.joint_names[j]] * self.stand_percent 180 | self.set_joint_command(j, pos_des, 0, 0, self.control_cfg['stiffness'], self.control_cfg['damping']) 181 | else: 182 | self.set_joint_command(0, 0, 0, self.wheel_joint_damping, 0, 0) 183 | # Increment the stand percentage over time 184 | self.stand_percent += 3 / (self.stand_duration * self.loop_frequency) 185 | else: 186 | # Switch to walk mode after standing 187 | self.mode = "WALK" 188 | 189 | # Handle the walk mode where the robot moves based on computed actions 190 | def handle_walk_mode(self): 191 | self.init_joint_angles[1] = 0.0; 192 | self.init_joint_angles[5] = 0.0; 193 | 194 | # Update the temporary robot state and IMU data 195 | self.robot_state_tmp = copy.deepcopy(self.robot_state) 196 | self.imu_data_tmp = copy.deepcopy(self.imu_data) 197 | 198 | # Execute actions every 'decimation' iterations 199 | if self.loop_count % self.control_cfg['decimation'] == 0: 200 | self.compute_observation() 201 | self.compute_encoder() 202 | self.compute_actions() 203 | # Clip the actions within predefined limits 204 | action_min = -self.rl_cfg['clip_scales']['clip_actions'] 205 | action_max = self.rl_cfg['clip_scales']['clip_actions'] 206 | self.actions = np.clip(self.actions, action_min, action_max) 207 | 208 | # Iterate over the joints and set commands based on actions 209 | joint_pos = np.array(self.robot_state_tmp.q) 210 | joint_vel = np.array(self.robot_state_tmp.dq) 211 | 212 | for i in range(len(joint_pos)): 213 | if (i + 1) % 4 != 0: 214 | # Compute the limits for the action based on joint position and velocity 215 | action_min = (joint_pos[i] - self.init_joint_angles[i] + 216 | (self.control_cfg['damping'] * joint_vel[i] - self.control_cfg['user_torque_limit']) / 217 | self.control_cfg['stiffness']) 218 | action_max = (joint_pos[i] - self.init_joint_angles[i] + 219 | (self.control_cfg['damping'] * joint_vel[i] + self.control_cfg['user_torque_limit']) / 220 | self.control_cfg['stiffness']) 221 | 222 | # Clip action within limits 223 | self.actions[i] = max(action_min / self.control_cfg['action_scale_pos'], 224 | min(action_max / self.control_cfg['action_scale_pos'], self.actions[i])) 225 | 226 | # Compute the desired joint position and set it 227 | pos_des = self.actions[i] * self.control_cfg['action_scale_pos'] + self.init_joint_angles[i] 228 | self.set_joint_command(i, pos_des, 0, 0, self.control_cfg['stiffness'], self.control_cfg['damping']) 229 | 230 | # Save the last action for reference 231 | self.last_actions[i] = self.actions[i] 232 | else: 233 | action_min = joint_vel[i] - self.wheel_joint_torque_limit / self.wheel_joint_damping 234 | action_max = joint_vel[i] + self.wheel_joint_torque_limit / self.wheel_joint_damping 235 | self.last_actions[i] = self.actions[i] 236 | self.actions[i] = max(action_min / self.wheel_joint_damping, 237 | min(action_max / self.wheel_joint_damping, self.actions[i])) 238 | velocity_des = self.actions[i] * self.wheel_joint_damping 239 | self.set_joint_command(i, 0, velocity_des, 0, 0, self.wheel_joint_damping) 240 | 241 | def compute_observation(self): 242 | # Convert IMU orientation from quaternion to Euler angles (ZYX convention) 243 | imu_orientation = np.array(self.imu_data_tmp.quat) 244 | q_wi = R.from_quat(imu_orientation).as_euler('zyx') # Quaternion to Euler ZYX conversion 245 | inverse_rot = R.from_euler('zyx', q_wi).inv().as_matrix() # Get the inverse rotation matrix 246 | 247 | # Project the gravity vector (pointing downwards) into the body frame 248 | gravity_vector = np.array([0, 0, -1]) # Gravity in world frame (z-axis down) 249 | projected_gravity = np.dot(inverse_rot, gravity_vector) # Transform gravity into body frame 250 | 251 | # Retrieve base angular velocity from the IMU data 252 | base_ang_vel = np.array(self.imu_data_tmp.gyro) 253 | # Apply IMU orientation offset correction (using Euler angles) 254 | rot = R.from_euler('zyx', self.imu_orientation_offset).as_matrix() # Rotation matrix for offset correction 255 | base_ang_vel = np.dot(rot, base_ang_vel) # Apply correction to angular velocity 256 | projected_gravity = np.dot(rot, projected_gravity) # Apply correction to projected gravity 257 | 258 | # Retrieve joint positions and velocities from the robot state 259 | joint_positions = np.array(self.robot_state_tmp.q) 260 | joint_velocities = np.array(self.robot_state_tmp.dq) 261 | 262 | # Retrieve the last actions that were applied to the robot 263 | actions = np.array(self.last_actions) 264 | 265 | # Create a command scaler matrix for linear and angular velocities 266 | command_scaler = np.diag([ 267 | self.user_cmd_cfg['lin_vel_x'], # Scale factor for linear velocity in x direction 268 | self.user_cmd_cfg['lin_vel_y'], # Scale factor for linear velocity in y direction 269 | self.user_cmd_cfg['ang_vel_yaw'] # Scale factor for yaw (angular velocity) 270 | ]) 271 | 272 | # Apply scaling to the command inputs (velocity commands) 273 | self.scaled_commands = np.dot(command_scaler, self.commands) 274 | 275 | # Populate observation vector 276 | joint_pos_value = (joint_positions - self.init_joint_angles) * self.obs_scales['dof_pos'] 277 | 278 | # In WF, joint pos does not include wheel speed, index(3, 7) needs to be removed 279 | joint_pos_input = np.array([joint_pos_value[idx] for idx in self.joint_pos_idxs]) 280 | 281 | # Create the observation vector by concatenating various state variables: 282 | # - Base angular velocity (scaled) 283 | # - Projected gravity vector 284 | # - Joint positions (difference from initial angles, scaled) 285 | # - Joint velocities (scaled) 286 | # - Last actions applied to the robot 287 | # - Scaled command inputs 288 | obs = np.concatenate([ 289 | base_ang_vel * self.obs_scales['ang_vel'], # Scaled base angular velocity 290 | projected_gravity, # Projected gravity vector in body frame 291 | joint_pos_input, # Scaled joint positions 292 | joint_velocities * self.obs_scales['dof_vel'], # Scaled joint velocities 293 | actions # Last actions taken by the robot 294 | ]) 295 | 296 | # Check if this is the first recorded observation 297 | if self.is_first_rec_obs: 298 | # Calculate the total size of the encoder input 299 | input_size = np.prod(self.encoder_input_shapes[0]) 300 | 301 | # Initialize the proprioceptive history buffer with zeros 302 | self.proprio_history_buffer = np.zeros(input_size) 303 | 304 | # Fill the proprioceptive history buffer with the current observation for the entire history length 305 | for i in range(self.obs_history_length): 306 | self.proprio_history_buffer[i * self.observations_size:(i + 1) * self.observations_size] = obs 307 | 308 | # Update the flag to indicate that the first observation has been processed 309 | self.is_first_rec_obs = False 310 | 311 | # Shift the existing proprioceptive history buffer to the left 312 | self.proprio_history_buffer[:-self.observations_size] = self.proprio_history_buffer[self.observations_size:] 313 | 314 | # Add the current observation to the end of the proprioceptive history buffer 315 | self.proprio_history_buffer[-self.observations_size:] = obs 316 | 317 | # Convert the proprioceptive history buffer to a numpy array 318 | self.proprio_history_vector = np.array(self.proprio_history_buffer) 319 | 320 | # Clip the observation values to within the specified limits for stability 321 | self.observations = np.clip( 322 | obs, 323 | -self.rl_cfg['clip_scales']['clip_observations'], # Lower limit for clipping 324 | self.rl_cfg['clip_scales']['clip_observations'] # Upper limit for clipping 325 | ) 326 | 327 | def compute_actions(self): 328 | """ 329 | Computes the actions based on the current observations using the policy session. 330 | """ 331 | # Concatenate observations into a single tensor and convert to float32 332 | input_tensor = np.concatenate([self.encoder_out, self.observations, self.scaled_commands], axis=0) 333 | input_tensor = input_tensor.astype(np.float32) 334 | 335 | # Create a dictionary of inputs for the policy session 336 | inputs = {self.policy_input_names[0]: input_tensor} 337 | 338 | # Run the policy session and get the output 339 | output = self.policy_session.run(self.policy_output_names, inputs) 340 | 341 | # Flatten the output and store it as actions 342 | self.actions = np.array(output).flatten() 343 | 344 | def compute_encoder(self): 345 | """ 346 | Computes the encoder output based on the proprioceptive history buffer. 347 | 348 | This method first concatenates the proprioceptive history buffer into a single input tensor. 349 | Then it converts the input tensor to the float32 data type. After that, it creates a dictionary 350 | of inputs for the encoder session and runs the encoder session to get the output. Finally, 351 | it flattens the output and stores it as the encoder output. 352 | """ 353 | # Concatenate the proprioceptive history buffer into a single tensor and convert to float32 354 | input_tensor = np.concatenate([self.proprio_history_buffer], axis=0) 355 | input_tensor = input_tensor.astype(np.float32) 356 | 357 | # Create a dictionary of inputs for the encoder session 358 | inputs = {self.encoder_input_names[0]: input_tensor} 359 | 360 | # Run the encoder session and get the output 361 | output = self.encoder_session.run(self.encoder_output_names, inputs) 362 | 363 | # Flatten the output and store it as the encoder output 364 | self.encoder_out = np.array(output).flatten() 365 | 366 | def set_joint_command(self, joint_index, q, dq, tau, kp, kd): 367 | """ 368 | Sends a command to configure the state of a specific joint. 369 | This method updates the joint's desired position, velocity, torque, and control gains. 370 | Replace this implementation with the actual communication logic for your hardware. 371 | 372 | Parameters: 373 | joint_index (int): The index of the joint to be controlled. 374 | q (float): The desired joint position, typically in radians or degrees. 375 | dq (float): The desired joint velocity, typically in radians/second or degrees/second. 376 | tau (float): The desired joint torque, typically in Newton-meters (Nm). 377 | kp (float): The proportional gain for position control. 378 | kd (float): The derivative gain for velocity control. 379 | """ 380 | self.robot_cmd.q[joint_index] = q 381 | self.robot_cmd.dq[joint_index] = dq 382 | self.robot_cmd.tau[joint_index] = tau 383 | self.robot_cmd.Kp[joint_index] = kp 384 | self.robot_cmd.Kd[joint_index] = kd 385 | 386 | def update(self): 387 | """ 388 | Updates the robot's state based on the current mode and publishes the robot command. 389 | """ 390 | if self.mode == "STAND": 391 | self.handle_stand_mode() 392 | elif self.mode == "WALK": 393 | self.handle_walk_mode() 394 | 395 | # Increment the loop count 396 | self.loop_count += 1 397 | 398 | # Publish the robot command 399 | self.robot.publishRobotCmd(self.robot_cmd) 400 | 401 | # Callback function for receiving robot command data 402 | def robot_state_callback(self, robot_state: datatypes.RobotState): 403 | """ 404 | Callback function to update the robot state from incoming data. 405 | 406 | Parameters: 407 | robot_state (datatypes.RobotState): The current state of the robot. 408 | """ 409 | self.robot_state = robot_state 410 | 411 | # Callback function for receiving imu data 412 | def imu_data_callback(self, imu_data: datatypes.ImuData): 413 | """ 414 | Callback function to update IMU data from incoming data. 415 | 416 | Parameters: 417 | imu_data (datatypes.ImuData): The IMU data containing stamp, acceleration, gyro, and quaternion. 418 | """ 419 | self.imu_data.stamp = imu_data.stamp 420 | self.imu_data.acc = imu_data.acc 421 | self.imu_data.gyro = imu_data.gyro 422 | 423 | # Rotate quaternion values 424 | self.imu_data.quat[0] = imu_data.quat[1] 425 | self.imu_data.quat[1] = imu_data.quat[2] 426 | self.imu_data.quat[2] = imu_data.quat[3] 427 | self.imu_data.quat[3] = imu_data.quat[0] 428 | 429 | # Callback function for receiving sensor joy data 430 | def sensor_joy_callback(self, sensor_joy: datatypes.SensorJoy): 431 | # Check if the robot is in the calibration state and both L1 (button index 4) and Y (button index 3) buttons are pressed. 432 | if not self.start_controller and self.calibration_state == 0 and sensor_joy.buttons[4] == 1 and sensor_joy.buttons[3] == 1: 433 | print(f"L1 + Y: start_controller...") 434 | self.start_controller = True 435 | 436 | # Check if both L1 (button index 4) and X (button index 2) are pressed to stop the controller 437 | if self.start_controller and sensor_joy.buttons[4] == 1 and sensor_joy.buttons[2] == 1: 438 | print(f"L1 + X: stop_controller...") 439 | self.start_controller = False 440 | 441 | linear_x = sensor_joy.axes[1] 442 | linear_y = sensor_joy.axes[0] 443 | angular_z = sensor_joy.axes[2] 444 | 445 | linear_x = 1.0 if linear_x > 1.0 else (-1.0 if linear_x < -1.0 else linear_x) 446 | linear_y = 1.0 if linear_y > 1.0 else (-1.0 if linear_y < -1.0 else linear_y) 447 | angular_z = 1.0 if angular_z > 1.0 else (-1.0 if angular_z < -1.0 else angular_z) 448 | 449 | self.commands[0] = linear_x * 0.5 450 | self.commands[1] = linear_y * 0.5 451 | self.commands[2] = angular_z * 0.5 452 | 453 | # Callback function for receiving diagnostic data 454 | def robot_diagnostic_callback(self, diagnostic_value: datatypes.DiagnosticValue): 455 | # Check if the received diagnostic data is related to calibration. 456 | if diagnostic_value.name == "calibration": 457 | print(f"Calibration state: {diagnostic_value.code}") 458 | self.calibration_state = diagnostic_value.code 459 | -------------------------------------------------------------------------------- /controllers/__init__.py: -------------------------------------------------------------------------------- 1 | from .PointfootController import * 2 | from .SolefootController import * 3 | from .WheelfootController import * -------------------------------------------------------------------------------- /controllers/model/PF_P441A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | loop_frequency: 500 3 | joint_names: 4 | - "abad_L_Joint" 5 | - "hip_L_Joint" 6 | - "knee_L_Joint" 7 | - "abad_R_Joint" 8 | - "hip_R_Joint" 9 | - "knee_R_Joint" 10 | 11 | init_state: 12 | # target angles [rad] when action = 0.0 13 | default_joint_angle: 14 | abad_L_Joint: 0.0 15 | hip_L_Joint: 0.0 16 | knee_L_Joint: 0.0 17 | 18 | abad_R_Joint: 0.0 19 | hip_R_Joint: 0.0 20 | knee_R_Joint: 0.0 21 | 22 | control: 23 | # PD Drive parameters: 24 | stiffness: 42.0 25 | damping: 3.5 26 | action_scale_pos: 0.25 27 | decimation: 10 28 | user_torque_limit: 80 29 | 30 | normalization: 31 | encoder_normalize: True 32 | clip_scales: 33 | clip_observations: 100. 34 | clip_actions: 100. 35 | obs_scales: 36 | lin_vel: 2.0 37 | ang_vel: 0.25 38 | dof_pos: 1.0 39 | dof_vel: 0.05 40 | 41 | size: 42 | actions_size: 6 43 | observations_size: 30 44 | commands_size: 3 45 | obs_history_length: 10 46 | encoder_output_size: 3 47 | 48 | stand_mode: 49 | stand_duration: 1.0 50 | 51 | imu_orientation_offset: 52 | roll: 0.0 53 | pitch: 0.0 54 | yaw: 0.0 55 | 56 | user_cmd_scales: 57 | lin_vel_x: 1.5 58 | lin_vel_y: 1.0 59 | ang_vel_yaw: 0.5 60 | -------------------------------------------------------------------------------- /controllers/model/PF_P441A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441A/policy/encoder.onnx -------------------------------------------------------------------------------- /controllers/model/PF_P441A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441A/policy/policy.onnx -------------------------------------------------------------------------------- /controllers/model/PF_P441B/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | loop_frequency: 500 3 | joint_names: 4 | - "abad_L_Joint" 5 | - "hip_L_Joint" 6 | - "knee_L_Joint" 7 | - "abad_R_Joint" 8 | - "hip_R_Joint" 9 | - "knee_R_Joint" 10 | 11 | init_state: 12 | # target angles [rad] when action = 0.0 13 | default_joint_angle: 14 | abad_L_Joint: 0.0 15 | hip_L_Joint: 0.0 16 | knee_L_Joint: 0.0 17 | 18 | abad_R_Joint: 0.0 19 | hip_R_Joint: 0.0 20 | knee_R_Joint: 0.0 21 | 22 | control: 23 | # PD Drive parameters: 24 | stiffness: 42.0 25 | damping: 3.5 26 | action_scale_pos: 0.25 27 | decimation: 10 28 | user_torque_limit: 80 29 | 30 | normalization: 31 | encoder_normalize: True 32 | clip_scales: 33 | clip_observations: 100. 34 | clip_actions: 100. 35 | obs_scales: 36 | lin_vel: 2.0 37 | ang_vel: 0.25 38 | dof_pos: 1.0 39 | dof_vel: 0.05 40 | 41 | size: 42 | actions_size: 6 43 | observations_size: 30 44 | commands_size: 3 45 | obs_history_length: 10 46 | encoder_output_size: 3 47 | 48 | stand_mode: 49 | stand_duration: 1.0 50 | 51 | imu_orientation_offset: 52 | roll: 0.0 53 | pitch: 0.0 54 | yaw: 0.0 55 | 56 | user_cmd_scales: 57 | lin_vel_x: 1.5 58 | lin_vel_y: 1.0 59 | ang_vel_yaw: 0.5 60 | -------------------------------------------------------------------------------- /controllers/model/PF_P441B/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441B/policy/encoder.onnx -------------------------------------------------------------------------------- /controllers/model/PF_P441B/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441B/policy/policy.onnx -------------------------------------------------------------------------------- /controllers/model/PF_P441C/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | loop_frequency: 500 3 | joint_names: 4 | - "abad_L_Joint" 5 | - "hip_L_Joint" 6 | - "knee_L_Joint" 7 | - "abad_R_Joint" 8 | - "hip_R_Joint" 9 | - "knee_R_Joint" 10 | 11 | init_state: 12 | # target angles [rad] when action = 0.0 13 | default_joint_angle: 14 | abad_L_Joint: 0.0 15 | hip_L_Joint: 0.0 16 | knee_L_Joint: 0.0 17 | 18 | abad_R_Joint: 0.0 19 | hip_R_Joint: 0.0 20 | knee_R_Joint: 0.0 21 | 22 | control: 23 | # PD Drive parameters: 24 | stiffness: 42.0 25 | damping: 3.5 26 | action_scale_pos: 0.25 27 | decimation: 10 28 | user_torque_limit: 80 29 | 30 | normalization: 31 | encoder_normalize: True 32 | clip_scales: 33 | clip_observations: 100. 34 | clip_actions: 100. 35 | obs_scales: 36 | lin_vel: 2.0 37 | ang_vel: 0.25 38 | dof_pos: 1.0 39 | dof_vel: 0.05 40 | 41 | size: 42 | actions_size: 6 43 | observations_size: 30 44 | commands_size: 3 45 | obs_history_length: 10 46 | encoder_output_size: 3 47 | 48 | stand_mode: 49 | stand_duration: 1.0 50 | 51 | imu_orientation_offset: 52 | roll: 0.0 53 | pitch: 0.0 54 | yaw: 0.0 55 | 56 | user_cmd_scales: 57 | lin_vel_x: 1.5 58 | lin_vel_y: 1.0 59 | ang_vel_yaw: 0.5 60 | -------------------------------------------------------------------------------- /controllers/model/PF_P441C/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441C/policy/encoder.onnx -------------------------------------------------------------------------------- /controllers/model/PF_P441C/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441C/policy/policy.onnx -------------------------------------------------------------------------------- /controllers/model/PF_P441C2/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | loop_frequency: 500 3 | joint_names: 4 | - "abad_L_Joint" 5 | - "hip_L_Joint" 6 | - "knee_L_Joint" 7 | - "abad_R_Joint" 8 | - "hip_R_Joint" 9 | - "knee_R_Joint" 10 | 11 | init_state: 12 | # target angles [rad] when action = 0.0 13 | default_joint_angle: 14 | abad_L_Joint: 0.0 15 | hip_L_Joint: 0.0 16 | knee_L_Joint: 0.0 17 | 18 | abad_R_Joint: 0.0 19 | hip_R_Joint: 0.0 20 | knee_R_Joint: 0.0 21 | 22 | control: 23 | # PD Drive parameters: 24 | stiffness: 42.0 25 | damping: 3.5 26 | action_scale_pos: 0.25 27 | decimation: 10 28 | user_torque_limit: 80 29 | 30 | normalization: 31 | encoder_normalize: True 32 | clip_scales: 33 | clip_observations: 100. 34 | clip_actions: 100. 35 | obs_scales: 36 | lin_vel: 2.0 37 | ang_vel: 0.25 38 | dof_pos: 1.0 39 | dof_vel: 0.05 40 | 41 | size: 42 | actions_size: 6 43 | observations_size: 30 44 | commands_size: 3 45 | obs_history_length: 10 46 | encoder_output_size: 3 47 | 48 | stand_mode: 49 | stand_duration: 1.0 50 | 51 | imu_orientation_offset: 52 | roll: 0.0 53 | pitch: 0.0 54 | yaw: 0.0 55 | 56 | user_cmd_scales: 57 | lin_vel_x: 1.5 58 | lin_vel_y: 1.0 59 | ang_vel_yaw: 0.5 60 | -------------------------------------------------------------------------------- /controllers/model/PF_P441C2/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441C2/policy/encoder.onnx -------------------------------------------------------------------------------- /controllers/model/PF_P441C2/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_P441C2/policy/policy.onnx -------------------------------------------------------------------------------- /controllers/model/PF_TRON1A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | loop_frequency: 500 3 | joint_names: 4 | - "abad_L_Joint" 5 | - "hip_L_Joint" 6 | - "knee_L_Joint" 7 | - "abad_R_Joint" 8 | - "hip_R_Joint" 9 | - "knee_R_Joint" 10 | 11 | init_state: 12 | # target angles [rad] when action = 0.0 13 | default_joint_angle: 14 | abad_L_Joint: 0.0 15 | hip_L_Joint: 0.0 16 | knee_L_Joint: 0.0 17 | 18 | abad_R_Joint: 0.0 19 | hip_R_Joint: 0.0 20 | knee_R_Joint: 0.0 21 | 22 | control: 23 | # PD Drive parameters: 24 | stiffness: 42.0 25 | damping: 3.5 26 | action_scale_pos: 0.25 27 | decimation: 10 28 | user_torque_limit: 80 29 | 30 | normalization: 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 | -------------------------------------------------------------------------------- /controllers/model/PF_TRON1A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_TRON1A/policy/encoder.onnx -------------------------------------------------------------------------------- /controllers/model/PF_TRON1A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/PF_TRON1A/policy/policy.onnx -------------------------------------------------------------------------------- /controllers/model/SF_TRON1A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | loop_frequency: 500 3 | joint_names: 4 | - "abad_L_Joint" 5 | - "hip_L_Joint" 6 | - "knee_L_Joint" 7 | - "ankle_L_Joint" 8 | - "abad_R_Joint" 9 | - "hip_R_Joint" 10 | - "knee_R_Joint" 11 | - "ankle_R_Joint" 12 | 13 | init_state: 14 | # target angles [rad] when action = 0.0 15 | default_joint_angle: 16 | abad_L_Joint: 0.0 17 | hip_L_Joint: 0.13 18 | knee_L_Joint: 0.0 19 | ankle_L_Joint: 0.0 20 | 21 | abad_R_Joint: 0.0 22 | hip_R_Joint: -0.13 23 | knee_R_Joint: 0.0 24 | ankle_R_Joint: 0.0 25 | 26 | control: 27 | # PD Drive parameters: 28 | stiffness: 45.0 29 | damping: 3.0 # 1.5 30 | ankle_joint_damping: 1.5 # 0.8 31 | ankle_joint_torque_limit: 20 32 | action_scale_pos: 0.25 33 | decimation: 10 34 | user_torque_limit: 80 35 | 36 | normalization: 37 | clip_scales: 38 | clip_observations: 100. 39 | clip_actions: 100. 40 | obs_scales: 41 | lin_vel: 2.0 42 | ang_vel: 0.25 43 | dof_pos: 1.0 44 | dof_vel: 0.05 45 | 46 | size: 47 | actions_size: 8 48 | observations_size: 36 49 | obs_history_length: 5 50 | encoder_output_size: 3 51 | commands_size: 5 52 | 53 | gait: 54 | frequencies: 1.3 55 | swing_height: 0.12 56 | 57 | stand_mode: 58 | stand_duration: 1.0 59 | 60 | imu_orientation_offset: 61 | roll: 0.0 62 | pitch: 0.0 63 | yaw: 0.0 64 | 65 | user_cmd_scales: 66 | lin_vel_x: 1.0 67 | lin_vel_y: 0.5 68 | ang_vel_yaw: 1.0 -------------------------------------------------------------------------------- /controllers/model/SF_TRON1A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/SF_TRON1A/policy/encoder.onnx -------------------------------------------------------------------------------- /controllers/model/SF_TRON1A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/SF_TRON1A/policy/policy.onnx -------------------------------------------------------------------------------- /controllers/model/WF_TRON1A/params.yaml: -------------------------------------------------------------------------------- 1 | PointfootCfg: 2 | loop_frequency: 500 3 | joint_names: 4 | - "abad_L_Joint" 5 | - "hip_L_Joint" 6 | - "knee_L_Joint" 7 | - "wheel_L_Joint" 8 | - "abad_R_Joint" 9 | - "hip_R_Joint" 10 | - "knee_R_Joint" 11 | - "wheel_R_Joint" 12 | 13 | init_state: 14 | # target angles [rad] when action = 0.0 15 | default_joint_angle: 16 | abad_L_Joint: 0.0 17 | hip_L_Joint: 0.0 18 | knee_L_Joint: 0.0 19 | wheel_L_Joint: 0.0 20 | 21 | abad_R_Joint: 0.0 22 | hip_R_Joint: 0.0 23 | knee_R_Joint: 0.0 24 | wheel_R_Joint: 0.0 25 | 26 | control: 27 | # PD Drive parameters: 28 | stiffness: 42.0 29 | damping: 2.5 30 | wheel_joint_damping: 0.8 31 | wheel_joint_torque_limit: 40 32 | action_scale_pos: 0.25 33 | decimation: 10 34 | user_torque_limit: 80 35 | 36 | normalization: 37 | clip_scales: 38 | clip_observations: 100. 39 | clip_actions: 100. 40 | obs_scales: 41 | lin_vel: 2.0 42 | ang_vel: 0.25 43 | dof_pos: 1.0 44 | dof_vel: 0.05 45 | 46 | size: 47 | actions_size: 8 48 | observations_size: 28 # 31 49 | commands_size: 3 50 | obs_history_length: 10 51 | jointpos_idxs: [0, 1, 2, 4, 5, 6] 52 | encoder_output_size: 3 53 | 54 | stand_mode: 55 | stand_duration: 1.0 56 | 57 | imu_orientation_offset: 58 | roll: 0.0 59 | pitch: 0.0 60 | yaw: 0.0 61 | 62 | user_cmd_scales: 63 | lin_vel_x: 1.5 64 | lin_vel_y: 1.0 65 | ang_vel_yaw: 0.5 66 | -------------------------------------------------------------------------------- /controllers/model/WF_TRON1A/policy/encoder.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/WF_TRON1A/policy/encoder.onnx -------------------------------------------------------------------------------- /controllers/model/WF_TRON1A/policy/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/controllers/model/WF_TRON1A/policy/policy.onnx -------------------------------------------------------------------------------- /doc/simulator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/limxdynamics/rl-deploy-with-python/a6b97160dc6970a300f76112a90749c6430a556c/doc/simulator.gif -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import limxsdk.robot.Robot as Robot 4 | import limxsdk.robot.RobotType as RobotType 5 | import controllers as controllers 6 | 7 | if __name__ == '__main__': 8 | # Get the robot type from the environment variable 9 | robot_type = os.getenv("ROBOT_TYPE") 10 | 11 | # Check if the ROBOT_TYPE environment variable is set, otherwise exit with an error 12 | if not robot_type: 13 | print("Error: Please set the ROBOT_TYPE using 'export ROBOT_TYPE='.") 14 | sys.exit(1) 15 | 16 | # Create a Robot instance of the specified type 17 | robot = Robot(RobotType.PointFoot) 18 | 19 | # Default IP address for the robot 20 | robot_ip = "127.0.0.1" 21 | 22 | # Check if command-line argument is provided for robot IP 23 | if len(sys.argv) > 1: 24 | robot_ip = sys.argv[1] 25 | 26 | # Initialize the robot with the provided IP address 27 | if not robot.init(robot_ip): 28 | sys.exit() 29 | 30 | # Determine if the simulation is running 31 | start_controller = robot_ip == "127.0.0.1" 32 | 33 | # Create and run the controller 34 | if robot_type.startswith("PF"): 35 | controller = controllers.PointfootController(f'{os.path.dirname(os.path.abspath(__file__))}/controllers/model', robot, robot_type, start_controller) 36 | controller.run() 37 | elif robot_type.startswith("WF"): 38 | controller = controllers.WheelfootController(f'{os.path.dirname(os.path.abspath(__file__))}/controllers/model', robot, robot_type, start_controller) 39 | controller.run() 40 | elif robot_type.startswith("SF"): 41 | controller = controllers.SolefootController(f'{os.path.dirname(os.path.abspath(__file__))}/controllers/model', robot, robot_type, start_controller) 42 | controller.run() 43 | else: 44 | print(f"Error: unknow robot type '{robot_type}'") 45 | --------------------------------------------------------------------------------