├── .gitignore ├── .gitmodules ├── .vscode ├── c_cpp_properties.json ├── launch.json └── settings.json ├── CMakeLists.txt ├── README.md ├── biped_controller.py ├── checkpoint ├── baseline │ ├── config.yaml │ └── policy.onnx └── passive │ ├── config.yaml │ └── policy.onnx ├── doc └── image │ ├── dukehumanoidv1-thumbnails.gif │ └── dukehumanoidv1-thumbnails_1.gif ├── gamepad.py ├── motor_controller.py ├── numpy_ringbuffer.py ├── publisher.py ├── sensor_controller.py ├── setup └── conda_env.yaml ├── src ├── Readme.md ├── ethercat_motor.h ├── ethercat_motor_py.cpp ├── pybind_test.cpp └── simple_test.cpp ├── sshkeyboard_pd_test.py ├── teensy_comm └── teensy_comm.ino └── trajectory_PD_test.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | 35 | # Byte-compiled / optimized / DLL files 36 | __pycache__/ 37 | *.py[cod] 38 | *$py.class 39 | 40 | # Distribution / packaging 41 | .Python 42 | build/ 43 | build2/ 44 | develop-eggs/ 45 | dist/ 46 | downloads/ 47 | eggs/ 48 | .eggs/ 49 | lib/ 50 | lib64/ 51 | parts/ 52 | sdist/ 53 | var/ 54 | wheels/ 55 | share/python-wheels/ 56 | *.egg-info/ 57 | .installed.cfg 58 | *.egg 59 | MANIFEST -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "SOEM"] 2 | path = ext/SOEM 3 | url = https://github.com/OpenEtherCATsociety/SOEM.git -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "${workspaceFolder}/ext/SOEM/soem", 8 | "${env:HOME}/repo/micromamba/envs/py38/include/**", 9 | "${env:HOME}/repo/micromamba/envs/py38/include" 10 | ], 11 | "defines": [], 12 | "compilerPath": "/usr/bin/gcc", 13 | "cStandard": "c17", 14 | "cppStandard": "gnu++17", 15 | "intelliSenseMode": "${default}", 16 | "configurationProvider": "ms-vscode.cmake-tools" 17 | } 18 | ], 19 | "version": 4 20 | } -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "name": "(gdb) Launch", 6 | "type": "cppdbg", 7 | "request": "launch", 8 | // Resolved by CMake Tools: 9 | "program": "${command:cmake.launchTargetPath}", 10 | "args": ["enp3s0", "csv", "0,0,0,0,0,0", "0.05", "20"], 11 | "stopAtEntry": false, 12 | "cwd": "${workspaceFolder}/build", 13 | // "miDebuggerPath": "${workspaceFolder}/.vscode/sudo_gdb.sh", // Path to your script 14 | "environment": [ 15 | { 16 | // add the directory where our target was built to the PATHs 17 | // it gets resolved by CMake Tools: 18 | "name": "PATH", 19 | "value": "${env:PATH}:${command:cmake.getLaunchTargetDirectory}" 20 | }, 21 | { 22 | "name": "OTHER_VALUE", 23 | "value": "Something something" 24 | } 25 | ], 26 | "MIMode": "gdb", 27 | "setupCommands": [ 28 | { 29 | "description": "Enable pretty-printing for gdb", 30 | "text": "-enable-pretty-printing", 31 | "ignoreFailures": true 32 | } 33 | ] 34 | } 35 | ] 36 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | // https://github.com/microsoft/vscode-cmake-tools/blob/main/docs/cmake-settings.md 3 | "cmake.generator":"Ninja", 4 | "cmake.configureSettings": { 5 | "CMAKE_PREFIX_PATH": "${env:HOME}/repo/micromamba/envs/py38", // conda env 6 | "Python_EXECUTABLE": "${env:HOME}/repo/micromamba/envs/py38/bin/python", 7 | "CMAKE_TOOLCHAIN_FILE":"${env:HOME}/repo/vcpkg/scripts/buildsystems/vcpkg.cmake", 8 | }, 9 | "files.associations": { 10 | "compare": "cpp", 11 | "concepts": "cpp", 12 | "type_traits": "cpp", 13 | "cmath": "cpp", 14 | "limits": "cpp", 15 | "new": "cpp", 16 | "stdexcept": "cpp", 17 | "system_error": "cpp", 18 | "array": "cpp", 19 | "functional": "cpp", 20 | "tuple": "cpp", 21 | "utility": "cpp", 22 | "*.tcc": "cpp", 23 | "string": "cpp", 24 | "string_view": "cpp", 25 | "atomic": "cpp", 26 | "bit": "cpp", 27 | "bitset": "cpp", 28 | "cctype": "cpp", 29 | "charconv": "cpp", 30 | "chrono": "cpp", 31 | "cinttypes": "cpp", 32 | "clocale": "cpp", 33 | "codecvt": "cpp", 34 | "condition_variable": "cpp", 35 | "cstdarg": "cpp", 36 | "cstddef": "cpp", 37 | "cstdint": "cpp", 38 | "cstdio": "cpp", 39 | "cstdlib": "cpp", 40 | "cstring": "cpp", 41 | "ctime": "cpp", 42 | "cwchar": "cpp", 43 | "cwctype": "cpp", 44 | "deque": "cpp", 45 | "forward_list": "cpp", 46 | "list": "cpp", 47 | "map": "cpp", 48 | "set": "cpp", 49 | "unordered_map": "cpp", 50 | "unordered_set": "cpp", 51 | "vector": "cpp", 52 | "exception": "cpp", 53 | "algorithm": "cpp", 54 | "iterator": "cpp", 55 | "memory": "cpp", 56 | "memory_resource": "cpp", 57 | "numeric": "cpp", 58 | "optional": "cpp", 59 | "random": "cpp", 60 | "ratio": "cpp", 61 | "initializer_list": "cpp", 62 | "iomanip": "cpp", 63 | "iosfwd": "cpp", 64 | "iostream": "cpp", 65 | "istream": "cpp", 66 | "mutex": "cpp", 67 | "numbers": "cpp", 68 | "ostream": "cpp", 69 | "semaphore": "cpp", 70 | "sstream": "cpp", 71 | "stop_token": "cpp", 72 | "streambuf": "cpp", 73 | "thread": "cpp", 74 | "typeindex": "cpp", 75 | "typeinfo": "cpp", 76 | "variant": "cpp", 77 | "complex": "cpp", 78 | "csignal": "cpp", 79 | "any": "cpp", 80 | "coroutine": "cpp", 81 | "source_location": "cpp", 82 | "future": "cpp", 83 | "span": "cpp", 84 | "valarray": "cpp", 85 | "dense": "cpp", 86 | "core": "cpp", 87 | "geometry": "cpp", 88 | "svd": "cpp", 89 | "qr": "cpp", 90 | "eigenvalues": "cpp", 91 | "lu": "cpp", 92 | "cholesky": "cpp", 93 | "*.inc": "cpp" 94 | } 95 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10 ) # Adjust version as needed 2 | 3 | 4 | project(biped 5 | LANGUAGES C CXX) 6 | set (CMAKE_CXX_STANDARD 17) 7 | 8 | # # Enable compiler caching 9 | # set(CMAKE_CXX_COMPILER_LAUNCHER ccache) 10 | 11 | # Use Ninja for faster builds 12 | # set(CMAKE_GENERATOR ninja) 13 | ## cmake -G Ninja 14 | 15 | message(STATUS "CMAKE_PREFIX_PATH: ${CMAKE_PREFIX_PATH}") 16 | 17 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 18 | 19 | # need to find with python3 -c "from distutils import sysconfig; print(sysconfig.get_python_inc())" 20 | set(Python_EXECUTABLE "/home/grl/repo/micromamba/envs/py38/bin/python") 21 | # set(Python3_INCLUDE_DIRS "/home/grl/repo/micromamba/envs/py38/include/python3.8/") 22 | find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) 23 | 24 | # #configure CMake to perform an optimized release build by default unless another build type is specified. 25 | # # Without this addition, binding code may run slowly and produce large binaries. 26 | # if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 27 | # set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) 28 | # set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 29 | # endif() 30 | 31 | 32 | file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/__init__.py") # Creates an empty file 33 | 34 | # set(CMAKE_TOOLCHAIN_FILE "/home/grl/repo/vcpkg/scripts/buildsystems/vcpkg.cmake") 35 | # cmake .. "-DCMAKE_TOOLCHAIN_FILE=/home/grl/repo/vcpkg/scripts/buildsystems/vcpkg.cmake" 36 | 37 | 38 | find_package(Eigen3 CONFIG REQUIRED) 39 | # target_link_libraries(main PRIVATE Eigen3::Eigen) 40 | # target_include_directories(main PRIVATE ${EIGEN3_INCLUDE_DIR}) 41 | 42 | 43 | find_package(pybind11 REQUIRED) 44 | 45 | find_package(Threads REQUIRED) 46 | 47 | # find_package(iir REQUIRED) # filters 48 | find_package(iir REQUIRED) 49 | 50 | # Include SOEM source files into your build 51 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/ext/SOEM) 52 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/ext/SOEM/test/linux/slaveinfo) # build slaveinfo 53 | 54 | 55 | 56 | if (MSVC) 57 | add_compile_options(/W4) 58 | else() 59 | add_compile_options(-Wall) 60 | endif() 61 | 62 | 63 | # Create your executable 64 | add_executable(simple_test 65 | src/simple_test.cpp 66 | src/ethercat_motor.h 67 | ) 68 | target_link_libraries(simple_test PRIVATE soem) # 'soem' is the target created by add_subdirectory 69 | target_include_directories(simple_test PRIVATE SOEM) 70 | 71 | # target_link_libraries(simple_test PRIVATE pybind11::module) 72 | # target_include_directories(simple_test PRIVATE ${pybind11_INCLUDE_DIRS}) 73 | target_link_libraries(simple_test PRIVATE Eigen3::Eigen) 74 | target_include_directories(simple_test PRIVATE ${EIGEN3_INCLUDE_DIR}) 75 | target_link_libraries(simple_test PRIVATE iir::iir_static) 76 | 77 | pybind11_add_module(ethercat_motor_py 78 | src/ethercat_motor_py.cpp 79 | src/ethercat_motor.h 80 | ) 81 | target_link_libraries(ethercat_motor_py PRIVATE soem) 82 | target_include_directories(ethercat_motor_py PRIVATE SOEM) 83 | target_link_libraries(ethercat_motor_py PRIVATE pybind11::module) 84 | target_include_directories(ethercat_motor_py PRIVATE ${pybind11_INCLUDE_DIRS}) 85 | target_link_libraries(ethercat_motor_py PRIVATE Eigen3::Eigen) 86 | target_include_directories(ethercat_motor_py PRIVATE ${EIGEN3_INCLUDE_DIR}) 87 | target_link_libraries(ethercat_motor_py PRIVATE iir::iir_static) 88 | 89 | 90 | pybind11_add_module(pybind_test src/pybind_test.cpp) 91 | # add_executable(pybind_test src/pybind_test.cpp) 92 | target_link_libraries(pybind_test PRIVATE pybind11::module) 93 | target_include_directories(pybind_test PRIVATE ${pybind11_INCLUDE_DIRS}) 94 | target_link_libraries(pybind_test PRIVATE Eigen3::Eigen) 95 | target_include_directories(pybind_test PRIVATE ${EIGEN3_INCLUDE_DIR}) 96 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # Biped controller code 3 | 4 | 5 | 6 | 7 | 8 | 9 |
Duke Humanoid V1 Thumbnail 1Duke Humanoid V1 Thumbnail 2
10 | 11 | For information on manufacturing and deploying the humanoid, please visit our 🔧 12 | **[hardware wiki](https://www.notion.so/Duke-Humanoid-V1-b-38d54de887d1403a82f2367490c45b89)**. 13 | 14 | ## Project structure 15 | 16 | ```bash 17 | 18 | ├── build                 # Compiled binaries and build artifacts 19 | │   ├── ethercat_motor_py.cpython-38-x86_64-linux-gnu.so  # Compiled Python extension 20 | │   ├── simple_test 21 | │   └── ... 22 | ├── checkpoint            # RL model checkpoints 23 | │   ├── baseline 24 | │   └── ... 25 | ├── setup                   26 | │   └── conda_env.yaml    # conda env configuration file 27 | ├── CMakeLists.txt        # CMake build configuration file 28 | ├── ext                   # External dependencies and libraries 29 | │   └── SOEM              # EtherCAT master library 30 | ├── biped_controller.py   # Main entrance for biped controller 31 | ├── motor_controller.py   # motor controller and initialization code 32 | ├── sensor_controller.py  # Sensor communication with teensy 33 | ├── trajectory_PD_test.py # Trajectory PD test script 34 | ├── gampad.py             # gamepad control script 35 | ├── numpy_ringbuffer.py   # Ring buffer implementation using NumPy 36 | ├── publisher.py          # Message publishing/receiving utility 37 | ├── README.md   38 | ├── src                        # C++ source code directory 39 | │   ├── ethercat_motor.h       # EtherCAT motor control header 40 | │   ├── ethercat_motor_py.cpp  # Python binding for EtherCAT motor control 41 | │   ├── pybind_test.cpp        # Pybind test implementation 42 | │   ├── Readme.md              # Source code README file 43 | │   └── simple_test.cpp        # Simple test implementation 44 | ├── sshkeyboard_pd_test.py     # SSH keyboard PD test script 45 | └──  teensy_comm 46 |     └── teensy_comm.ino        # Teensy communication Arduino code 47 | 48 | ``` 49 | 50 | ## Setting up 51 | 52 | ### Tested environment 53 | - Operating system: Ubuntu 22.04 with CUDA 12.3 54 | - Development environment: Visual Studio Code (VSCode) 55 | - C++ build tools: 56 | - vcpkg (package manager) 57 | - CMake (build system generator) 58 | - GCC 12 (compiler) 59 | - Python environment management: micromanba 60 | 61 | ### Setup python virtual environment 62 | first [install Micromamba](https://mamba.readthedocs.io/en/latest/installation/micromamba-installation.html) if you have not done so. Recommended to install at `~/repo/micromamba` 63 | 64 | 65 | ```bash 66 | "${SHELL}" <(curl -L micro.mamba.pm/install.sh) 67 | ``` 68 | 69 | Setup a python virtual envirnpoment named  `py38` with conda yaml file `setup/conda_env.yaml` 70 | 71 | ```bash 72 | alias conda="micromamba" 73 | 74 | # Create environment 75 | conda env create --file setup/conda_env_py38.yaml -y 76 | 77 | # Activate the environment 78 | conda activate py38 79 | 80 | # Export library path 81 | export LD_LIBRARY_PATH=${CONDA_PREFIX}/lib 82 | ``` 83 | 84 | 85 | ### Setup vcpkg 86 | 87 | follow the instructions to install vcpkg: 88 | https://learn.microsoft.com/en-us/vcpkg/get_started/get-started?pivots=shell-bash 89 | 90 | ```bash 91 | git clone https://github.com/microsoft/vcpkg.git 92 | cd vcpkg && ./bootstrap-vcpkg.sh 93 | ./vcpkg install msgpack eigen3 iir1 94 | 95 | ``` 96 | 97 | Set vcpkg environment variables: 98 | ```bash 99 | export VCPKG_ROOT=/path/to/vcpkg 100 | export PATH=$VCPKG_ROOT:$PATH 101 | ``` 102 | 103 | 104 | 105 | ### Build the c++ code 106 | 107 | Create the build directory and navigate to it. 108 | 109 | For example: 110 | - the python virtual environment is `~/repo/micromamba/envs/py38` 111 | - the python executatble is `~/repo/micromamba/envs/py38/bin/python` 112 | - the vcpkg tool chain file is `~/repo/vcpkg/scripts/buildsystems/vcpkg.cmake` 113 | 114 | change these variables to your actual variable to run the following: 115 | 116 | ```bash 117 | mkdir build 118 | cd build 119 | cmake .. -DCMAKE_BUILD_TYPE=Debug -DCMAKE_PREFIX_PATH=~/repo/micromamba/envs/py38 -DPython_EXECUTABLE=~/repo/micromamba/envs/py38/bin/python -DCMAKE_TOOLCHAIN_FILE=~/repo/vcpkg/scripts/buildsystems/vcpkg.cmake -G Ninja 120 | ``` 121 | 122 | ## setup python environment 123 | 124 | 125 | 126 | ### Setup vscode 127 | Install VSCode: [Download and install](https://code.visualstudio.com/download) vscode if you have not done so 128 | 129 | Install vscode Extensions: 130 | - C/C++: ms-vscode.cpptools 131 | - CMake Tools: ms-vscode.cmake-tools 132 | - Python: ms-python.python 133 | Optionally Install other extensions such as Git based on your needs. 134 | 135 | To configure the project using vscode: 136 | - Open the project folder. 137 | - Ensure C/C++ uses GCC 12. 138 | - Select the correct Python virtual environment. 139 | - Configure CMake Tools if needed, You should replace [.vscode/c_cpp_properties.json](.vscode/c_cpp_properties.json) to point to your Python path: 140 | ``` 141 |   "includePath": [ 142 |       "${workspaceFolder}/**", 143 |       "${workspaceFolder}/ext/SOEM/soem", 144 |       // replace those to point to your python include 145 |       "${env:HOME}/repo/micromamba/envs/py38/include/**", 146 |       "${env:HOME}/repo/micromamba/envs/py38/include" 147 | ], 148 | ``` 149 | Also replace [.vscode/settings.json](.vscode/settings.json) to poin to your python path 150 | ``` 151 |   "cmake.configureSettings": { 152 |       // replace to your path 153 |       "CMAKE_PREFIX_PATH": "${env:HOME}/repo/micromamba/envs/py38", 154 |       "Python_EXECUTABLE": "${env:HOME}/repo/micromamba/envs/py38/bin/python", 155 |       "CMAKE_TOOLCHAIN_FILE":"${env:HOME}/repo/vcpkg/scripts/buildsystems/vcpkg.cmake", 156 | }, 157 | ``` 158 | 159 | ### Build the Arduino code 160 | 161 | setup arduino + Teensyduino 162 | - Install the Arduino IDE from https://www.arduino.cc/en/software 163 | - Install Teensyduino from https://www.pjrc.com/teensy/td_download.html 164 | - Teensy Arduino usage can be found at https://www.pjrc.com/teensy/td_usage.html 165 | - Install Adurino library: go to Sketch -> Include Library -> Manage Libraries..., In the search bar, type "TCA9548" and then "packetizer", and install. 166 | 167 | Build the teensy code: in the Arduino IDE, open `teensy_comm/teensy_comm.ino` and upload. 168 | 169 | 170 | ## Run 171 | 172 | ### Initialization 173 | First confirm the sensors are working 174 | ```bash 175 | sudo chrt -f 99 $(which python) -u sensor_controller.py 176 | ``` 177 | 178 | Then, initialize the joint at the zero standing position. refer to wiki:[Joint initialization](https://sleepy-yoke-a21.notion.site/Sim2real-ebbd3cc351294a97a796040b54091273#a068aad6d80e4ee8bc613af9ba50b854) 179 | ```bash 180 | sudo chrt -f 99 $(which python) -u motor_controller.py 181 | 182 | ``` 183 | after the initalization you can choose either to run dynamic matching with the robot hanging, 184 | or run the RL biped controller. 185 | 186 | 187 | ### Dynamic matching/Trajectory test 188 | in one terminal run `trajectory_PD_test.py` 189 | ```bash 190 | sudo chrt -f 99 $(which python) -u trajectory_PD_test.py 191 | 192 | ``` 193 | in another terminal run keyboard control code 194 | ```bash 195 | sudo chrt -f 99 $(which python) -u sshkeyboard_pd_test.py 196 | ``` 197 | 198 | ### RL biped control 199 | run `biped_controller.py`. 200 | ``` 201 | sudo chrt -f 99 $(which python) -u biped_controller.py 202 | ``` 203 | in another terminal run `gampad.py` 204 | ``` 205 | sudo chrt -f 99 $(which python) -u gampad.py 206 | ``` 207 | 208 | Note: 209 | 210 | For the gamepad, we use [8Bitdo Ultimate Controller](https://www.8bitdo.com/ultimate-bluetooth-controller/) with a 2.4g adapter. The gamepad code should work with any gampad that pygame supports with some modification. -------------------------------------------------------------------------------- /biped_controller.py: -------------------------------------------------------------------------------- 1 | # DEBUG LIBRARIES 2 | import sys 3 | import time 4 | import signal 5 | import onnxruntime as ort 6 | import numpy as np 7 | from omegaconf import OmegaConf 8 | import os 9 | import copy 10 | 11 | from operator import itemgetter 12 | from sensor_controller import SensorController 13 | from motor_controller import MotorController, CONTROL_MODE 14 | from publisher import DataPublisher, DataReceiver 15 | from numpy_ringbuffer import RingArrayBuffer 16 | 17 | class Model: 18 | def __init__(self, onnx_model_path) -> None: 19 | self.ort_model = ort.InferenceSession(onnx_model_path) 20 | 21 | def get_action(self, obs): 22 | mu, log_std, value = self.ort_model.run( 23 | None, 24 | {"obs": np.asarray(obs, dtype=np.float32).reshape(1, -1)}, 25 | ) 26 | return mu 27 | 28 | 29 | class BipedController: 30 | 31 | def __init__(self) -> None: 32 | 33 | # baseline policy 34 | path = "checkpoint/baseline" 35 | # # passive (uncomment this line to use it) 36 | # path = "checkpoint/passive" 37 | 38 | 39 | if (not path.startswith("checkpoint")) and (not os.path.isabs(path)): # Check if it's relative 40 | root_path = "/home/grl/repo/legged_env" # change to your root path fo 41 | path = os.path.join(root_path, path) 42 | 43 | model_path = f"{path}/policy.onnx" 44 | config_path = f"{path}/config.yaml" 45 | 46 | # init model 47 | self.model = Model(model_path) 48 | 49 | cfg = OmegaConf.load(config_path) 50 | 51 | # cfg.task.env.control.actionScale 52 | 53 | self.keep_still_at_zero_command: bool = cfg.task.env.get("keep_still_at_zero_command",False) 54 | 55 | self.obs_names: list = cfg.task.env.observationNames 56 | self.lin_vel_scale: float = cfg.task.env.learn.linearVelocityScale 57 | self.ang_vel_scale: float = cfg.task.env.learn.angularVelocityScale 58 | self.dof_pos_scale: float = cfg.task.env.learn.dofPositionScale 59 | self.dof_vel_scale: float = cfg.task.env.learn.dofVelocityScale 60 | 61 | self.commands_scale = np.array([self.lin_vel_scale, self.lin_vel_scale, self.ang_vel_scale]) 62 | self.command_zero_threshold = cfg.task.env.commandZeroThreshold 63 | 64 | self.dt: float = cfg.task.sim.dt 65 | self.decimation:int = cfg.task.env.control.decimation 66 | self.rl_dt_ns: float = self.dt*self.decimation*1e9 # to [ns] 67 | # print(self.rl_dt_ns) 68 | 69 | self.num_dof = 10 70 | self.num_foot = 2 71 | 72 | def get_dof_param(param): 73 | if isinstance(param, (float, int)): 74 | dof_param = np.array([param]*self.num_dof,dtype=np.float64) 75 | else: # assuming list 76 | dof_param = np.array(param,dtype=np.float64) 77 | return dof_param 78 | 79 | self.kp = get_dof_param(cfg.task.env.control.stiffness) 80 | self.kd = get_dof_param(cfg.task.env.control.damping) 81 | 82 | # self.kp = self.kp*1.1 83 | 84 | self.action_scale:float = cfg.task.env.control.actionScale 85 | # self.action_scale=0 86 | 87 | self.default_dof_pos=np.array(cfg.task.env.defaultJointPositions) 88 | 89 | # # # HACK 90 | # # self.default_dof_pos=np.zeros(10) 91 | # # self.action_scale=0 92 | # # # # HACK 93 | # self.kp[:]=10 94 | # self.kd[:]=1 95 | 96 | self.phase_freq: float = cfg.task.env.learn.guided_contact.phase_freq 97 | self.phase_stance_ratio: float = cfg.task.env.learn.guided_contact.phase_stance_ratio 98 | self.phase_swing_ratio: float = 1 - self.phase_stance_ratio 99 | # NOTE! BREAKING CHANGE 09/07 : phase start with swing first instead 100 | self.phase_start_with_swing: bool = cfg.task.env.learn.guided_contact.get("phase_start_with_swing",False) # default phase_start_with_stance in previous policy 101 | print(f"\033[93mphase_start_with_swing: {self.phase_start_with_swing}\033[0m") 102 | 103 | self.phase_offset = np.array(cfg.task.env.learn.guided_contact.phase_offset) 104 | 105 | 106 | if "enablePassiveDynamics" in cfg.task.env.learn and cfg.task.env.learn.enablePassiveDynamics: 107 | self.enablePassiveDynamics = True 108 | self.num_action = self.num_dof*2 109 | self.action_is_on_sigmoid_k: float = cfg.task.env.learn.action_is_on_sigmoid_k 110 | self.compute_action=self.compute_action_passive 111 | # BRAKING CHANGE 112 | self.min_action_is_on: float = cfg.task.env.learn.get("action_is_on_min",0) 113 | # self.min_action_is_on: float = cfg.task.env.learn.action_is_on_min 114 | 115 | else: 116 | self.enablePassiveDynamics = False 117 | self.num_action = self.num_dof 118 | self.action = np.zeros(self.num_action) 119 | self.action_to_use = np.zeros_like(self.action) 120 | 121 | self.obs_dim_dict = { 122 | "linearVelocity": 3, 123 | "angularVelocity": 3, 124 | "projectedGravity": 3, 125 | "projected_gravity_filtered": 3, 126 | "projected_gravity_xy": 2, 127 | "commands": 3, # vel_x,vel_y, vel_yaw, (excluding heading) 128 | "dofPosition": self.num_dof, 129 | "dofVelocity": self.num_dof, 130 | "dof_force_target": self.num_dof, 131 | "dof_strength": self.num_dof, 132 | "dofForce": self.num_dof, 133 | "base_height": 1, 134 | "actions": self.num_action, 135 | "last_actions": self.num_action, 136 | "contact": self.num_foot, # foot contact indicator 137 | "phase": self.num_foot*2, # phase of each foot (contact sequece) 138 | "contactTarget": self.num_foot, # foot contact indicator 139 | } 140 | 141 | self.num_stacked_obs_frame: int = cfg.task.env.get("num_stacked_obs_frame", 1) 142 | self.num_obs_single_frame: int = np.sum(itemgetter(*self.obs_names)(self.obs_dim_dict)) 143 | self.num_obs = self.num_obs_single_frame * self.num_stacked_obs_frame 144 | self.batched_obs_buf = RingArrayBuffer(buffer_len=self.num_stacked_obs_frame,shape=self.num_obs_single_frame,dtype=np.float64) 145 | # debugging 146 | import inspect 147 | for attr in inspect.getmembers(self): 148 | if not attr[0].startswith('_'): # Exclude private/internal attributes 149 | if not inspect.ismethod(attr[1]): # Exclude methods 150 | print(f"{attr[0]}: {attr[1]}") 151 | 152 | self.should_reset = False 153 | 154 | 155 | # command 156 | self.commands = np.zeros(3) 157 | self.dof_vel =np.zeros(10) 158 | 159 | # filters 160 | 161 | # initialize sensors and data collection 162 | self.sensors = SensorController() 163 | self.sensors.start() 164 | self.sensor_data = self.sensors.get_latest_data() 165 | # wait until sensor data is ready 166 | while self.sensor_data is None: 167 | self.sensor_data = self.sensors.get_latest_data() 168 | print("Waiting for sensor data to be ready...") 169 | time.sleep(0.01) 170 | print("Sensor data is ready!") 171 | 172 | # data receiver 173 | self.data_receiver = DataReceiver(port=9871,decoding="msgpack",broadcast=True) 174 | self.data_receiver_data_id = self.data_receiver.data_id # for check if data is new 175 | self.data_receiver.receive_continuously() 176 | 177 | self.data_receiver2 = DataReceiver(port=9876,decoding="msgpack",broadcast=False) 178 | self.data_receiver2_data_id = self.data_receiver2.data_id # for check if data is new 179 | # self.data_receiver2.receive_continuously() 180 | self.dof_pos_target = self.default_dof_pos 181 | 182 | self.data_publisher = DataPublisher('udp://localhost:9870',encoding="msgpack",broadcast=False) 183 | 184 | # initialize motors 185 | self.max_torque = 1000.0 186 | self.motor = MotorController( 187 | "enp3s0", CONTROL_MODE.CYCLIC_SYNC_TORQUE, 20, self.max_torque 188 | ) 189 | self.motor.run() 190 | 191 | self.motor_counter = self.motor.loop_counter 192 | self.decimation_actual = 0 193 | self.rl_dt_measured = 0 194 | self.t_ns = time.perf_counter_ns() 195 | 196 | # # # HACK remove later 197 | # self.motor.set_max_torque(np.ones(10)*1500) 198 | 199 | 200 | # setup signal handler if it has not been setup elsewhere 201 | if not signal.getsignal(signal.SIGINT): 202 | self.setup_signal_handler() 203 | 204 | 205 | def update_phase(self): 206 | """update normalized contact phase for each foot""" 207 | # TODO: may need to change into relative time 208 | self.phase = (self.phase_freq * self.t_ns*1e-9 +self.phase_offset) % 1 209 | # print(self.phase) 210 | phase_rad = self.phase*np.pi*2 211 | self.phase_sin_cos = np.hstack([np.sin(phase_rad), np.cos(phase_rad)]) 212 | # print(self.phase_sin_cos) 213 | 214 | if self.phase_start_with_swing: 215 | self.contact_target = self.phase > self.phase_swing_ratio 216 | else: 217 | self.contact_target = self.phase <= self.phase_stance_ratio 218 | 219 | self.is_zero_command = np.linalg.norm(self.commands[:3]) < self.command_zero_threshold 220 | 221 | if self.is_zero_command and self.keep_still_at_zero_command: 222 | self.contact_target[:] = 1 223 | self.commands[:] = 0 224 | # self.contact_target[self.is_zero_command] = 1 # set contact_target to 1 if zero command 225 | # self.commands[self.is_zero_command]=0 226 | 227 | 228 | def initialize_stance(self): 229 | self.motor.control_mode_int8 = CONTROL_MODE.CYCLIC_SYNC_TORQUE 230 | self.motor.kp = np.ones(10)*40 231 | self.motor.kd = np.ones(10)*8.0 232 | self.motor.use_position_pd = True 233 | self.motor.target_dof_position = self.default_dof_pos 234 | time.sleep(3) 235 | # set back to default kp kd 236 | self.motor.kp = self.kp 237 | self.motor.kd = self.kd 238 | print(f"kp={self.motor.kp}") 239 | print(f"kd={self.motor.kd}") 240 | def check_termination(self): 241 | if self.projected_gravity[2]>-0.7: # base tilt 45 degree 242 | self.should_reset = True 243 | 244 | if self.should_reset: 245 | self.motor.set_max_torque(np.zeros(self.motor.num_dof)) 246 | self.shutdown() 247 | exit() 248 | 249 | def step(self): 250 | # check if cmmd is updated 251 | self.update_command() 252 | 253 | # update observations 254 | self.update_observation() 255 | 256 | self.check_termination() 257 | 258 | # # compute action 259 | self.compute_action() 260 | # # limit check to prevent overshoot 261 | self.dof_pos_target = self.motor.limit_check(self.dof_pos_target) 262 | 263 | self.motor.target_dof_position = self.dof_pos_target 264 | if self.enablePassiveDynamics: 265 | # self.motor.set_max_torque(self.action_is_on.astype(np.float64)*1000) 266 | self.motor.torque_multiplier = self.action_is_on.astype(np.float64) 267 | 268 | motor_counter = self.motor.loop_counter 269 | self.decimation_actual = motor_counter - self.motor_counter # masured decimation per RL loop 270 | self.motor_counter = motor_counter 271 | 272 | # # if self.decimation_actual>40: 273 | # print(self.decimation_actual, time.perf_counter_ns()-self.t_ns, time.perf_counter_ns(),self.t_ns) 274 | 275 | data = { 276 | "decimation_actual": self.decimation_actual, 277 | "dof_pos": self.dof_pos, 278 | "dof_vel": self.dof_vel, 279 | "commands": self.commands, 280 | "base_ang_vel": self.base_ang_vel, 281 | "projected_gravity": self.projected_gravity, 282 | "action": self.action, 283 | "action_filtered":self.action_to_use, 284 | "phase_sin_cos": self.phase_sin_cos, 285 | "phase":self.phase, 286 | # "actions_to_use":self.action_to_use, 287 | "base_quat": self.base_quat, 288 | "contact": self.contact, 289 | "force_measurement": self.force_measurement, 290 | "contact_target": self.contact_target, 291 | # "dof_force_target": self.target_input_torque, 292 | "dof_force_target": self.motor.target_dof_torque_Nm, 293 | "dof_pos_target": self.dof_pos_target, 294 | # "rl_dt_measured_div_rl_dt": self.rl_dt_measured/self.rl_dt_ns, 295 | # "obs_buf": self.obs_buf, 296 | # "obs_buf_diff": self.obs_buf - self.obs_buf_sim, 297 | # "obs_buf_diff": np.sum(self.obs_buf - self.obs_buf_sim), 298 | 299 | } 300 | 301 | if self.enablePassiveDynamics: 302 | data["action_is_on"] = self.action_is_on 303 | 304 | self.data_publisher.publish({"real":data}) 305 | 306 | # sleep for rl_dt_ns 307 | t_ns = time.perf_counter_ns() 308 | elapsed = t_ns - self.t_ns 309 | if elapsed < self.rl_dt_ns: 310 | time.sleep((self.rl_dt_ns - elapsed)*1e-9) 311 | 312 | self.rl_dt_measured = time.perf_counter_ns()-self.t_ns 313 | self.t_ns = time.perf_counter_ns() 314 | 315 | def update_command(self): 316 | if self.data_receiver_data_id != self.data_receiver.data_id: 317 | self.data_receiver_data_id = self.data_receiver.data_id 318 | if "cmd" in self.data_receiver.data: 319 | self.commands[:] = np.array(self.data_receiver.data["cmd"]) 320 | print(f"{self.commands[:]}") 321 | if "reset" in self.data_receiver.data and self.data_receiver.data["reset"]: 322 | self.should_reset = True 323 | 324 | def compute_action(self): 325 | self.action = self.model.get_action(self.obs_buf)[0] 326 | alpha = 0.9 327 | self.action_to_use = alpha*self.action + (1-alpha)*self.action 328 | self.dof_pos_target = self.action_to_use * self.action_scale + self.default_dof_pos 329 | 330 | def compute_action_passive(self): 331 | self.action = self.model.get_action(self.obs_buf)[0] 332 | alpha = 0.9 333 | self.action_to_use = alpha*self.action + (1-alpha)*self.action_to_use 334 | self.dof_pos_target = self.action_scale * self.action_to_use[:self.num_dof] + self.default_dof_pos 335 | self.action_is_on = sigmoid_k(self.action_to_use[self.num_dof:], self.action_is_on_sigmoid_k) 336 | self.action_is_on = self.min_action_is_on + (1 - self.min_action_is_on) * self.action_is_on 337 | 338 | def update_observation(self): 339 | # Return a dictionary of observable data 340 | self.sensor_data = self.sensors.get_latest_data() 341 | if self.sensor_data is None: 342 | print("WARNING: No sensor data received!") 343 | self.should_reset = True 344 | return 345 | 346 | # print(self.sensor_data["sps"]) 347 | self.base_ang_vel = np.array(self.sensor_data["ang_vel"]) 348 | self.projected_gravity = np.array(self.sensor_data["gravity_vec"]) 349 | # # HACK:TODO VERIFY THIS 350 | # self.projected_gravity = np.array([-self.projected_gravity[0],self.projected_gravity[1],self.projected_gravity[2]]) 351 | self.base_quat = np.array(self.sensor_data['base_quat']) 352 | 353 | self.force_measurement = self.sensor_data["force_measurement"] 354 | self.contact = self.sensor_data["contact"] 355 | 356 | # self.base_ang_vel = np.zeros(3) 357 | # self.projected_gravity = np.array([0,0,-1.0]) 358 | 359 | self.update_phase() 360 | 361 | # HACK: broken force sensor repalce 362 | # self.contact = self.contact_target 363 | # self.contact[:]=0 364 | 365 | # dof_vel = self.motor.dof_vel*0.95 + self.dof_vel * 0.05 366 | # self.dof_vel = dof_vel 367 | 368 | self.dof_pos = self.motor.dof_pos 369 | self.dof_vel = self.motor.dof_vel 370 | 371 | 372 | # data, _ = self.data_receiver2.receive() 373 | # # if (self.data_receiver2.data is not None): 374 | # # data = copy.deepcopy(self.data_receiver2.data) 375 | # if data is not None: 376 | # self.base_ang_vel = np.array(data["base_ang_vel"]) 377 | # self.projected_gravity = np.array(data["projected_gravity"]) 378 | # self.dof_pos = np.array(data["dof_pos"]) 379 | # self.dof_vel = np.array(data["dof_vel"]) 380 | # self.action = np.array(data["action"]) 381 | # self.contact = np.array(data["contact"]) 382 | # # self.contact = np.zeros(2) # HACK ONLY 383 | # self.contact_target = np.array(data["contact_target"]) 384 | # self.phase = np.array(data["phase"]) 385 | # self.phase_sin_cos = np.array(data["phase_sin_cos"]) 386 | # self.commands = np.array(data["cmd"]) 387 | # self.obs_buf_sim = np.array(data["obs_buf"]) 388 | # else: 389 | # print("\033[93m" + "self.data_receiver2.data is None!!!!!!!!!!!!!!!!!!!!!!!!" + "\033[0m") 390 | # pass 391 | 392 | self.obs_dict = { 393 | # 'linearVelocity': sensor_data['linearVelocity'], # DOES NOT HAVE THIS IN REAL 394 | # "linearVelocity": np.zeros(3), # HACK: FOR DEBUGGING 395 | "angularVelocity": self.base_ang_vel * self.ang_vel_scale, 396 | "projectedGravity": self.projected_gravity, 397 | "projected_gravity_filtered": self.projected_gravity, 398 | "projected_gravity_xy": self.projected_gravity[:2], 399 | "commands": self.commands * self.commands_scale, 400 | "dofPosition": self.dof_pos * self.dof_pos_scale, 401 | "dofVelocity": self.dof_vel* self.dof_vel_scale, 402 | # "heightMap": 403 | # "actions": np.zeros(10), # HACK: FOR DEBUGGING 404 | "actions": self.action, # TODO 405 | # "actionFiltered": self.action_to_use, 406 | "phase": self.phase_sin_cos, 407 | "contactTarget":self.contact_target, 408 | "contact":self.contact, 409 | } 410 | obs_buf_single_frame = np.concatenate( 411 | itemgetter(*self.obs_names)(self.obs_dict), axis=-1 412 | ) 413 | # HACK 414 | # self.obs_buf=obs_buf_single_frame 415 | self.batched_obs_buf.add(obs_buf_single_frame) 416 | self.obs_buf = self.batched_obs_buf.get_last_n(self.num_stacked_obs_frame).ravel() 417 | 418 | def setup_signal_handler(self): 419 | def signal_handler(sig, frame): 420 | print("Keyboard Interrupt!") 421 | self.shutdown() 422 | sys.exit(0) 423 | 424 | signal.signal(signal.SIGINT, signal_handler) 425 | 426 | def shutdown(self): 427 | # Ensure motors and sensors are properly stopped 428 | self.motor.set_should_terminate(True) 429 | self.sensors.stop() 430 | self.data_receiver.stop() 431 | print("BipedController has been shut down cleanly.") 432 | 433 | def sigmoid_k(x: np.ndarray, k: float) -> np.ndarray: 434 | return 1 / (1 + np.exp(-k*x)) 435 | 436 | # DEBUGGING 437 | if __name__ == "__main__": 438 | controller = BipedController() 439 | controller.initialize_stance() 440 | np.set_printoptions(formatter={"float": "{: 3.2f}".format}) 441 | t = time.time() 442 | while True: 443 | controller.step() 444 | -------------------------------------------------------------------------------- /checkpoint/baseline/config.yaml: -------------------------------------------------------------------------------- 1 | capture_video: false 2 | capture_video_freq: 1464 3 | capture_video_len: 100 4 | checkpoint: /home/grl/repo/RobotsMakingRobots/legged_env/outputs/Biped/train/bd5_00_01_02/runs/BipedAsymm_22-17-32-51/nn/BipedAsymm.pth 5 | experiment: '' 6 | force_render: true 7 | graphics_device_id: 0 8 | headless: false 9 | max_iterations: '' 10 | multi_gpu: false 11 | num_envs: 1 12 | num_subscenes: 4 13 | num_threads: 10 14 | pbt: 15 | enabled: false 16 | physics_engine: physx 17 | pipeline: gpu 18 | rl_device: cuda:0 19 | seed: 42 20 | sigma: '' 21 | sim_device: cuda:0 22 | solver_type: 1 23 | task: 24 | env: 25 | assetDofProperties: 26 | armature: [0.21, 0.42, 0.38, 0.21, 0.075, 0.21, 0.42, 0.38, 0.21, 0.075] 27 | damping: 0 28 | friction: [0.01, 0.01, 0.01, 0.04, 0.04, 0.01, 0.01, 0.01, 0.04, 0.04] 29 | velocity: 10 30 | asymmetric_observations: true 31 | baseHeightOffset: 0.08 32 | baseHeightTarget: null 33 | baseHeightTargetOffset: 0 34 | baseInitState: 35 | pos: [0.0, 0.0, 0.4] 36 | rot: [0.0, 0.0, 0.0, 1.0] 37 | vAngular: [0.0, 0.0, 0.0] 38 | vLinear: [0.0, 0.0, 0.0] 39 | commandZeroProbability: 0.1 40 | commandZeroThreshold: 0.05 41 | control: 42 | actionScale: 1.0 43 | damping: [8, 8, 8, 8, 5, 8, 8, 8, 8, 5] 44 | decimation: 4 45 | limit: 60 46 | stiffness: [80, 80, 80, 80, 60, 80, 80, 80, 80, 60] 47 | dataPublisher: 48 | enable: true 49 | target_url: udp://localhost:9870 50 | data_root_label: sim 51 | defaultJointPositions: [0.0, 0.175, 0.1, 0.387, -0.213, 0.0, -0.175, -0.1, -0.387, 52 | 0.213] 53 | enableCameraSensors: false 54 | enableDebugVis: false 55 | envSpacing: 1.0 56 | heightmap: 57 | x: '${linspace: -0.6, 0.6, 13}' 58 | y: '${linspace: -0.4, 0.4, 9}' 59 | keep_still_at_zero_command: false 60 | learn: 61 | action_is_on_sigmoid_k: 10 62 | addNoise: false 63 | allowKneeContacts: false 64 | angularVelocityNoise: 0.2 65 | angularVelocityScale: 0.25 66 | dofLimitMargins: 0 67 | dofPositionNoise: 0.01 68 | dofPositionScale: 1.0 69 | dofVelocityNoise: 1.5 70 | dofVelocityScale: 0.05 71 | enablePassiveDynamics: false 72 | episodeLength_s: 999 73 | foot_contact_threshold: 20 74 | gravityNoise: 0.05 75 | guided_contact: 76 | enable: true 77 | phase_freq: 1.25 78 | phase_offset: [0, 0.5] 79 | phase_stance_ratio: 0.6 80 | phase_start_with_swing: true 81 | heightMapNoise: 0.06 82 | heightMapScale: 5.0 83 | linearVelocityNoise: 0.1 84 | linearVelocityScale: 2.0 85 | noiseLevel: 1.0 86 | passiveCurriculum: true 87 | reward: 88 | action: 89 | fcn: abs 90 | scale: 0 91 | action_rate: 92 | exp_scale: -0.001 93 | fcn: square_sum 94 | scale: 0 95 | air_time: 96 | fcn: duration_since_condition_reward 97 | offset: -0.3 98 | scale: 1 99 | ang_vel: 100 | exp_scale: -8.0 101 | fcn: exp_weighted_square_sum 102 | normalize_by: [0.1, 0.1, 1] 103 | scale: 0.5 104 | base_height: 105 | exp_scale: -2000 106 | fcn: null 107 | scale: 0.1 108 | collision: 109 | fcn: sum 110 | scale: 0 111 | contact_force: 112 | fcn: contact_force_reward 113 | offset: -100 114 | scale: 0 115 | dof_acc: 116 | exp_scale: -0.0001 117 | fcn: exp_square_sum 118 | scale: 0.1 119 | dof_force_target: 120 | exp_scale: -1 121 | fcn: exp_square_sum 122 | scale: 0.05 123 | dof_force_target_swing: 124 | exp_scale: -1.0 125 | fcn: exp_square_sum 126 | scale: 0 127 | dof_jerk: 128 | exp_scale: -1.0e-06 129 | fcn: exp_square_sum 130 | scale: 0 131 | dof_limit: 132 | fcn: out_of_bound_square_sum 133 | margin: 0.1 134 | scale: -100.0 135 | dof_pos: 136 | fcn: abs_sum 137 | scale: -0.05 138 | dof_pow: 139 | fcn: abs_sum 140 | scale: 0 141 | dof_vel: 142 | fcn: square_sum 143 | scale: 0 144 | foot_forward: 145 | exp_scale: -10 146 | fcn: foot_forward_fcn 147 | scale: 0.1 148 | foot_height: 149 | clamp_max: 0.05 150 | fcn: foot_height_reward 151 | scale: 1.0 152 | foot_orientation: 153 | exp_scale: -8 154 | fcn: exp_square_sum_mean 155 | scale: 0.1 156 | foot_pos: 157 | exp_scale: -1000 158 | normalize_by: [1, 1, 0] 159 | scale: 0.1 160 | impact: 161 | fcn: abs_sum 162 | scale: 0 163 | lin_vel: 164 | exp_scale: -4.0 165 | fcn: exp_weighted_square_sum 166 | normalize_by: [1, 1, 0.1] 167 | scale: 1.0 168 | orientation: 169 | fcn: square_sum 170 | scale: -20 171 | should_contact: 172 | fcn: sum 173 | scale: 0.5 174 | single_contact: 175 | fcn: duration_within_period_reward 176 | grace_period: 0.2 177 | max_single_contact: 1 178 | scale: 0 179 | slip: 180 | fcn: slip_reward 181 | scale: 0 182 | stance_time: 183 | fcn: duration_since_condition_reward 184 | offset: -0.5 185 | scale: 0 186 | stumble: 187 | fcn: sum 188 | scale: 0 189 | terminalReward: 0.0 190 | max_observation_delay_steps: 1 191 | numActions: infer 192 | numEnvs: ${resolve_default:4096,${...num_envs}} 193 | numObservations: infer 194 | num_stacked_obs_frame: 5 195 | num_stacked_state_frame: 3 196 | observationNames: [angularVelocity, projected_gravity_filtered, commands, dofPosition, 197 | dofVelocity, actions, contactTarget, phase] 198 | randomCommandVelocityRanges: 199 | linear_x: [0, 1] 200 | linear_y: [0, 0] 201 | yaw: [0, 0] 202 | randomize: 203 | action_delay: 204 | enable: true 205 | range: [0.1, 0.5] 206 | baseInertiaOrigin: 207 | enable: false 208 | range: [[-0.02, 0.02], [-0.02, 0.02], [-0.02, 0.02]] 209 | baseMass: 210 | enable: false 211 | range: [-0.5, 5.0] 212 | base_init_pos_xy: 213 | enable: true 214 | range: [-0.5, 0.5] 215 | body_force: 216 | decay_time_constant: 0.2 217 | enable: false 218 | prob_range: [0.001, 0.1] 219 | scale: 0.05 220 | default_dof_pos: 221 | enable: false 222 | range: [-0.02, 0.02] 223 | dof_strength: 224 | enable: false 225 | range: [0.95, 1.02] 226 | erfi: 227 | enable: false 228 | rao_range: [-2.0, 2.0] 229 | rfi_range: [-3.0, 3.0] 230 | friction: 231 | enable: false 232 | range: [0.2, 1.2] 233 | initDofPos: 234 | enable: false 235 | range: [-0.1, 0.1] 236 | initDofVel: 237 | enable: false 238 | range: [-0.1, 0.1] 239 | link_inertia: 240 | enable: false 241 | range: [0.95, 1.05] 242 | link_mass: 243 | enable: false 244 | range: [0.95, 1.05] 245 | projected_gravity_delay: 246 | enable: true 247 | range: [0.05, 0.5] 248 | push: 249 | enable: false 250 | interval_s: 8 251 | velMax: [0.2, 0.2, 0.2, 0.4, 0.4, 0.4] 252 | velMin: [-0.2, -0.2, -0.2, -0.4, -0.4, -0.4] 253 | renderFPS: 50 254 | stateNames: [linearVelocity, angularVelocity, projectedGravity, commands, dofPosition, 255 | dofVelocity, actions, contactTarget, contact, phase] 256 | terrain: 257 | borderSize: 25 258 | curriculum: true 259 | difficultySale: 0.2 260 | discrete: 261 | height: 0.15 262 | num_rects: 40 263 | size: [0.4, 2.0] 264 | dynamicFriction: 1.0 265 | horizontalScale: 0.1 266 | mapLength: 10.0 267 | mapWidth: 10.0 268 | maxInitMapLevel: 1 269 | numLevels: 2 270 | numTerrains: 10 271 | platformSize: 2.4 272 | restitution: 0.0 273 | slope: 0.4 274 | slopeTreshold: 0.2 275 | stair: 276 | height: 0.15 277 | width: 0.31 278 | staticFriction: 1.0 279 | terrainProportions: [1, 1, 0, 1, 1, 0, 0, 1, 0] 280 | terrainType: plane 281 | uniform: 282 | downsampled_scale: 0.2 283 | height: 0.15 284 | step: 0.005 285 | verticalScale: 0.001 286 | urdfAsset: 287 | AssetOptions: 288 | collapse_fixed_joints: true 289 | default_dof_drive_mode: 3 290 | fix_base_link: false 291 | flip_visual_attachments: false 292 | override_inertia: false 293 | replace_cylinder_with_capsule: true 294 | vhacd_enabled: true 295 | vhacd_params: 296 | max_convex_hulls: 3 297 | max_num_vertices_per_ch: 128 298 | collision_filter: 0 299 | file: urdf/v6biped_urdf_v4_aug29/v6biped_urdf_v4_squarefoot_aug29.urdf 300 | footName: foot 301 | viewer: 302 | follow: true 303 | keyboardOperator: true 304 | lookat: [1.0, 1, 9] 305 | pos: [0, 0, 10] 306 | refEnv: 0 307 | sync: true 308 | name: Biped 309 | physics_engine: physx 310 | sim: 311 | dt: 0.005 312 | gravity: [0.0, 0.0, -9.81] 313 | physx: 314 | bounce_threshold_velocity: 0.2 315 | contact_collection: 1 316 | contact_offset: 0.01 317 | default_buffer_size_multiplier: 5.0 318 | max_depenetration_velocity: 100.0 319 | max_gpu_contact_pairs: 8388608 320 | num_position_iterations: 4 321 | num_subscenes: ${....num_subscenes} 322 | num_threads: ${....num_threads} 323 | num_velocity_iterations: 1 324 | rest_offset: 0.0 325 | solver_type: ${....solver_type} 326 | use_gpu: ${contains:"cuda",${....sim_device}} 327 | substeps: 1 328 | up_axis: z 329 | use_gpu_pipeline: ${eq:${...pipeline},"gpu"} 330 | test: ${..test} 331 | task_name: ${task.name} 332 | test: export 333 | torch_deterministic: false 334 | train: 335 | params: 336 | algo: 337 | name: a2c_continuous 338 | config: 339 | bounds_loss_coef: 0.0 340 | central_value_config: 341 | clip_value: true 342 | kl_threshold: ${..kl_threshold} 343 | learning_rate: ${..learning_rate} 344 | lr_schedule: ${..lr_schedule} 345 | mini_epochs: ${..mini_epochs} 346 | minibatch_size: ${..minibatch_size} 347 | network: 348 | central_value: true 349 | mlp: 350 | activation: elu 351 | d2rl: false 352 | initializer: 353 | name: default 354 | regularizer: 355 | name: None 356 | units: [512, 256, 128] 357 | name: actor_critic 358 | normalize_input: true 359 | schedule_type: standard 360 | truncate_grads: ${..truncate_grads} 361 | clip_actions: false 362 | clip_value: true 363 | critic_coef: 2 364 | e_clip: 0.2 365 | entropy_coef: 0.001 366 | env_name: rlgpu 367 | full_experiment_name: ${.name} 368 | gamma: 0.99 369 | grad_norm: 1.0 370 | horizon_length: 32 371 | kl_threshold: 0.008 372 | learning_rate: 0.0003 373 | lr_schedule: adaptive 374 | max_epochs: 3000 375 | mini_epochs: 8 376 | minibatch_size: 16384 377 | mixed_precision: true 378 | name: ${resolve_default:BipedAsymm,${....experiment}} 379 | normalize_advantage: true 380 | normalize_input: true 381 | normalize_value: true 382 | num_actors: ${....task.env.numEnvs} 383 | player: 384 | games_num: 100000 385 | ppo: true 386 | print_stats: true 387 | reward_shaper: 388 | scale_value: 1.0 389 | save_best_after: 200 390 | save_frequency: 50 391 | score_to_win: 20000 392 | seq_length: 4 393 | tau: 0.95 394 | truncate_grads: true 395 | value_bootstrap: true 396 | load_checkpoint: ${if:${...checkpoint},True,False} 397 | load_path: ${...checkpoint} 398 | model: 399 | name: continuous_a2c_logstd 400 | network: 401 | mlp: 402 | activation: elu 403 | d2rl: false 404 | initializer: 405 | name: default 406 | regularizer: 407 | name: None 408 | units: [512, 256, 128] 409 | name: actor_critic 410 | separate: false 411 | space: 412 | continuous: 413 | fixed_sigma: true 414 | mu_activation: None 415 | mu_init: 416 | name: default 417 | sigma_activation: None 418 | sigma_init: 419 | name: const_initializer 420 | val: 0.0 421 | seed: ${...seed} 422 | wandb_activate: false 423 | wandb_entity: '' 424 | wandb_group: '' 425 | wandb_logcode_dir: '' 426 | wandb_name: ${train.params.config.name} 427 | wandb_project: isaacgymenvs 428 | wandb_tags: [] 429 | -------------------------------------------------------------------------------- /checkpoint/baseline/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/generalroboticslab/dukeHumanoidHardwareControl/3a9c3cd4c8daf6b3ffe2e41768360d0515f2df55/checkpoint/baseline/policy.onnx -------------------------------------------------------------------------------- /checkpoint/passive/config.yaml: -------------------------------------------------------------------------------- 1 | capture_video: false 2 | capture_video_freq: 1464 3 | capture_video_len: 100 4 | checkpoint: /home/grl/repo/RobotsMakingRobots/legged_env/outputs/Biped/train/bd5_00_00_passive/runs/BipedAsymm_16-17-28-30/nn/BipedAsymm.pth 5 | experiment: '' 6 | force_render: true 7 | graphics_device_id: 0 8 | headless: false 9 | max_iterations: '' 10 | multi_gpu: false 11 | num_envs: 1 12 | num_subscenes: 4 13 | num_threads: 10 14 | pbt: 15 | enabled: false 16 | physics_engine: physx 17 | pipeline: gpu 18 | rl_device: cuda:0 19 | seed: 42 20 | sigma: '' 21 | sim_device: cuda:0 22 | solver_type: 1 23 | task: 24 | env: 25 | assetDofProperties: 26 | armature: [0.21, 0.42, 0.38, 0.21, 0.075, 0.21, 0.42, 0.38, 0.21, 0.075] 27 | damping: 0 28 | friction: [0.01, 0.01, 0.01, 0.04, 0.04, 0.01, 0.01, 0.01, 0.04, 0.04] 29 | velocity: 10 30 | asymmetric_observations: true 31 | baseHeightOffset: 0.08 32 | baseHeightTarget: null 33 | baseHeightTargetOffset: 0 34 | baseInitState: 35 | pos: [0.0, 0.0, 0.4] 36 | rot: [0.0, 0.0, 0.0, 1.0] 37 | vAngular: [0.0, 0.0, 0.0] 38 | vLinear: [0.0, 0.0, 0.0] 39 | commandZeroProbability: 0.1 40 | commandZeroThreshold: 0.05 41 | control: 42 | actionScale: 1.0 43 | damping: [8, 8, 8, 8, 5, 8, 8, 8, 8, 5] 44 | decimation: 4 45 | limit: 60 46 | stiffness: [80, 80, 80, 80, 60, 80, 80, 80, 80, 60] 47 | dataPublisher: 48 | enable: true 49 | target_url: udp://localhost:9870 50 | data_root_label: sim 51 | defaultJointPositions: [0.0, 0.175, 0.1, 0.387, -0.213, 0.0, -0.175, -0.1, -0.387, 52 | 0.213] 53 | enableCameraSensors: false 54 | enableDebugVis: false 55 | envSpacing: 1.0 56 | heightmap: 57 | x: '${linspace: -0.6, 0.6, 13}' 58 | y: '${linspace: -0.4, 0.4, 9}' 59 | keep_still_at_zero_command: false 60 | learn: 61 | action_is_on_min: 0 62 | action_is_on_sigmoid_k: 10 63 | addNoise: false 64 | allowKneeContacts: false 65 | angularVelocityNoise: 0.2 66 | angularVelocityScale: 0.25 67 | dofLimitMargins: 0 68 | dofPositionNoise: 0.01 69 | dofPositionScale: 1.0 70 | dofVelocityNoise: 1.5 71 | dofVelocityScale: 0.05 72 | enablePassiveDynamics: true 73 | episodeLength_s: 999 74 | foot_contact_threshold: 20 75 | gravityNoise: 0.05 76 | guided_contact: 77 | enable: true 78 | phase_freq: 1.25 79 | phase_offset: [0, 0.5] 80 | phase_stance_ratio: 0.6 81 | phase_start_with_swing: true 82 | heightMapNoise: 0.06 83 | heightMapScale: 5.0 84 | linearVelocityNoise: 0.1 85 | linearVelocityScale: 2.0 86 | noiseLevel: 1.0 87 | passiveCurriculum: true 88 | reward: 89 | action: 90 | fcn: abs 91 | scale: 0 92 | action_rate: 93 | exp_scale: -0.001 94 | fcn: square_sum 95 | scale: 0 96 | air_time: 97 | fcn: duration_since_condition_reward 98 | offset: -0.3 99 | scale: 1 100 | ang_vel: 101 | exp_scale: -8.0 102 | fcn: exp_weighted_square_sum 103 | normalize_by: [0.1, 0.1, 1] 104 | scale: 0.5 105 | base_height: 106 | exp_scale: -2000 107 | fcn: null 108 | scale: 0.1 109 | collision: 110 | fcn: sum 111 | scale: 0 112 | contact_force: 113 | fcn: contact_force_reward 114 | offset: -100 115 | scale: 0 116 | dof_acc: 117 | exp_scale: -0.0001 118 | fcn: exp_square_sum 119 | scale: 0.1 120 | dof_force_target: 121 | exp_scale: -1 122 | fcn: exp_square_sum 123 | scale: 0.05 124 | dof_force_target_swing: 125 | exp_scale: -1.0 126 | fcn: exp_square_sum 127 | scale: 0 128 | dof_jerk: 129 | exp_scale: -1.0e-06 130 | fcn: exp_square_sum 131 | scale: 0 132 | dof_limit: 133 | fcn: out_of_bound_square_sum 134 | margin: 0.1 135 | scale: -100.0 136 | dof_pos: 137 | fcn: abs_sum 138 | scale: -0.05 139 | dof_pow: 140 | fcn: abs_sum 141 | scale: 0 142 | dof_vel: 143 | fcn: square_sum 144 | scale: 0 145 | foot_forward: 146 | exp_scale: -10 147 | fcn: foot_forward_fcn 148 | scale: 0.1 149 | foot_height: 150 | clamp_max: 0.05 151 | fcn: foot_height_reward 152 | scale: 1.0 153 | foot_orientation: 154 | exp_scale: -8 155 | fcn: exp_square_sum_mean 156 | scale: 0.1 157 | foot_pos: 158 | exp_scale: -1000 159 | normalize_by: [1, 1, 0] 160 | scale: 0.1 161 | impact: 162 | fcn: abs_sum 163 | scale: 0 164 | lin_vel: 165 | exp_scale: -4.0 166 | fcn: exp_weighted_square_sum 167 | normalize_by: [1, 1, 0.1] 168 | scale: 1.0 169 | orientation: 170 | fcn: square_sum 171 | scale: -20 172 | passive_action: 173 | scale: 0.005 174 | passive_action_rate: 175 | scale: 0 176 | should_contact: 177 | fcn: sum 178 | scale: 0.5 179 | single_contact: 180 | fcn: duration_within_period_reward 181 | grace_period: 0.2 182 | max_single_contact: 1 183 | scale: 0 184 | slip: 185 | fcn: slip_reward 186 | scale: 0 187 | stance_time: 188 | fcn: duration_since_condition_reward 189 | offset: -0.5 190 | scale: 0 191 | stumble: 192 | fcn: sum 193 | scale: 0 194 | terminalReward: 0.0 195 | max_observation_delay_steps: 1 196 | numActions: infer 197 | numEnvs: ${resolve_default:4096,${...num_envs}} 198 | numObservations: infer 199 | num_stacked_obs_frame: 5 200 | num_stacked_state_frame: 3 201 | observationNames: [angularVelocity, projected_gravity_filtered, commands, dofPosition, dofVelocity, 202 | actions, contactTarget, phase] 203 | randomCommandVelocityRanges: 204 | linear_x: [0, 1] 205 | linear_y: [0, 0] 206 | yaw: [0, 0] 207 | randomize: 208 | action_delay: 209 | enable: true 210 | range: [0.1, 0.5] 211 | baseInertiaOrigin: 212 | enable: false 213 | range: [[-0.02, 0.02], [-0.02, 0.02], [-0.02, 0.02]] 214 | baseMass: 215 | enable: false 216 | range: [-0.5, 5.0] 217 | body_force: 218 | decay_time_constant: 0.2 219 | enable: false 220 | prob_range: [0.001, 0.1] 221 | scale: 0.05 222 | default_dof_pos: 223 | enable: false 224 | range: [-0.02, 0.02] 225 | dof_strength: 226 | enable: false 227 | range: [0.95, 1.02] 228 | erfi: 229 | enable: false 230 | rao_range: [-2.0, 2.0] 231 | rfi_range: [-3.0, 3.0] 232 | friction: 233 | enable: false 234 | range: [0.2, 1.2] 235 | initDofPos: 236 | enable: false 237 | range: [-0.1, 0.1] 238 | initDofVel: 239 | enable: false 240 | range: [-0.1, 0.1] 241 | link_inertia: 242 | enable: false 243 | range: [0.95, 1.05] 244 | link_mass: 245 | enable: false 246 | range: [0.95, 1.05] 247 | projected_gravity_delay: 248 | enable: true 249 | range: [0.1, 0.8] 250 | push: 251 | enable: false 252 | interval_s: 8 253 | velMax: [0.2, 0.2, 0.2, 0.4, 0.4, 0.4] 254 | velMin: [-0.2, -0.2, -0.2, -0.4, -0.4, -0.4] 255 | renderFPS: 50 256 | stateNames: [linearVelocity, angularVelocity, projectedGravity, commands, dofPosition, 257 | dofVelocity, actions, contactTarget, contact, phase] 258 | terrain: 259 | borderSize: 25 260 | curriculum: true 261 | difficultySale: 0.2 262 | discrete: 263 | height: 0.15 264 | num_rects: 40 265 | size: [0.4, 2.0] 266 | dynamicFriction: 1.0 267 | horizontalScale: 0.1 268 | mapLength: 10.0 269 | mapWidth: 10.0 270 | maxInitMapLevel: 1 271 | numLevels: 2 272 | numTerrains: 10 273 | platformSize: 2.4 274 | restitution: 0.0 275 | slope: 0.4 276 | slopeTreshold: 0.2 277 | stair: 278 | height: 0.15 279 | width: 0.31 280 | staticFriction: 1.0 281 | terrainProportions: [1, 1, 0, 1, 1, 0, 0, 1, 0] 282 | terrainType: plane 283 | uniform: 284 | downsampled_scale: 0.2 285 | height: 0.15 286 | step: 0.005 287 | verticalScale: 0.001 288 | urdfAsset: 289 | AssetOptions: 290 | collapse_fixed_joints: true 291 | default_dof_drive_mode: 3 292 | fix_base_link: false 293 | flip_visual_attachments: false 294 | override_inertia: false 295 | replace_cylinder_with_capsule: true 296 | vhacd_enabled: true 297 | vhacd_params: 298 | max_convex_hulls: 3 299 | max_num_vertices_per_ch: 128 300 | collision_filter: 0 301 | file: urdf/v6biped_urdf_v4_aug29/v6biped_urdf_v4_squarefoot_aug29.urdf 302 | footName: foot 303 | viewer: 304 | follow: true 305 | keyboardOperator: true 306 | lookat: [1.0, 1, 9] 307 | pos: [0, 0, 10] 308 | refEnv: 0 309 | sync: true 310 | name: Biped 311 | physics_engine: physx 312 | sim: 313 | dt: 0.005 314 | gravity: [0.0, 0.0, -9.81] 315 | physx: 316 | bounce_threshold_velocity: 0.2 317 | contact_collection: 1 318 | contact_offset: 0.01 319 | default_buffer_size_multiplier: 5.0 320 | max_depenetration_velocity: 100.0 321 | max_gpu_contact_pairs: 8388608 322 | num_position_iterations: 4 323 | num_subscenes: ${....num_subscenes} 324 | num_threads: ${....num_threads} 325 | num_velocity_iterations: 1 326 | rest_offset: 0.0 327 | solver_type: ${....solver_type} 328 | use_gpu: ${contains:"cuda",${....sim_device}} 329 | substeps: 1 330 | up_axis: z 331 | use_gpu_pipeline: ${eq:${...pipeline},"gpu"} 332 | test: ${..test} 333 | task_name: ${task.name} 334 | test: export 335 | torch_deterministic: false 336 | train: 337 | params: 338 | algo: 339 | name: a2c_continuous 340 | config: 341 | bounds_loss_coef: 0.0 342 | central_value_config: 343 | clip_value: true 344 | kl_threshold: ${..kl_threshold} 345 | learning_rate: ${..learning_rate} 346 | lr_schedule: ${..lr_schedule} 347 | mini_epochs: ${..mini_epochs} 348 | minibatch_size: ${..minibatch_size} 349 | network: 350 | central_value: true 351 | mlp: 352 | activation: elu 353 | d2rl: false 354 | initializer: 355 | name: default 356 | regularizer: 357 | name: None 358 | units: [512, 256, 128] 359 | name: actor_critic 360 | normalize_input: true 361 | schedule_type: standard 362 | truncate_grads: ${..truncate_grads} 363 | clip_actions: false 364 | clip_value: true 365 | critic_coef: 2 366 | e_clip: 0.2 367 | entropy_coef: 0.001 368 | env_name: rlgpu 369 | full_experiment_name: ${.name} 370 | gamma: 0.99 371 | grad_norm: 1.0 372 | horizon_length: 32 373 | kl_threshold: 0.008 374 | learning_rate: 0.0003 375 | lr_schedule: adaptive 376 | max_epochs: 3000 377 | mini_epochs: 8 378 | minibatch_size: 16384 379 | mixed_precision: true 380 | name: ${resolve_default:BipedAsymm,${....experiment}} 381 | normalize_advantage: true 382 | normalize_input: true 383 | normalize_value: true 384 | num_actors: ${....task.env.numEnvs} 385 | player: 386 | games_num: 100000 387 | ppo: true 388 | print_stats: true 389 | reward_shaper: 390 | scale_value: 1.0 391 | save_best_after: 200 392 | save_frequency: 50 393 | score_to_win: 20000 394 | seq_length: 4 395 | tau: 0.95 396 | truncate_grads: true 397 | value_bootstrap: true 398 | load_checkpoint: ${if:${...checkpoint},True,False} 399 | load_path: ${...checkpoint} 400 | model: 401 | name: continuous_a2c_logstd 402 | network: 403 | mlp: 404 | activation: elu 405 | d2rl: false 406 | initializer: 407 | name: default 408 | regularizer: 409 | name: None 410 | units: [512, 256, 128] 411 | name: actor_critic 412 | separate: false 413 | space: 414 | continuous: 415 | fixed_sigma: true 416 | mu_activation: None 417 | mu_init: 418 | name: default 419 | sigma_activation: None 420 | sigma_init: 421 | name: const_initializer 422 | val: 0.0 423 | seed: ${...seed} 424 | wandb_activate: false 425 | wandb_entity: '' 426 | wandb_group: '' 427 | wandb_logcode_dir: '' 428 | wandb_name: ${train.params.config.name} 429 | wandb_project: isaacgymenvs 430 | wandb_tags: [] 431 | -------------------------------------------------------------------------------- /checkpoint/passive/policy.onnx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/generalroboticslab/dukeHumanoidHardwareControl/3a9c3cd4c8daf6b3ffe2e41768360d0515f2df55/checkpoint/passive/policy.onnx -------------------------------------------------------------------------------- /doc/image/dukehumanoidv1-thumbnails.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/generalroboticslab/dukeHumanoidHardwareControl/3a9c3cd4c8daf6b3ffe2e41768360d0515f2df55/doc/image/dukehumanoidv1-thumbnails.gif -------------------------------------------------------------------------------- /doc/image/dukehumanoidv1-thumbnails_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/generalroboticslab/dukeHumanoidHardwareControl/3a9c3cd4c8daf6b3ffe2e41768360d0515f2df55/doc/image/dukehumanoidv1-thumbnails_1.gif -------------------------------------------------------------------------------- /gamepad.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from publisher import DataPublisher 3 | import pygame 4 | 5 | 6 | lin_vel_x_max = 0.3 7 | lin_vel_y_max = 0.25 8 | ang_vel_z_max = 0.2 9 | 10 | 11 | keyboard_operator_cmd = np.zeros(3) # vel_x, vel_y, yaw_orientation 12 | reset = False 13 | 14 | data_publisher = DataPublisher( 15 | 'udp://localhost:9871', encoding="msgpack", broadcast=True) 16 | 17 | pygame.init() 18 | pygame.joystick.init() 19 | 20 | joysticks = [pygame.joystick.Joystick(x) for x in range(pygame.joystick.get_count())] 21 | for joystick in joysticks: 22 | joystick.init() 23 | print(joystick.get_name()) 24 | 25 | running = True 26 | while running: 27 | 28 | updated = False 29 | 30 | 31 | for event in pygame.event.get(): 32 | # print(event) 33 | if event.type == pygame.QUIT: 34 | running = False 35 | elif event.type == pygame.JOYBUTTONDOWN: 36 | # print("Button pressed:", event.button) 37 | if event.button == 4: 38 | updated = True 39 | reset = True 40 | # print("reset") 41 | elif event.type == pygame.JOYAXISMOTION: 42 | # print("Axis moved:", event.axis, event.value) 43 | if event.axis==1: 44 | keyboard_operator_cmd[0] = np.round(-event.value * lin_vel_x_max,decimals=2) 45 | updated = True 46 | elif event.axis==0: 47 | keyboard_operator_cmd[2] = np.round(-event.value * ang_vel_z_max,decimals=2) 48 | updated = True 49 | elif event.axis==3: 50 | keyboard_operator_cmd[1] = np.round(-event.value * lin_vel_y_max,decimals=2) 51 | updated = True 52 | 53 | # print(gamepad_states) 54 | if updated: 55 | 56 | data = { 57 | "cmd": keyboard_operator_cmd, 58 | "reset": reset, 59 | } 60 | print(data) 61 | 62 | data_publisher.publish(data) 63 | reset=False 64 | 65 | pygame.quit() -------------------------------------------------------------------------------- /motor_controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import signal 3 | import time 4 | import publisher 5 | 6 | import torch 7 | import yaml 8 | 9 | from numpy_ringbuffer import filterBuffer 10 | 11 | from build import ethercat_motor_py as MOTOR 12 | 13 | pi = np.pi 14 | 15 | CONTROL_MODE = MOTOR.CONTROL_MODE 16 | 17 | 18 | class MotorController(MOTOR.PyMotor): 19 | 20 | def __init__( 21 | self, 22 | interface_name = "enp3s0", 23 | control_mode_int8 = None, 24 | max_velocity = None, 25 | max_torque=None, 26 | ): 27 | super().__init__(interface_name, control_mode_int8, max_velocity, max_torque) 28 | self.should_print = False 29 | self.debug=False 30 | 31 | self.running = False 32 | 33 | self.rated_current = np.asarray(self.rated_torque) 34 | 35 | self.num_dof = 10 36 | # motor joint limits 37 | # [l_hip1,l_hip2,l_hip3,l_knee,l_ankle,r_hip1,r_hip2,r_hip3,r_knee,r_ankle] 38 | # self.min_limit = np.array([-pi/3, -pi/6, -pi/9,-pi/24, -pi/5, -pi/4, -pi/2, -pi/5, -pi/2, -pi/5]) 39 | # self.max_limit = np.array([pi/4, pi/2, pi/5, pi/2, pi/5, pi/3, pi/6, pi/9, pi/24, pi/5]) 40 | min_limit = np.array([ -pi/4, -pi/2.5, -pi/8, -0.3 , -pi*38/180, -pi/4, -pi/2.5, -pi/5, -pi/3, -pi*40/180]) 41 | max_limit = np.array([ pi/4, pi/2.5, pi/5, pi/3 , pi*40/180, pi/4, pi/2.5, pi/8, 0.3 , pi*38/180]) 42 | # hard_min_limit = np.array([-pi/2, -pi/2, -pi*2/9, -0.02 , -pi*36/180, -pi/2, -pi/2, -pi*2/9, -pi*5/9, -pi*40/180]) 43 | # hard_max_limit = np.array([ pi/2, pi/2, pi*2/9, pi*5/9, pi*40/180, pi/2, pi/2, pi*2/9, 0.02 , pi*36/180]) 44 | # soft_limit_offset = min_limit - hard_min_limit 45 | # def arry2string(arr): 46 | # return np.array2string(arr, separator=',',max_line_width=999,precision=5).replace(" ", "") 47 | # print(f"++task.env.assetDofProperties.lower={arry2string(hard_min_limit)}") 48 | # print(f"++task.env.assetDofProperties.upper={arry2string(hard_max_limit)}") 49 | # print(f"++task.env.dof_soft_limit.lower={arry2string(min_limit)}") 50 | # print(f"++task.env.dof_soft_limit.upper={arry2string(max_limit)}") 51 | 52 | self.min_limit = min_limit 53 | self.max_limit = max_limit 54 | 55 | gear_ratio = [18, 20, 18, 18, 10, 18, 20, 18, 18, 10] 56 | # 0 ,1 ,2 ,3 ,4 ,5 ,6 ,7 ,8 ,9 57 | rotor_current_to_torque_ratio = [0.165,0.165,0.165,0.165,0.165,0.165,0.165,0.165,0.165,0.165] #default 58 | # defined in c++ 59 | self.gear_ratio = np.array(gear_ratio,dtype=np.float64) 60 | self.rotor_current_to_torque_ratio=np.array(rotor_current_to_torque_ratio, dtype=np.float64) 61 | 62 | self.last_limit_check_time= time.time() 63 | 64 | fix_yaml_sequence() 65 | 66 | # self.motor_pos_offset = np.zeros(self.dof) 67 | with open("motor_config.yaml", "r") as file: 68 | config = yaml.safe_load(file) 69 | self.motor_pos_offset = np.array(config["motor_pos_offset"]) 70 | self.set_position_offset(self.motor_pos_offset) 71 | 72 | print("motor_pos_offset:",repr(self.motor_pos_offset)) 73 | # self.motor_pos_offset = np.array([0.06983874, 0.07876512, 0.30102242, 0.1620134 , 0.45463356,0.32062063, 0.21344384, 0.33541981, 0.22198779, 0.12190354]) 74 | 75 | def signal_handler(sig, frame): 76 | print("Keyboard Interrupt detected. Attempting to stop background thread...") 77 | self.set_should_terminate(True) 78 | exit() 79 | 80 | signal.signal(signal.SIGINT, signal_handler) 81 | 82 | def limit_check(self, dof_pos_target): 83 | over_max_limit = (self.dof_pos > self.max_limit) & (dof_pos_target > self.max_limit) 84 | under_min_limit = (self.dof_pos < self.min_limit) & (dof_pos_target0.5: # only print every 0.5 s 88 | print(f"\033[93mLimit check: index {np.flatnonzero(under_min_limit)} under min limit, index {np.flatnonzero(over_max_limit)} over max limit!\033[0m") 89 | self.last_limit_check_time = t 90 | result = np.where(over_max_limit, self.max_limit, dof_pos_target) 91 | result = np.where(under_min_limit, self.min_limit, result) 92 | return result 93 | else: 94 | return dof_pos_target 95 | 96 | @property 97 | def dof_pos(self) -> np.ndarray: 98 | """returns current dof positions""" 99 | # return self._motor_value_to_dof_value(np.asarray(self.actual_position)-self.motor_pos_offset) 100 | return self.dof_position 101 | 102 | @property 103 | def motor_pos_after_offset(self) -> np.ndarray: 104 | """returns current dof positions""" 105 | return np.asarray(self.actual_position)-self.motor_pos_offset 106 | 107 | @property 108 | def _dof_vel_raw(self) -> np.ndarray: 109 | """returns current dof joint velocities""" 110 | return self._motor_value_to_dof_value(np.asarray(self.actual_velocity)) 111 | 112 | @property 113 | def dof_vel(self) -> np.ndarray: 114 | """returns current dof joint velocities""" 115 | # return self.dof_velocity 116 | return self.dof_velocity_filtered 117 | 118 | @property 119 | def dof_current(self) -> np.ndarray: 120 | return np.asarray(self.actual_torque) 121 | 122 | @property 123 | def dof_force(self): 124 | return self.actual_torque*self.rotor_current_to_torque_ratio 125 | # return np.asarray(self.target_dof_torque_Nm) 126 | 127 | def run(self): 128 | if not self.running: 129 | self.running = True 130 | super().run() 131 | time.sleep(0.05) 132 | 133 | def limit_dof_pos(self, unsafe_joint_pos): 134 | safe_joint_pos = np.clip(unsafe_joint_pos) 135 | return 136 | 137 | def dof_pos_to_motor_pos_safe(self, joint_pos): 138 | motor_pos = np.clip(self._dof_value_to_motor_value(joint_pos), self.min_limit, self.max_limit) + self.motor_pos_offset 139 | return motor_pos 140 | 141 | def set_motor_pos_from_dof_pos_safe(self, joint_pos): 142 | motor_pos = np.clip(self._dof_value_to_motor_value(joint_pos), self.min_limit, self.max_limit) + self.motor_pos_offset 143 | self.set_target_input(motor_pos) 144 | 145 | def set_motor_pos(self, motor_pos): 146 | motor_pos = np.clip(motor_pos, self.min_limit, self.max_limit) + self.motor_pos_offset 147 | self.set_target_input(motor_pos) 148 | 149 | def set_motor_pos_without_offset(self, motor_pos): 150 | """directly set motor positions, not limited by joint limits, no offset""" 151 | self.set_target_input(np.asarray(motor_pos)) 152 | 153 | def set_motor_pos_offset(self, motor_pos_offset): 154 | """set motor position offset""" 155 | self.motor_pos_offset = np.asarray(motor_pos_offset) 156 | 157 | def initialize_motor_pos_offset(self): 158 | self.control_mode_int8 = CONTROL_MODE.CYCLIC_SYNC_VELOCITY 159 | self.should_print = False # disable printing 160 | self.run() 161 | time.sleep(0.2) 162 | motor_actual_pos=np.asarray(self.actual_position).copy() 163 | desire_motor_pos = np.array([0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0]) 164 | # motor_actual_pos - offset = desire_motor_pos 165 | offset = - desire_motor_pos + motor_actual_pos 166 | with open("motor_config.yaml", "w") as file: 167 | yaml.dump({"motor_pos_offset": offset}, file, width=float("inf")) 168 | print(np.array2string(offset, separator=', ',precision=5,max_line_width=200)) 169 | 170 | # return joint position given actual motor position 171 | def _motor_value_to_dof_value(self, motor_value): 172 | """convert motor value to joint value, value can be position/velocity""" 173 | dof_value = np.copy(motor_value) 174 | dof_value[4] -= motor_value[3] 175 | dof_value[9] -= motor_value[8] 176 | return dof_value 177 | 178 | def _dof_value_to_motor_value(self, joint_value): 179 | """convert joint value to motor value, value can be position/velocity""" 180 | motor_value = np.copy(joint_value) 181 | motor_value[4] += motor_value[3] # left ankle 182 | motor_value[9] += motor_value[8] # right ankle 183 | return motor_value 184 | 185 | # get motor position given joint positions 186 | def _dof_pos_to_motor_pos_batch(self, joint_positions): 187 | motor_positions = np.copy(joint_positions) 188 | motor_positions[:, 4] += motor_positions[:, 3] # left ankle 189 | motor_positions[:, 9] += motor_positions[:, 8] # right ankle 190 | return motor_positions 191 | 192 | def fix_yaml_sequence(precision=5): 193 | """Fix yaml sequence dumper to have numerical sequence in one line""" 194 | 195 | def represent_array_sequence(dumper, data, precision=precision): 196 | if isinstance(data, torch.Tensor): 197 | data = data.cpu().numpy().astype(np.float64).round(precision).tolist() 198 | elif isinstance(data, np.ndarray): 199 | data = data.round(precision).tolist() 200 | return dumper.represent_sequence('tag:yaml.org,2002:seq', data, flow_style=True) 201 | 202 | # Register representers for lists, tuples, numpy arrays, and torch tensors 203 | for type in (list, tuple, np.ndarray, torch.Tensor): 204 | yaml.add_representer(type, represent_array_sequence) 205 | 206 | # py38 && export LD_LIBRARY_PATH=${CONDA_PREFIX}/lib && sudo $(which python) motor.py 207 | if __name__ == "__main__": 208 | motor = MotorController("enp3s0", CONTROL_MODE.CYCLIC_SYNC_VELOCITY, 1, 1000) 209 | motor.initialize_motor_pos_offset() 210 | -------------------------------------------------------------------------------- /numpy_ringbuffer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from collections.abc import Iterable 3 | from typing import Union 4 | from scipy import signal 5 | 6 | class _BaseRingBuffer: 7 | def __init__(self, buffer_len: int, shape: Union[Iterable,int], dtype=np.float32): 8 | """ 9 | Base class for circular numpy buffers. 10 | 11 | Args: 12 | buffer_len: Maximum number of arrays the buffer can hold. 13 | shape: Shape of the arrays to be stored (e.g., (3, 4), (6,)). 14 | dtype: The data type of the arrays in the buffer (default: np.float32). 15 | """ 16 | self.buffer_len = buffer_len 17 | if not isinstance(shape, Iterable): 18 | shape = (shape,) 19 | # self.storage acts as a circular buffer, the element at self._step is the newest element 20 | self.storage = np.zeros((buffer_len, *shape), dtype=dtype) 21 | self.step = -1 # current step 22 | self.max_step = self.buffer_len - 1 23 | 24 | def add(self, array: np.ndarray): 25 | """ 26 | Appends an array to the buffer, wrapping around if full. 27 | 28 | Args: 29 | array: The array to add to the buffer. Must have the correct shape. 30 | """ 31 | self.step += 1 32 | self.storage[self.step % self.buffer_len] = array 33 | 34 | 35 | def __getitem__(self, index: Union[int, slice, Iterable]) -> np.ndarray: 36 | """ 37 | Retrieves an array from the buffer by index, newest to oldest. 38 | For example: 39 | index=0 returns the latest array added to the buffer. 40 | index=-1 returns the oldest array added to the buffer. 41 | index=slice(3) returns the last three arrays added to the buffer, newest to oldest. 42 | 43 | Args: 44 | index: The index of the array to retrieve. 45 | 46 | Returns: 47 | The array at the specified index. 48 | """ 49 | current_step = self.step 50 | if isinstance(index, slice): 51 | start, stop, step = index.indices(self.buffer_len) # Concise slice handling 52 | indices = (current_step - np.arange(start, stop, step)) % self.buffer_len 53 | return self.storage[indices] 54 | elif isinstance(index, Iterable): 55 | indices = (current_step - np.array(index)) % self.buffer_len 56 | return self.storage[indices] 57 | else: 58 | return self.storage[(current_step - index) % self.buffer_len] 59 | 60 | def get_last(self): 61 | """Returns the last array in the buffer""" 62 | return self.storage[self.step % self.buffer_len] 63 | 64 | def get_last_n(self, n: int) -> np.ndarray: 65 | """Returns the last n arrays in the buffer, newest to oldest""" 66 | assert n <= self.buffer_len and n > 0 67 | # print(np.arange(self.step, self.step-n, -1) % self.buffer_len) 68 | return self.storage[np.arange(self.step, self.step-n, -1) % self.buffer_len] 69 | 70 | 71 | class RingArrayBuffer(_BaseRingBuffer): 72 | def __len__(self) -> int: 73 | """ 74 | Returns the current number of elements in the buffer. 75 | 76 | Returns: 77 | The number of elements in the buffer. 78 | """ 79 | return min(self.step, self.buffer_len) 80 | 81 | def full(self) -> bool: 82 | """ 83 | Checks if the buffer is full. 84 | 85 | Returns: 86 | True if the buffer is full, False otherwise. 87 | """ 88 | return self.step >= self.max_step 89 | 90 | def reset(self): 91 | """Resets the buffer current step to -1""" 92 | self.step = 0 93 | 94 | def clear(self): 95 | """Clears the contents of the buffer by filling it with zeros.""" 96 | self.storage.fill(0) 97 | self.step = 0 # Reset the step to indicate an empty buffer 98 | 99 | 100 | 101 | 102 | class filterBuffer: 103 | def __init__(self,shape,fs:float=200,filter_order:int=4,cut_off_frequency:float=20,dtype=np.float32): 104 | 105 | 106 | self.filter_order = filter_order 107 | self.buf_raw = RingArrayBuffer(buffer_len=filter_order+1, shape=shape,dtype=dtype) # x, input 108 | self.buf_filt = RingArrayBuffer(buffer_len=filter_order+1, shape=shape,dtype=dtype) # y, filtered 109 | 110 | self.b, self.a = signal.butter(N=filter_order, Wn=cut_off_frequency, btype='low', analog=False,fs=fs) 111 | self.b: np.ndarray = self.b.reshape(-1, *([1] * len(self.buf_raw.storage.shape[1:]))) 112 | self.a: np.ndarray = self.a.reshape(-1, *([1] * len(self.buf_raw.storage.shape[1:])))[1:filter_order+1] 113 | # self.b = self.b[:,np.newaxis] 114 | # self.a = self.a[1:filter_order+1,np.newaxis] 115 | 116 | def add(self,sample:np.ndarray): 117 | self.buf_raw.add(sample) 118 | filtered_sample = (self.b*self.buf_raw[:]).sum(axis=0) - \ 119 | (self.a*self.buf_filt[0:self.filter_order]).sum(axis=0) 120 | self.buf_filt.add(filtered_sample) 121 | return filtered_sample 122 | 123 | def get_latest_filtered(self): 124 | return self.buf_filt[0].copy() 125 | 126 | def get_latest_raw(self): 127 | return self.buf_raw[0].copy() 128 | 129 | def reset(self): 130 | self.buf_raw.reset() 131 | self.buf_filt.reset() 132 | 133 | 134 | #--- test 135 | 136 | def test_ring_array_buffer(): 137 | # Create a RingArrayBuffer with a buffer length of 5 and shape (2,) 138 | buffer = RingArrayBuffer(buffer_len=5, shape=(2,)) 139 | 140 | # Add arrays to the buffer 141 | buffer.add(np.array([1, 2])) 142 | buffer.add(np.array([3, 4])) 143 | buffer.add(np.array([5, 6])) 144 | buffer.add(np.array([7, 8])) 145 | buffer.add(np.array([9, 10])) 146 | 147 | # Check if buffer is full 148 | assert buffer.full() == True 149 | 150 | # Get the last added array 151 | assert np.array_equal(buffer.get_last(), np.array([9, 10])) 152 | 153 | # Get the last 3 arrays 154 | assert np.array_equal(buffer.get_last_n(3), np.array([[9, 10], [7, 8], [5, 6]])) 155 | 156 | # Add another array, should overwrite the oldest one 157 | buffer.add(np.array([11, 12])) 158 | 159 | # Get the last 3 arrays 160 | assert np.array_equal(buffer.get_last_n(3), np.array([[11, 12], [9, 10], [7, 8]])) 161 | 162 | # Retrieve individual arrays 163 | assert np.array_equal(buffer[0], np.array([11, 12])) 164 | assert np.array_equal(buffer[-1], np.array([3, 4])) 165 | 166 | # Slice retrieval 167 | assert np.array_equal(buffer[:3], np.array([[11, 12], [9, 10], [7, 8]])) 168 | 169 | # Reset the buffer 170 | buffer.reset() 171 | # print(buffer.get_last()) 172 | np.array_equal(buffer.get_last(),np.array([ 9., 10.])) 173 | 174 | # Clear the buffer 175 | buffer.clear() 176 | assert np.array_equal(buffer.get_last(), np.array([0, 0])) 177 | 178 | print("All tests passed!") 179 | 180 | if __name__ == "__main__": 181 | test_ring_array_buffer() -------------------------------------------------------------------------------- /publisher.py: -------------------------------------------------------------------------------- 1 | import socket 2 | import urllib.parse 3 | import orjson 4 | import msgpack 5 | import torch 6 | import numpy as np 7 | import select 8 | import threading 9 | import queue 10 | 11 | class DataPublisher: 12 | """ 13 | Manages data publishing to a specified target URL. 14 | 15 | Methods 16 | ------- 17 | publish(data: dict) 18 | Sends data to the target URL if publishing is enabled. 19 | """ 20 | 21 | SUPPORTED_SCHEMES = {'unix', 'tcp', 'udp'} 22 | 23 | def __init__( 24 | self, 25 | target_url: str = 'udp://localhost:9870', 26 | encoding: str = 'msgpack', 27 | broadcast: bool = False, 28 | enable: bool = True, 29 | thread: bool = True, 30 | **socket_kwargs, 31 | ): 32 | """ 33 | Initialize DataPublisher with connection and encoding details. 34 | 35 | Parameters 36 | ---------- 37 | target_url : str 38 | URL to which data will be sent. 39 | encoding : str 40 | Encoding for sending data, default is 'json'. 41 | broadcast : bool 42 | If True, data is broadcasted; defaults to True. 43 | enable : bool 44 | If False, publishing is inactive; defaults to False. 45 | thread : bool 46 | If True, publishing is done in a separate thread; defaults to True. 47 | socket_kwargs : 48 | Additional keyword arguments for the socket. 49 | """ 50 | self.enable = enable 51 | self.url = urllib.parse.urlparse(target_url) 52 | 53 | # Validate scheme 54 | if self.url.scheme not in self.SUPPORTED_SCHEMES: 55 | raise ValueError(f"Unsupported scheme in URL: {target_url}") 56 | 57 | # Set socket family and type 58 | family = self._get_socket_family() 59 | socket_type = socket.SOCK_DGRAM if 'udp' in self.url.scheme else socket.SOCK_STREAM 60 | 61 | # Create socket 62 | self.socket = socket.socket(family, socket_type, **socket_kwargs) 63 | if broadcast: 64 | self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) 65 | self.hostname = '' if broadcast else self.url.hostname 66 | 67 | # Set encoding function 68 | self._setup_encoding(encoding) 69 | 70 | if thread: 71 | # Create a queue for data and start the publishing thread 72 | self.data_queue = queue.Queue() 73 | self.stop_event = threading.Event() 74 | self.publisher_thread = threading.Thread(target=self._publisher_thread_func, daemon=True) 75 | self.publisher_thread.start() 76 | self.publish=self._publish_continuously 77 | self.__del__=self._stop 78 | else: 79 | self.publish=self._send_data 80 | 81 | def _get_socket_family(self): 82 | """Determine and return the appropriate socket family based on the URL.""" 83 | if 'unix' in self.url.scheme: 84 | return socket.AF_UNIX 85 | return socket.AF_INET6 if ':' in (self.url.hostname or '') else socket.AF_INET 86 | 87 | def _setup_encoding(self, encoding_type): 88 | """Configure the data encoding method based on the provided encoding_type.""" 89 | encodings = { 90 | 'raw': lambda data: data, # raw/bytes 91 | 'utf-8': lambda data: data.encode('utf-8'), 92 | 'msgpack': lambda data: msgpack.packb(data, use_single_float=False, use_bin_type=True), 93 | 'json': lambda data: orjson.dumps(data), 94 | } 95 | if encoding_type not in encodings: 96 | raise ValueError(f'Invalid encoding: {encoding_type}') 97 | self.encode = encodings[encoding_type] 98 | 99 | def _send_data(self, data: dict): 100 | if self.enable: 101 | converted_data = convert_to_python_builtin_types(data) 102 | encoded_data = self.encode(converted_data) 103 | try: 104 | self.socket.sendto(encoded_data, (self.hostname, self.url.port)) 105 | except Exception as e: 106 | print(f"Failed to send data: {e}") 107 | 108 | def _publish_continuously(self,data: dict): 109 | self.data_queue.put(data, block=False) 110 | 111 | def _publisher_thread_func(self): 112 | """Function run by the publisher thread to send data from the queue.""" 113 | while not self.stop_event.is_set(): 114 | try: 115 | data = self.data_queue.get(timeout=0.1) # Wait for data with timeout 116 | self._send_data(data) 117 | except queue.Empty: 118 | continue 119 | 120 | def _stop(self): 121 | """Stops the publishing thread.""" 122 | self.stop_event.set() 123 | self.publisher_thread.join() 124 | 125 | 126 | class DataReceiver: 127 | """ 128 | Receives data published by a DataPublisher instance using a non-blocking socket. 129 | """ 130 | 131 | def __init__( 132 | self, 133 | port: int = 9870, 134 | decoding: str = "msgpack", 135 | broadcast: bool = False, 136 | ): 137 | """ 138 | Initializes the DataReceiver. 139 | 140 | Args: 141 | target_port (int): The port to listen on for incoming data. Defaults to 9870. 142 | decoding (str): The decoding method for data (raw/utf-8/msgpack/json). Defaults to "msgpack". 143 | """ 144 | self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 145 | self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 146 | self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) 147 | ip_address = "" 148 | if broadcast: 149 | ip_address = '' 150 | self.socket.bind((ip_address, port)) 151 | self.socket.setblocking(False) 152 | decodings = { 153 | "raw": lambda data: data, # raw/bytes 154 | "utf-8": lambda data: data.decode("utf-8"), 155 | "msgpack": lambda data: msgpack.unpackb(data), 156 | "json": lambda data: orjson.loads(data), 157 | } 158 | if decoding not in decodings: 159 | raise ValueError(f"Invalid decoding: {decoding}") 160 | self.decode = decodings[decoding] 161 | self.data = None 162 | self.address = None 163 | self.data_id = -1 # received data id 164 | self._running = True # Flag to control the receive loop 165 | 166 | def receive(self, timeout=0.1, buffer_size=1024*1024*8): 167 | """Receive and decode data from the socket if available, otherwise return None.""" 168 | ready = select.select([self.socket], [], [], timeout) 169 | if ready[0]: 170 | data, self.address = self.socket.recvfrom(buffer_size) 171 | self.data = self.decode(data) 172 | self.data_id += 1 173 | # print(f"Received from {self.address}: {self.data}") 174 | return self.data, self.address # Return decoded data and sender address 175 | else: 176 | return None, None # No data received within the timeout 177 | 178 | def receive_continuously(self, timeout=0.1, buffer_size=1024*1024*8): 179 | """Continuously receive and decode data from the socket in a dedicated thread.""" 180 | 181 | def _receive_loop(): 182 | while self._running: 183 | self.receive(timeout=timeout, buffer_size=buffer_size) 184 | 185 | thread = threading.Thread(target=_receive_loop, daemon=True) 186 | thread.start() 187 | 188 | def stop(self): 189 | """Stop continuous receiving.""" 190 | self._running = False 191 | self.socket.close() 192 | 193 | 194 | def convert_to_python_builtin_types(nested_data: dict): 195 | """ 196 | Converts nested data (including tensors and arrays) to built-in types. 197 | 198 | Parameters 199 | ---------- 200 | nested_data : dict 201 | Data to be converted. 202 | 203 | Returns 204 | ------- 205 | dict 206 | Data converted to Python built-in types. 207 | """ 208 | converted_data = {} 209 | for key, value in nested_data.items(): 210 | if isinstance(value, dict): 211 | converted_data[key] = convert_to_python_builtin_types(value) 212 | elif isinstance(value, (torch.Tensor, np.ndarray)): 213 | converted_data[key] = value.tolist() 214 | else: 215 | converted_data[key] = value 216 | return converted_data 217 | 218 | 219 | if __name__ == "__main__": 220 | import time 221 | 222 | # Sample data to publish 223 | time_since_start = time.time() 224 | 225 | def test_data(i): 226 | """example test data""" 227 | return { 228 | "id": i, 229 | "sensor_id": np.random.randint(0, 10), 230 | "temperature": 25.5, 231 | "time": time.time()-time_since_start, 232 | } 233 | 234 | # Create a publisher instance 235 | publisher = DataPublisher( 236 | target_url="udp://localhost:9871", encoding="msgpack",broadcast=True,thread=True) 237 | 238 | # Create a receiver instance 239 | num_receivers = 2 240 | receivers = [] 241 | for i in range(num_receivers): 242 | receiver = DataReceiver(port=9871, decoding="msgpack",broadcast=True) 243 | # Start continuous receiving in a thread 244 | receiver.receive_continuously() 245 | receivers.append(receiver) 246 | 247 | # Send data multiple times with a delay 248 | for i in range(10): 249 | publisher.publish(test_data(i)) 250 | time.sleep(1e-3) # add a small delay 251 | for k,receiver in enumerate(receivers): 252 | print(f"receiver [{k}] {receiver.data_id}, {receiver.address}: {receiver.data}") 253 | 254 | # Stop continuous receiving after a while 255 | receiver.stop() 256 | 257 | print("Publisher and Receiver have stopped.") 258 | -------------------------------------------------------------------------------- /sensor_controller.py: -------------------------------------------------------------------------------- 1 | import serial # conda install -c conda-forge pyserial 2 | from cobs import cobs # pip install cobs 3 | import crc8 4 | import struct 5 | import time 6 | from multiprocessing import Process, Queue 7 | from queue import Empty 8 | from scipy.spatial.transform import Rotation as R 9 | import numpy as np 10 | from serial.tools import list_ports 11 | from numpy_ringbuffer import RingArrayBuffer,filterBuffer 12 | 13 | 14 | def quaternion_to_rotation_matrix(q): 15 | """Converts a quaternion to a rotation matrix. 16 | 17 | Args: 18 | q: A numpy array representing a quaternion in the format [x, y, z, w]. 19 | 20 | Returns: 21 | A 3x3 numpy array representing the rotation matrix. 22 | """ 23 | 24 | x, y, z, w = q 25 | 26 | R = np.array([ 27 | [1 - 2*y**2 - 2*z**2, 2*x*y - 2*z*w, 2*x*z + 2*y*w], 28 | [2*x*y + 2*z*w, 1 - 2*x**2 - 2*z**2, 2*y*z - 2*x*w], 29 | [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x**2 - 2*y**2] 30 | ]) 31 | 32 | return R 33 | 34 | 35 | class SerialDataCollector: 36 | 37 | # Bit masks for fresh data 38 | FRESH_LOAD_CELL_0 = 0x0001 39 | FRESH_LOAD_CELL_1 = 0x0002 40 | FRESH_LOAD_CELL_2 = 0x0004 41 | FRESH_LOAD_CELL_3 = 0x0008 42 | FRESH_LOAD_CELL_ALL = 0x000F 43 | FRESH_FILTER_STATUS = 0x0010 44 | FRESH_LINEAR_ACCEL = 0x0020 45 | FRESH_QUATERNION = 0x0040 46 | FRESH_ROTATION_MATRIX = 0x0080 47 | FRESH_GRAVITY_VECTOR = 0x0100 48 | FRESH_GYRO = 0x0200 49 | FRESH_ACCEL = 0x0400 50 | 51 | def __init__(self, baudrate, queue, max_queue_size=1): 52 | self.ser = None 53 | self.baudrate = baudrate 54 | # Hexadecimal VendorID=0x16c0 & ProductID=0x483 55 | self.vendor_id = 0x16c0 56 | self.product_id = 0x483 57 | self.sample_timestamps = RingArrayBuffer(buffer_len=100, shape=1,dtype=np.float64) # List to store sample timestamps 58 | self.sample_timestamps.storage[:] = time.time() 59 | 60 | self.unpacked = None 61 | self.running = False 62 | self.queue = queue 63 | self.max_queue_size = max_queue_size 64 | 65 | self.filter_for_ang_vel = filterBuffer(shape=3,fs=500, filter_order=4,cut_off_frequency=20,dtype=np.float64) 66 | self.filter_for_lin_acc = filterBuffer(shape=3,fs=500, filter_order=4,cut_off_frequency=20,dtype=np.float64) 67 | 68 | self.filter_force_measurement = filterBuffer(shape=4,fs=200, filter_order=4,cut_off_frequency=20,dtype=np.float64) 69 | # self.filter_force_measurement.buf_filt.storage[:]=2000 70 | # self.filter_force_measurement.buf_raw.storage[:]=2000 71 | 72 | # Initialize data variables 73 | 74 | self.ang_vel = np.zeros(3,dtype=np.float64) 75 | self.lin_acc = np.zeros(3,dtype=np.float64) 76 | 77 | self.lin_acc_raw = np.zeros(3,dtype=np.float64) 78 | 79 | self.ang_vel_raw = np.zeros(3,dtype=np.float64) 80 | 81 | self.gravity_vec = np.zeros(3,dtype=np.float64) 82 | self.base_quat = np.array([0, 0, 0, 1],dtype=np.float64) 83 | self.rotationMatrix = np.zeros(9,dtype=np.float64) 84 | self.ang_vel_raw = np.zeros(3,dtype=np.float64) 85 | 86 | self.lin_acc_imu_filtered = np.zeros(3,dtype=np.float64) 87 | self.ang_vel_imu_filtered = np.zeros(3,dtype=np.float64) 88 | 89 | self.force_measurement_raw = np.zeros(4,dtype=np.int32) 90 | self.force_measurement = np.zeros(4,dtype=np.int32) # filtered 91 | self.force_measurement_min = np.ones(4,dtype=np.int32)*5000 92 | 93 | # NOTE: MUST match the actual values of the force sensor at zero contact 94 | self.force_measurement_min = np.array([730,1020,1000,780]) 95 | # self.force_measurement_raw_buf = RingArrayBuffer(buffer_len=20,shape=(4,),dtype=int) 96 | self.force_measurement_threshold = 400 97 | 98 | self.contact = np.zeros(2,dtype=bool) 99 | self.last_contact = np.zeros_like(self.contact) 100 | self.contact_filt = np.zeros_like(self.contact) 101 | 102 | 103 | self.filterStatus = 0 104 | self.filterDynamicsMode = 0 105 | self.filterStatusFlags = 0 106 | self.fresh = 0 107 | 108 | def find_port(self): 109 | ports = list_ports.comports() 110 | for port in ports: 111 | if port.vid == self.vendor_id and port.pid == self.product_id: 112 | return port.device 113 | raise serial.SerialException("Teensy not connected!") 114 | 115 | def connect(self): 116 | self.ser = None 117 | port = self.find_port() 118 | if port: 119 | try: 120 | self.ser = serial.Serial(port, self.baudrate, timeout=1) 121 | self.ser.set_low_latency_mode(True) 122 | print(f"Connected to {port}") 123 | except serial.SerialException as e: 124 | print(f"Failed to connect to {port}: {e}") 125 | else: 126 | print("No suitable port found!") 127 | 128 | 129 | def start(self): 130 | self.running = True 131 | self.process = Process(target=self.read_loop) 132 | self.process.start() 133 | 134 | def stop(self): 135 | self.running = False 136 | self.process.join() 137 | 138 | def read_loop(self): 139 | while self.running: 140 | self.read() 141 | 142 | def read(self): 143 | # # see if there is a connection and retry to connect if not 144 | if not self.ser: 145 | self.connect() 146 | if not self.ser: 147 | self.queue.put(None) 148 | return 149 | 150 | received_data = bytearray() 151 | while True: 152 | try: 153 | byte = self.ser.read(1) # Read a single byte 154 | except serial.SerialException as e: 155 | self.connect() 156 | self.queue.put(None) 157 | return 158 | 159 | if byte == b'\x00': # Check if it's the null byte 160 | break 161 | received_data += byte 162 | 163 | try: 164 | decoded = cobs.decode(received_data) 165 | except cobs.DecodeError: 166 | print("bad COBS") 167 | # self.queue.put(None) 168 | # print("CRC mismatch") 169 | return 170 | 171 | try: 172 | # self.unpacked = struct.unpack("@hhhhffffffffffffffffffffffffffffhhhh", decoded[1:-1]) 173 | self.unpacked = struct.unpack("@hhhhfffffffffffffffffffhhhh", decoded[1:-1]) # no rotation matrix 174 | self.sample_timestamps.add(time.time()) # Add timestamp for the new sample 175 | 176 | # x is forward, y is left, z is up 177 | # in the imu coordinate x is forward, y is right, z is down 178 | # so have to negate y and z 179 | # Update data variables 180 | 181 | self.fresh = self.unpacked[26] # [35] 182 | 183 | # force_measurement_fresh = bool(self.fresh & self.FRESH_LOAD_CELL_ALL) 184 | # if force_measurement_fresh: 185 | 186 | self.force_measurement_raw[:] = self.unpacked[0:4] # 0 is left-back, 1 left-front, 2 right-back, 3 right-front 187 | self.force_measurement[:] = self.filter_force_measurement.add(self.force_measurement_raw) 188 | self.contact[0] = (self.force_measurement[0] + self.force_measurement[1]-self.force_measurement_min[0]-self.force_measurement_min[1])>self.force_measurement_threshold 189 | self.contact[1] = (self.force_measurement[2] + self.force_measurement[3]-self.force_measurement_min[2]-self.force_measurement_min[3])>self.force_measurement_threshold 190 | 191 | self.lin_acc_raw[:] = self.unpacked[4:7] 192 | self.ang_vel_raw[:] = self.unpacked[7:10] 193 | self.gravity_vec[:] = self.unpacked[10:13] 194 | # self.rotationMatrix[:] = self.unpacked[13:22] 195 | # 4 196 | self.base_quat[:] = self.unpacked[13:17] # [22:26] 197 | self.lin_acc_imu_filtered[:] = self.unpacked[17:20] # [26:29] 198 | self.ang_vel_imu_filtered[:] = self.unpacked[20:23] #[29:32] 199 | self.filterStatus = self.unpacked[23] #[32] 200 | self.filterDynamicsMode = self.unpacked[24] #[33] 201 | self.filterStatusFlags = self.unpacked[25] #[34] 202 | self.sps = 100/(self.sample_timestamps[0][0] - self.sample_timestamps[-1][0]) # sample per second 203 | 204 | # # filtered data 205 | self.ang_vel[:] = self.filter_for_ang_vel.add(self.ang_vel_imu_filtered) 206 | self.lin_acc[:] = self.filter_for_lin_acc.add(self.lin_acc_imu_filtered) 207 | 208 | # self.rot_from_quat = quaternion_to_rotation_matrix(self.base_quat) 209 | 210 | # Send data to the main process 211 | latest_data = self.get_latest_data() 212 | if self.queue.qsize() >= self.max_queue_size: 213 | try: 214 | self.queue.get_nowait() # Remove oldest item 215 | except Empty: 216 | pass 217 | self.queue.put(latest_data) 218 | 219 | except struct.error as e: 220 | print("Struct unpacking error:", e) 221 | 222 | def get_latest_data(self): 223 | return { 224 | 'force_measurement': self.force_measurement_raw, 225 | 'self.force_measurement_min': self.force_measurement_min, 226 | 'lin_acc': self.lin_acc, 227 | 'contact': self.contact, 228 | # 'lin_acc_raw': self.lin_acc_raw, 229 | # 'lin_acc_filtered': self.lin_acc_imu_filtered, 230 | 'ang_vel': self.ang_vel, 231 | # 'ang_vel_raw': self.ang_vel_raw, #self.gyroData_filtered, # self.gyroData, 232 | # 'ang_vel_filtered': self.ang_vel_imu_filtered, 233 | 'base_quat': self.base_quat, 234 | # 'rot': self.rotationMatrix, 235 | # 'rot_form_quat': self.rot_from_quat.ravel(), 236 | # 'rot_debug': self.rotationMatrix.ravel()/self.rot_from_quat.ravel(), 237 | # 'rotationMatrix': self.rotationMatrix, 238 | 'gravity_vec': self.gravity_vec, 239 | 'sps': self.sps 240 | } 241 | 242 | def interpret_fresh_bytes(self): 243 | fresh_dict = { 244 | 'forceSensorData_0': bool(self.fresh & self.FRESH_LOAD_CELL_0), 245 | 'forceSensorData_1': bool(self.fresh & self.FRESH_LOAD_CELL_1), 246 | 'forceSensorData_2': bool(self.fresh & self.FRESH_LOAD_CELL_2), 247 | 'forceSensorData_3': bool(self.fresh & self.FRESH_LOAD_CELL_3), 248 | 'filterStatus': bool(self.fresh & self.FRESH_FILTER_STATUS), 249 | 'linearAccel': bool(self.fresh & self.FRESH_LINEAR_ACCEL), 250 | 'quaternionData': bool(self.fresh & self.FRESH_QUATERNION), 251 | # 'rotationMatrix': bool(self.fresh & self.FRESH_ROTATION_MATRIX), 252 | 'gravityVector': bool(self.fresh & self.FRESH_GRAVITY_VECTOR), 253 | 'gyroData': bool(self.fresh & self.FRESH_GYRO), 254 | 'accelData': bool(self.fresh & self.FRESH_ACCEL), 255 | } 256 | return fresh_dict 257 | 258 | 259 | class SensorController: 260 | def __init__(self, baudrate=1500000): 261 | self.queue = Queue(maxsize=1) 262 | self.data_collector = SerialDataCollector(baudrate, self.queue) 263 | 264 | def start(self): 265 | self.data_collector.start() 266 | 267 | def stop(self): 268 | self.data_collector.stop() 269 | 270 | def get_samples_per_second(self): 271 | return self.data_collector.get_samples_per_second() 272 | 273 | def get_latest_data(self): 274 | try: 275 | return self.queue.get(timeout=0.1) 276 | except Empty: 277 | print("No data received!") 278 | return None 279 | 280 | if __name__ == "__main__": 281 | 282 | from publisher import DataPublisher 283 | 284 | publisher = DataPublisher('udp://localhost:9870',encoding="msgpack",broadcast=False) 285 | 286 | # Initialize the SensorController instance 287 | sensors = SensorController() 288 | 289 | # Start the collection process 290 | sensors.start() 291 | 292 | target_fps = 500 293 | frame_time = 1 / target_fps 294 | 295 | try: 296 | while True: 297 | start_time = time.time() 298 | latest_data = sensors.get_latest_data() 299 | if latest_data: 300 | # print(f"{latest_data['sps']:.2f}") 301 | # print(latest_data['sps']) 302 | # print(latest_data['gyroData']) 303 | print(latest_data['contact'],latest_data['force_measurement']) 304 | 305 | publisher.publish({"sensors":latest_data}) 306 | pass 307 | else: 308 | print("sensorController: No data received!") 309 | 310 | elapsed_time = time.time() - start_time 311 | sleep_time = frame_time - elapsed_time 312 | 313 | if sleep_time > 0: 314 | time.sleep(sleep_time) 315 | 316 | except KeyboardInterrupt: 317 | # Stop the data collection gracefully on interrupt 318 | sensors.stop() 319 | print("Data collection stopped.") 320 | -------------------------------------------------------------------------------- /setup/conda_env.yaml: -------------------------------------------------------------------------------- 1 | # to run: 2 | # $ conda env create --file conda_env.yaml 3 | name: py38 4 | channels: 5 | # - pytorch 6 | # - nvidia 7 | # - open3d-admin 8 | - conda-forge 9 | - defaults 10 | dependencies: 11 | - python=3.8 12 | # - absl-py 13 | - autopep8 14 | - box2d-py 15 | - cython 16 | - embree 17 | - gym 18 | - imageio 19 | - joblib 20 | - lxml 21 | - matplotlib 22 | - meshio 23 | - msgpack-python 24 | - networkx 25 | - numba 26 | - numpy 27 | - pandas 28 | - pip 29 | # - point_cloud_utils 30 | - pydot 31 | # - pythonocc-core 32 | - nvidia::cudatoolkit=11.3 33 | - pytorch::pytorch 34 | - pytorch::torchaudio 35 | - pytorch::torchvision 36 | - termcolor 37 | # - rtree=0.9.4 38 | - scikit-learn 39 | - seaborn 40 | - shapely 41 | - tensorboard 42 | - tqdm 43 | - trimesh 44 | - jupyter 45 | - jupyter_contrib_nbextensions 46 | - line_profiler 47 | - opencv 48 | - aiohttp 49 | - onnxruntime 50 | - onnx 51 | - mlflow 52 | - pytest 53 | - black 54 | - pip: 55 | - sshkeyboard 56 | - pygame 57 | - ray 58 | - open3d 59 | - hydra-core 60 | - pybullet 61 | - gmsh 62 | - parametrize-from-file 63 | - casadi 64 | - inputs # pypi version 65 | - lcm # pypi version 66 | - wandb 67 | # - pyembree # on windows this has to be installed manually 68 | -------------------------------------------------------------------------------- /src/Readme.md: -------------------------------------------------------------------------------- 1 | # Command input 2 | ``` 3 | sudo ./simple_test ifname control_mode target max_velocity 4 | ``` 5 | - ifname: the name of adapter 6 | - control_mode: "csp","csv" 7 | - target:\ 8 | if mode is csp, traget means target position, unit is rad \ 9 | if mode is csv,target means target velocity, unit is rad/s 10 | - max_velocity: the maximum velocity of motor.unit is rad/s, a comma separated string 11 | 12 | 13 | example (e.g enp3s0 is the interface_name, change "enp3s0" to your adaptor name for the following code) 14 | ``` 15 | //sudo ./simple_test enp3s0 csv 0,0,0,0,0,0 1 40 16 | // sudo ./simple_test enp3s0 csp 0,0,0,0,0,0 1 40 17 | 18 | sudo ./ext/SOEM/test/linux/slaveinfo/slaveinfo enp3s0 -sdo 19 | 20 | 21 | // restart interface 22 | sudo ip link set enp3s0 down 23 | sudo ip link set enp3s0 up 24 | ``` 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/ethercat_motor_py.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include // For passing NumPy arrays 6 | #include 7 | 8 | #include "ethercat_motor.h" 9 | 10 | namespace py = pybind11; 11 | 12 | class PyMotor : public Motor 13 | { 14 | public: 15 | // tx_pdo_t tx_pdo_example{}; 16 | 17 | void run_in_background() 18 | { 19 | pybind11::gil_scoped_release release; 20 | thread2 = std::thread(&Motor::run, this); 21 | thread2.detach(); 22 | } 23 | // for the inherited function you have to specify the constructor again for pybind11 :( 24 | PyMotor(const std::string &ifname, int8_t control_mode_int8, double max_velocity, double max_torque) 25 | { 26 | _init(ifname, control_mode_int8, max_velocity, max_torque); 27 | } 28 | }; 29 | 30 | PYBIND11_MODULE(ethercat_motor_py, m) 31 | { 32 | 33 | py::enum_(m, "CONTROL_MODE", py::arithmetic()) 34 | .value("CYCLIC_SYNC_POSITION", CONTROL_MODE::CYCLIC_SYNC_POSITION) 35 | .value("CYCLIC_SYNC_VELOCITY", CONTROL_MODE::CYCLIC_SYNC_VELOCITY) 36 | .value("CYCLIC_SYNC_TORQUE", CONTROL_MODE::CYCLIC_SYNC_TORQUE) 37 | .export_values(); 38 | 39 | // py::class_(m, "tx_pdo_t") 40 | // .def(py::init([]() 41 | // { return tx_pdo_t(); })) 42 | // .def_readwrite("target_position", &tx_pdo_t::target_position) 43 | // .def_readwrite("target_velocity", &tx_pdo_t::target_velocity) 44 | // .def_readwrite("velocity_offset", &tx_pdo_t::velocity_offset) 45 | // .def_readwrite("max_torque", &tx_pdo_t::max_torque) 46 | // .def_readwrite("target_torque_raw", &tx_pdo_t::target_torque) 47 | // .def_readwrite("torque_offset", &tx_pdo_t::torque_offset) 48 | // .def_readwrite("control_word", &tx_pdo_t::control_word) 49 | // .def_readwrite("mode_of_operation", &tx_pdo_t::mode_of_operation) 50 | // .def("astuple", 51 | // [](const tx_pdo_t &self) 52 | // { 53 | // return py::make_tuple( 54 | // self.target_position, 55 | // self.target_velocity, 56 | // self.velocity_offset, 57 | // self.max_torque, 58 | // self.target_torque, 59 | // self.torque_offset, 60 | // self.control_word, 61 | // self.mode_of_operation); 62 | // }) 63 | // .def_static("fromtuple", [](const py::tuple &tup) 64 | // { if (py::len(tup) != 8) {throw py::cast_error("Invalid size");} 65 | // return tx_pdo_t{ 66 | // tup[0].cast(), 67 | // tup[1].cast(), 68 | // tup[2].cast(), 69 | // tup[3].cast(), 70 | // tup[4].cast(), 71 | // tup[5].cast(), 72 | // tup[6].cast(), 73 | // tup[7].cast()}; }); 74 | 75 | // PYBIND11_NUMPY_DTYPE(tx_pdo_t, 76 | // target_position, 77 | // target_velocity, 78 | // velocity_offset, 79 | // max_torque, 80 | // target_torque, 81 | // torque_offset, 82 | // control_word, 83 | // mode_of_operation); 84 | 85 | // py::class_(m, "rx_pdo_t") 86 | // .def(py::init([]() 87 | // { return rx_pdo_t(); })) 88 | // .def_readwrite("position_actual", &rx_pdo_t::position_actual) 89 | // .def_readwrite("position_follow_err", &rx_pdo_t::position_follow_err) 90 | // .def_readwrite("velocity_actual", &rx_pdo_t::velocity_actual) 91 | // .def_readwrite("torque_actual", &rx_pdo_t::torque_actual) 92 | // .def_readwrite("status_word", &rx_pdo_t::status_word) 93 | // .def_readwrite("mode_of_operation_disp", &rx_pdo_t::mode_of_operation_disp) 94 | // .def("astuple", 95 | // [](const rx_pdo_t &self) 96 | // { 97 | // return py::make_tuple( 98 | // self.position_actual, 99 | // self.position_follow_err, 100 | // self.velocity_actual, 101 | // self.torque_actual, 102 | // self.status_word, 103 | // self.mode_of_operation_disp); 104 | // }) 105 | // .def_static("fromtuple", [](const py::tuple &tup) 106 | // { if (py::len(tup) != 6) {throw py::cast_error("Invalid size");} 107 | // return rx_pdo_t{ 108 | // tup[0].cast(), 109 | // tup[1].cast(), 110 | // tup[2].cast(), 111 | // tup[3].cast(), 112 | // tup[4].cast(), 113 | // tup[5].cast()}; }); 114 | 115 | // PYBIND11_NUMPY_DTYPE(rx_pdo_t, 116 | // position_actual, 117 | // position_follow_err, 118 | // velocity_actual, 119 | // torque_actual, 120 | // status_word, 121 | // mode_of_operation_disp); 122 | 123 | py::class_(m, "PyMotor", py::dynamic_attr()) 124 | .def(py::init()) 125 | .def("set_should_terminate", &PyMotor::set_should_terminate) 126 | // .def_readonly("target_int32", &PyMotor::target_int32) // Bind the run function 127 | .def_readwrite("control_mode_int8", &PyMotor::control_mode_int8) 128 | .def_readwrite("should_print", &PyMotor::should_print) 129 | .def_readwrite("debug", &PyMotor::debug) 130 | .def_readonly("rated_torque", &PyMotor::rated_torque) 131 | .def_readonly("actual_position", &PyMotor::actual_position) 132 | .def_readonly("actual_position_error", &PyMotor::actual_position_error) 133 | .def_readonly("actual_velocity", &PyMotor::actual_velocity) 134 | .def_readonly("actual_velocity_filtered", &PyMotor::actual_velocity_filtered) 135 | .def_readonly("actual_torque_raw", &PyMotor::actual_torque_raw) 136 | .def_readonly("actual_torque", &PyMotor::actual_torque) 137 | .def_readwrite("use_position_pd", &PyMotor::use_position_pd) 138 | .def_readwrite("rotor_current_to_torque_ratio", &PyMotor::rotor_current_to_torque_ratio) 139 | .def_readwrite("gear_ratio", &PyMotor::gear_ratio) 140 | .def_readwrite("kp", &PyMotor::kp) 141 | .def_readwrite("kd", &PyMotor::kd) 142 | .def_readonly("dof_position", &PyMotor::dof_position) 143 | .def_readonly("dof_velocity", &PyMotor::dof_velocity) 144 | .def_readonly("dof_velocity_filtered", &PyMotor::dof_velocity_filtered) 145 | .def_readwrite("target_dof_position", &PyMotor::target_dof_position) 146 | .def_readonly("target_dof_torque_Nm", &PyMotor::target_dof_torque_Nm) 147 | .def_readwrite("torque_multiplier", &PyMotor::torque_multiplier) 148 | .def_readonly("target_dof_torque_Nm_filtered", &PyMotor::target_dof_torque_Nm_filtered) 149 | .def_readonly("target_dof_torque_A", &PyMotor::target_dof_torque_A) 150 | .def_readonly("target_dof_torque_A_adjusted", &PyMotor::target_dof_torque_A_adjusted) 151 | 152 | .def_readonly("loop_counter", &PyMotor::loop_counter) 153 | .def_readonly("actual_position_after_offset", &PyMotor::actual_position_after_offset) 154 | .def_readonly("position_offset", &PyMotor::position_offset) 155 | .def_readonly("dt_measured", &PyMotor::dt_measured) 156 | // .def_readonly("actual_velocity_measured", &PyMotor::actual_velocity_measured) 157 | 158 | .def("set_target_input", &PyMotor::set_target_input) 159 | .def("set_target_position", &PyMotor::set_target_position,py::arg().noconvert()) 160 | .def("set_target_velocity", &PyMotor::set_target_velocity,py::arg().noconvert()) 161 | .def("set_target_torque", &PyMotor::set_target_torque,py::arg().noconvert()) 162 | .def("set_target_position_offset", &PyMotor::set_target_position_offset,py::arg().noconvert()) 163 | .def("set_velocity_offset", &PyMotor::set_velocity_offset,py::arg().noconvert()) 164 | .def("set_torque_offset", &PyMotor::set_torque_offset,py::arg().noconvert()) 165 | .def("set_max_torque", &PyMotor::set_max_torque,py::arg().noconvert()) 166 | .def("set_position_offset", &PyMotor::set_position_offset,py::arg().noconvert()) 167 | .def("run", &PyMotor::run_in_background,R"pbdoc(Starts the motor control loop in a background thread.)pbdoc"); 168 | } -------------------------------------------------------------------------------- /src/pybind_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace py = pybind11; 15 | 16 | #define NUM_TARGET 6 // num of motors 17 | 18 | using MatrixXdR = Eigen::Matrix; 19 | 20 | template 21 | using Vec12 = Eigen::Matrix; 22 | 23 | class BackgroundWorker 24 | { 25 | public: 26 | 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | Eigen::VectorXd eigen_vec = Eigen::VectorXd::Zero(NUM_TARGET); // Declare and initialize in one line 29 | // Vec12 eigen_vec; 30 | 31 | std::chrono::system_clock::time_point t; 32 | std::vector data_vector{1, 2, 3}; 33 | 34 | BackgroundWorker() = default; // Default constructor 35 | BackgroundWorker(const std::string &ifname_, int8_t control_mode_int8, double max_velocity, double max_torque) 36 | { 37 | std::cout << "BackgroundWorker(int k)" << std::endl; 38 | std::cout << "ifname_:" << ifname_ << std::endl; 39 | std::cout << "control_mode_int8:" << (int)control_mode_int8 << std::endl; 40 | std::cout << "max_velocity:" << max_velocity << std::endl; 41 | std::cout << "max_torque:" << max_torque << std::endl; 42 | } 43 | 44 | void start_background_thread(int duration) 45 | { 46 | py::gil_scoped_release release; 47 | worker_thread_ = std::thread(&BackgroundWorker::background_worker, this, duration); 48 | worker_thread_.detach(); 49 | } 50 | 51 | void set_should_terminate(bool value) 52 | { 53 | should_terminate = value; 54 | std::cout << "set_should_terminate()" << std::endl; 55 | } 56 | void change_data_vector(const py::array_t &a) 57 | { 58 | for (int i = 0; i < 3; i++) 59 | { 60 | data_vector[i] = a.at(i); 61 | } 62 | } 63 | 64 | private: 65 | void background_worker(int duration) 66 | { 67 | for (int i = 0; i < duration && !should_terminate; i++) 68 | { 69 | std::this_thread::sleep_for(std::chrono::seconds(1)); 70 | t = std::chrono::system_clock::now(); 71 | eigen_vec(i%NUM_TARGET)=static_cast(std::chrono::system_clock::to_time_t(t)); 72 | data_vector[2] = i; 73 | std::cout << "Background thread running: " << i << std::endl; 74 | } 75 | std::cout << "Background thread terminating..." << std::endl; 76 | } 77 | 78 | std::atomic should_terminate{false}; 79 | std::thread worker_thread_; 80 | }; 81 | 82 | PYBIND11_MODULE(pybind_test, m) 83 | { 84 | py::class_(m, "BackgroundWorker", py::dynamic_attr()) 85 | .def(py::init<>()) // Expose the constructor 86 | .def(py::init()) 87 | .def_readonly("t", &BackgroundWorker::t) 88 | .def_readwrite("data_vector", &BackgroundWorker::data_vector) 89 | .def("change_data_vector", &BackgroundWorker::change_data_vector) 90 | .def("start_background_thread", &BackgroundWorker::start_background_thread) 91 | .def("set_should_terminate", &BackgroundWorker::set_should_terminate) 92 | .def_readwrite("eigen_vec", &BackgroundWorker::eigen_vec, py::return_value_policy::reference); 93 | } 94 | 95 | // #include 96 | 97 | // namespace py = pybind11; 98 | 99 | // class MyClass { 100 | // public: 101 | // MyClass(const std::string& name) : name_(name) {} 102 | 103 | // std::string get_name() const { return name_; } 104 | 105 | // private: 106 | // std::string name_; 107 | // }; 108 | 109 | // PYBIND11_MODULE(mymodule, m) { 110 | // py::class_(m, "MyClass") 111 | // .def(py::init()) // Constructor with std::string argument 112 | // .def("get_name", &MyClass::get_name); 113 | // } -------------------------------------------------------------------------------- /src/simple_test.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Example code for Simple Open EtherCAT master 3 | * 4 | * Usage : simple_test [ifname1] 5 | * ifname is NIC interface, f.e. eth0 6 | * 7 | * This is a minimal test. 8 | * 9 | * (c)Arthur Ketels 2010 - 2011 10 | */ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include // TODO 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include "ethercat_motor.h" 28 | 29 | Eigen::VectorXd parse_doubles(const char *input) 30 | { 31 | std::stringstream ss(input); 32 | // Initialize an empty Eigen vector 33 | Eigen::VectorXd result; 34 | 35 | // Pre-allocate if NUM_TARGET is known (slightly different approach) 36 | if (NUM_TARGET > 0) { 37 | result.resize(NUM_TARGET); 38 | } 39 | 40 | std::string substr; 41 | int index = 0; 42 | while (std::getline(ss, substr, ',')) 43 | { 44 | std::istringstream iss(substr); 45 | double value; 46 | if (!(iss >> value)) { 47 | throw std::runtime_error("Invalid number format in input string"); 48 | } 49 | result(index++) = value; 50 | } 51 | 52 | return result; 53 | } 54 | 55 | 56 | int main(int argc, char *argv[]) 57 | { 58 | printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); 59 | // get arguments from command line, contains the adapter,type of control mode: csp, csv,target position or target velocity, 60 | // if is csp mode, the second argument is the target position, if is csv mode, the second argument is the target velocity 61 | // if there is no argument, the program will print the available control mode and exit 62 | 63 | int8_t control_mode; 64 | 65 | if (argc < 6) 66 | { 67 | // print argc and all argv 68 | printf("argc: %d\n", argc); 69 | for (int i = 0; i < argc; i++) 70 | { 71 | printf("argv[%d]: %s\n", i, argv[i]); 72 | } 73 | ec_adaptert *adapter = NULL; 74 | printf("Usage: simple_test ifname1 control_mode target max_velocity[rad/s] max_torque[0.1%%]\n"); 75 | printf("\nAvailable adapters:\n"); 76 | adapter = ec_find_adapters(); 77 | while (adapter != NULL) 78 | { 79 | printf(" - %s (%s)\n", adapter->name, adapter->desc); 80 | adapter = adapter->next; 81 | } 82 | ec_free_adapters(adapter); 83 | printf("control_mode: csp, csv, cst\n"); 84 | printf("target: target position [rad] if control_mode == csp\n"); 85 | printf(" target velocity [rad/s] if control_mode == csv\n"); 86 | printf(" target torque [0.1%% of rated current] if control mode == cst\n"); 87 | return (0); 88 | } 89 | // else if the mode is csp,check the string, wheather is "csp", the target position is the third argument 90 | // else if the mode is csv,check the string, wheather is "csv", the target velocity is the third argument 91 | else 92 | { 93 | std::string modeStr = argv[2]; 94 | Eigen::VectorXd target_input = parse_doubles(argv[3]); 95 | 96 | // double target = atof(argv[3]); 97 | double max_velocity = atof(argv[4]); 98 | double max_torque = atof(argv[5]); 99 | 100 | if (modeStr == "csp") 101 | { 102 | control_mode = CONTROL_MODE::CYCLIC_SYNC_POSITION; 103 | printf("Control mode: csp\n"); 104 | printf("Target position [rad]: "); 105 | } 106 | else if (modeStr == "csv") 107 | { 108 | control_mode = CONTROL_MODE::CYCLIC_SYNC_VELOCITY; 109 | printf("Control mode: csv\n"); 110 | printf("Target velocity [rad/s]: "); 111 | } 112 | else if (modeStr == "cst") 113 | { 114 | control_mode = CONTROL_MODE::CYCLIC_SYNC_TORQUE; 115 | // double target_torque_raw = atof(argv[3]); 116 | printf("Control mode: cst\n"); 117 | printf("Target torque: [%%rated torue]"); 118 | } 119 | else 120 | { 121 | printf("Invalid control mode\n"); 122 | return (0); 123 | } 124 | 125 | for (double value : target_input) 126 | { 127 | printf("%.3f ", value); 128 | } 129 | printf("\nMax velocity: %f rad/s \n", max_velocity); 130 | // return (0); 131 | 132 | Motor motor(argv[1], control_mode, max_velocity,max_torque); 133 | motor.set_target_input(target_input); 134 | 135 | if (argc == 7){ 136 | 137 | Eigen::VectorXd target_pos_offset = parse_doubles(argv[6]); 138 | motor.set_target_position_offset(target_pos_offset); 139 | } 140 | 141 | motor.run(); 142 | 143 | printf("End program\n"); 144 | return (0); 145 | } 146 | } 147 | 148 | // sudo ./simple_test enp3s0 csv 0.1,0.2,0.4,0.8,0.1,0.2 1 149 | // sudo ./simple_test enp3s0 csp 0,0,0,0,0,0 1 -------------------------------------------------------------------------------- /sshkeyboard_pd_test.py: -------------------------------------------------------------------------------- 1 | import time 2 | from sshkeyboard import listen_keyboard 3 | import asyncio 4 | from publisher import DataPublisher 5 | import random 6 | import sys 7 | import numpy as np 8 | pi = np.pi 9 | 10 | from contextlib import contextmanager 11 | 12 | 13 | 14 | def set_zero(): 15 | set_PD_value(np.ones(10)*60,np.ones(10)*5) 16 | print("reseting to zeroth position") 17 | change_pos_dof_target(target=np.zeros(10),action_is_on=np.ones(10,dtype=bool)) 18 | 19 | 20 | @contextmanager 21 | def conditionally_publish(should_set_zero=True): 22 | try: 23 | change_publish_state(should_publish=True) # Assuming this enables motor control 24 | yield 25 | finally: 26 | change_publish_state(should_publish=False) # Assuming this enables motor control 27 | if should_set_zero: 28 | set_zero() 29 | 30 | 31 | data_publisher = DataPublisher('udp://localhost:9871',broadcast=True) 32 | # data_publisher = DataPublisher('udp://localhost:9871',broadcast=False) 33 | 34 | 35 | def publish(data): 36 | data_publisher.publish(data) 37 | # data_publisher2.publish(data) 38 | 39 | 40 | def change_publish_state(should_publish): 41 | for k in range(10): 42 | publish({"should_publish":should_publish}) 43 | time.sleep(0.01) 44 | 45 | default_action_is_on = np.ones(10) 46 | def change_pos_dof_target(target,action_is_on=default_action_is_on): 47 | data = { 48 | "dof_pos_target":target, 49 | "action_is_on":action_is_on, 50 | } 51 | publish(data) 52 | 53 | 54 | def set_PD_value(kp,kd): 55 | data = { 56 | "kp":kp, 57 | "kd":kd, 58 | } 59 | publish(data) 60 | 61 | dt = 1/400 62 | 63 | # REFERENCE 64 | # self.min_limit = np.array([-pi/3, -pi/6, -pi/9, 0, -pi/5, -pi/6, -pi/2, -pi/5, -pi/2, -pi/5]) 65 | # self.max_limit = np.array([pi/6, pi/2, pi/5, pi/2, pi/5, pi/3, pi/6, pi/9, 0, pi/5]) 66 | 67 | def sweep(t, a, b, freq_start, freq_end): 68 | 69 | x = np.arange(0, t, dt) 70 | 71 | frequencies = np.linspace(freq_start, freq_end, x.size) 72 | 73 | 74 | y = (a + b) / 2 + (b - a) / 2 * np.cos(2 * np.pi * frequencies * x + np.pi) 75 | 76 | action = np.ones_like(x) 77 | 78 | off_ratio=0 79 | hz=50 80 | cluster_repeat = int((1/dt)*(1/hz)) #amount of points to make up x hz 81 | tentative_action_is_on = np.repeat(np.random.rand(len(action)//cluster_repeat)>off_ratio,cluster_repeat) 82 | action[:len(tentative_action_is_on)] = tentative_action_is_on 83 | 84 | return np.concatenate((y, np.flip(y))), np.concatenate((action, np.flip(action))) 85 | 86 | def indvidual_motor_test_passive(key): 87 | if key == "0": 88 | with conditionally_publish(should_set_zero=True): 89 | y, action = sweep(t=12, a=-pi/5, b=pi/5, freq_start=0.5, freq_end=3.5) 90 | print("motor " + key + " is moving") 91 | for i, act in zip(y, action): 92 | change_pos_dof_target(target=[i, 0, 0, 0, 0, -i, 0, 0, 0, 0], action_is_on=[act, 1, 1, 1, 1, act, 1, 1, 1, 1]) 93 | time.sleep(dt) 94 | 95 | if key == "1": 96 | with conditionally_publish(should_set_zero=True): 97 | y, action = sweep(t=12, a=-pi/5, b=pi/5, freq_start=0.5, freq_end=3) 98 | print("motor " + key + " is moving") 99 | for i, act in zip(y, action): 100 | change_pos_dof_target(target=[0, i, 0, 0, 0, 0, i, 0, 0, 0], action_is_on=[1, act, 1, 1, 1, 1, act, 1, 1, 1]) 101 | time.sleep(dt) 102 | 103 | if key == "2": 104 | with conditionally_publish(should_set_zero=True): 105 | y, action = sweep(t=12, a=-pi/5.5, b=pi/5.5, freq_start=0.5, freq_end=2.5) 106 | print("motor " + key + " is moving") 107 | # for i, act in zip(y, action): 108 | # change_pos_dof_target(target=[0, 0, i, 0, 0, 0, 0, i, 0, 0], action_is_on=[1, 1, act, 1, 1, 1, 1, act, 1, 1]) 109 | # time.sleep(dt) 110 | 111 | for i, act in zip(y, action): 112 | change_pos_dof_target(target=[0, 0, i, 0, 0, 0, 0, -pi/4.5, 0, 0], action_is_on=[1, 1, act, 1, 1, 1, 1, 1, 1, 1]) 113 | time.sleep(dt) 114 | 115 | # for i, act in zip(y, action): 116 | # change_pos_dof_target(target=[0, 0, pi/4.5, 0, 0, 0, 0, i, 0, 0], action_is_on=[1, 1, 1, 1, 1, 1, 1, act, 1, 1]) 117 | # time.sleep(dt) 118 | 119 | if key == "3": 120 | with conditionally_publish(should_set_zero=True): 121 | y, action = sweep(t=12, a=0, b=pi/5, freq_start=0.5, freq_end=3) 122 | print("motor " + key + " is moving") 123 | for i, act in zip(y, action): 124 | change_pos_dof_target(target=[0, 0, 0, i, 0, 0, 0, 0, -i, 0], action_is_on=[1, 1, 1, act, 1, 1, 1, 1, act, 1]) 125 | time.sleep(dt) 126 | 127 | if key == "4": 128 | with conditionally_publish(should_set_zero=True): 129 | y, action = sweep(t=12, a=pi/5.5, b=-pi/5.5, freq_start=0.5, freq_end=3) 130 | print("motor " + key + " is moving") 131 | for i, act in zip(y, action): 132 | change_pos_dof_target(target=[0, 0, 0, 0, i, 0, 0, 0, 0, -i], action_is_on=[1, 1, 1, 1, act, 1, 1, 1, 1, act]) 133 | time.sleep(dt) 134 | if key=='q': 135 | with conditionally_publish(should_set_zero=True): 136 | y3 = sweep(5,0,pi/4,1,3) 137 | y4 = sweep(5,pi/5.5,-pi/5.5,1,4) 138 | print("motor " +key+ " is moving") 139 | for i,j in zip(y3,y4): 140 | change_pos_dof_target(target=[0, 0, 0, i, j, 0, 0, 0, -i, -j]) 141 | time.sleep(dt) 142 | if key=='w': 143 | with conditionally_publish(should_set_zero=True): 144 | y2 = sweep(5,-pi/15,pi/15,1.5,2.5) 145 | y3 = sweep(5,0,pi/4,1,3) 146 | y4 = sweep(5,pi/5.5,-pi/5.5,1,4) 147 | print("motor " +key+ " is moving") 148 | for y2i,y3i,y4i in zip(y2,y3,y4): 149 | change_pos_dof_target(target=[0, 0, y2i, y3i, y4i, 0, 0, y2i, -y3i, -y4i]) 150 | time.sleep(dt) 151 | if key=='e': 152 | with conditionally_publish(should_set_zero=True): 153 | y1 = sweep(5,-pi/15,pi/15,1,3) 154 | y2 = sweep(5,-pi/15,pi/15,1.5,2.5) 155 | y3 = sweep(5,0,pi/4,1,3) 156 | y4 = sweep(5,pi/5.5,-pi/5.5,1,4) 157 | print("motor " +key+ " is moving") 158 | for y1i, y2i,y3i,y4i in zip(y1,y2,y3,y4): 159 | change_pos_dof_target(target=[0, y1i, y2i, y3i, y4i, 0, y1i, y2i, -y3i, -y4i]) 160 | time.sleep(dt) 161 | if key=='r': 162 | with conditionally_publish(should_set_zero=True): 163 | y0,a0 = sweep(30,-pi/5,pi/5,0.5,2.5) 164 | y1,a1 = sweep(30,-pi/5,pi/5,0.5,2.5) 165 | y2,a2 = sweep(30,-pi/8,pi/8,0.5,2.5) 166 | y3,a3 = sweep(30,0,pi/2,0.5,2.5) 167 | y4,a4 = sweep(30,pi/5.5,-pi/5.5,0.5,2.5) 168 | print("motor " +key+ " is moving") 169 | for y0i, y1i, y2i,y3i, y4i, a0i, a1i, a2i, a3i, a4i in zip(y0, y1,y2,y3,y4,a0,a1,a2,a3,a4): 170 | change_pos_dof_target(target=[y0i, y1i, y2i, y3i, y4i, y0i, y1i, y2i, -y3i, -y4i], 171 | action_is_on=[a0i, a1i, a2i, a3i, a4i, a0i, a1i, a2i, a3i, a4i]) 172 | time.sleep(dt) 173 | if key == "9": 174 | set_zero() 175 | 176 | def walking_test(key): 177 | if key == "0": 178 | import os 179 | import pandas as pd 180 | 181 | # Read trajectory data from CSV file 182 | output_folder = '/home/grl/repo/legged_env/trajectories' 183 | input_file = os.path.join(output_folder, 'walking_trajectory.csv') 184 | trajectory_data = pd.read_csv(input_file) 185 | 186 | positions = [] 187 | for i in range(len(trajectory_data)): 188 | positions.append([float(trajectory_data['0_hip_L'][i]), float(trajectory_data['1_hip_L'][i]), float(trajectory_data['2_hip_L'][i]), 189 | float(trajectory_data['3_knee_L'][i]), float(trajectory_data['4_foot_L'][i]), 190 | float(trajectory_data['5_hip_R'][i]), float(trajectory_data['6_hip_R'][i]), float(trajectory_data['7_hip_R'][i]), 191 | float(trajectory_data['8_knee_R'][i]), float(trajectory_data['9_foot_R'][i])]) 192 | print(positions[-1]) 193 | if key == '0': 194 | 195 | dt = 0.01 196 | while(True): 197 | for pos in positions: 198 | change_pos_dof_target(key='0',target=pos) 199 | time.sleep(dt) 200 | 201 | 202 | def change_pos_dof_target_kp_kd(target,kp,kd): 203 | data = { 204 | "dof_pos_target":target, 205 | "kp":kp, 206 | "kd":kd 207 | } 208 | publish(data) 209 | 210 | 211 | def change_pos_dof_target(target): 212 | data = { 213 | "dof_pos_target":target, 214 | } 215 | # print(target) 216 | publish(data) 217 | 218 | 219 | def low_vel_friction_test(key): 220 | if key == "9": 221 | pos_target=np.zeros(10) 222 | print(pos_target) 223 | change_pos_dof_target_kp_kd(target=pos_target,kp=np.ones(10)*60,kd=np.ones(10)*5) 224 | 225 | if key == "4": #ankle 226 | i = -pi/5.5 227 | pos_target = np.array([0, 0, 0, 0, i, 0, 0, 0, 0, -i]) 228 | print(pos_target.round(2)) 229 | change_pos_dof_target_kp_kd(target=pos_target,kp=np.ones(10)*60,kd=np.ones(10)*5) 230 | 231 | if key == "3": #knee 232 | i = pi/3 233 | pos_target = np.array([0, 0, 0, i, 0, 0, 0, 0, -i, 0]) 234 | print(pos_target.round(2)) 235 | change_pos_dof_target_kp_kd(target=pos_target,kp=np.ones(10)*60,kd=np.ones(10)*5) 236 | 237 | if key == "2": #HAA 238 | i = pi/12 239 | pos_target = np.array([0, 0, i, 0, 0, 0, 0, -i, 0, 0]) 240 | print(pos_target.round(2)) 241 | change_pos_dof_target_kp_kd(target=pos_target,kp=np.ones(10)*60,kd=np.ones(10)*5) 242 | 243 | if key == "1": #HFE 244 | i = pi/6 245 | pos_target = np.array([0, i, 0, 0, 0, 0, i, 0, 0, 0]) 246 | print(pos_target.round(2)) 247 | change_pos_dof_target_kp_kd(target=pos_target,kp=np.ones(10)*60,kd=np.ones(10)*5) 248 | 249 | if key == "0": #HFE 250 | i = pi/6 251 | pos_target = np.array([i, 0, 0, 0, 0, -i, 0, 0, 0, 0]) 252 | print(pos_target.round(2)) 253 | change_pos_dof_target_kp_kd(target=pos_target,kp=np.ones(10)*60,kd=np.ones(10)*5) 254 | 255 | if key == "r": 256 | data = { 257 | "kp":np.zeros(10), 258 | "kd":np.zeros(10)} 259 | print("kp,kd=0") 260 | publish(data) 261 | 262 | # target_joint_positions = np.array([ 263 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 264 | # [0, 0, 0, 0, pi*36/180, 0, 0, 0, 0, -pi*36/180], 265 | # # joint 0 266 | # [-pi/2, 0, 0, 0, pi*36/180, -pi/2, 0, 0, 0, -pi*36/180], 267 | # # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 268 | # [pi/2, 0, 0, 0, pi*36/180, pi/2, 0, 0, 0, -pi*36/180], 269 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 270 | # # join 1 271 | # [0, -pi/2, 0, 0, 0, 0, pi/2, 0, 0, 0], 272 | # # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 273 | # [0, pi/2, 0, 0, 0, 0, -pi/2, 0, 0, 0], 274 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 275 | # # joint 2 276 | # [0, 0, -2/9*pi, 0, 0, 0, 0, -2/9*pi, 0, 0,], 277 | # [0, 0, 2/9*pi, 0, 0, 0, 0, 2/9*pi, 0, 0,], 278 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 279 | # # join 3 280 | # [0, 0, 0, pi*5/9, 0, 0, 0, 0, -pi*5/9, 0], 281 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 282 | # # joint 4 283 | # [0, 0, 0, 0, pi*36/180, 0, 0, 0, 0, -pi*36/180], 284 | # [0, 0, 0, 0, -pi*36/180, 0, 0, 0, 0, pi*36/180], 285 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 286 | 287 | 288 | # ]) 289 | 290 | # current_pos_index = 0 291 | # time_per_pos = 2 292 | # num_steps = 100 293 | 294 | # # def joint_monkey(key): 295 | # # def go_to_next(): 296 | # # global current_pos_index 297 | # # global target_joint_positions 298 | # # if(current_pos_index < len(target_joint_positions) - 1): 299 | # # path = np.linspace(target_joint_positions[current_pos_index], target_joint_positions[current_pos_index + 1], num_steps) 300 | # # current_pos_index += 1 301 | # # else: 302 | # # path = np.linspace([0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], num_steps) 303 | # # current_pos_index = 0 304 | 305 | # # for pos in path: 306 | # # change_pos_dof_target(pos) 307 | # # time.sleep(time_per_pos/num_steps) 308 | 309 | # # print(current_pos_index) 310 | 311 | # # if(key == "1"): 312 | # # for i in range(len(target_joint_positions-1)): 313 | # # go_to_next() 314 | 315 | def joint_monkey(key): 316 | target_joint_positions = np.array([ 317 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 318 | [0, 0, 0, 0, pi*36/180, 0, 0, 0, 0, -pi*36/180], 319 | # joint 0 320 | [-pi/2, 0, 0, 0, pi*36/180, -pi/2, 0, 0, 0, -pi*36/180], 321 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 322 | [pi/2, 0, 0, 0, pi*36/180, pi/2, 0, 0, 0, -pi*36/180], 323 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 324 | # join 1 325 | [0, -pi/2, 0, 0, 0, 0, pi/2, 0, 0, 0], 326 | # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 327 | [0, pi/2, 0, 0, 0, 0, -pi/2, 0, 0, 0], 328 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 329 | # joint 2 330 | [0, 0, -2/9*pi, 0, 0, 0, 0, -2/9*pi, 0, 0,], 331 | [0, 0, 2/9*pi, 0, 0, 0, 0, 2/9*pi, 0, 0,], 332 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 333 | # join 3 334 | [0, 0, 0, pi*5/9, 0, 0, 0, 0, -pi*5/9, 0], 335 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 336 | # joint 4 337 | [0, 0, 0, 0, pi*36/180, 0, 0, 0, 0, -pi*36/180], 338 | [0, 0, 0, 0, -pi*36/180, 0, 0, 0, 0, pi*36/180], 339 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 340 | ]) 341 | 342 | if(key == "1"): 343 | for i in range(len(target_joint_positions)-1): 344 | start=target_joint_positions[i] 345 | end = target_joint_positions[i+1] 346 | print(start.shape,end.shape) 347 | d = np.max(np.abs(start-end)) 348 | v = 0.5 349 | dt = 0.001 350 | duration = d/v 351 | path = np.linspace(start, end, int(duration/dt)) 352 | for pos in path: 353 | change_pos_dof_target(pos) 354 | time.sleep(dt) 355 | 356 | 357 | 358 | def press(key): 359 | 360 | # indvidual_motor_test_passive(key) 361 | 362 | low_vel_friction_test(key) 363 | 364 | # walking_test(key) 365 | 366 | # joint_monkey(key) 367 | 368 | # if key=='9': 369 | # data = { 370 | # "reset":True, 371 | # "id":random.randint(-sys.maxsize, sys.maxsize) 372 | # } 373 | # # reset 374 | # publish(data) 375 | # print("9 pressed") 376 | 377 | listen_keyboard(on_press=press,delay_second_char=0.001,delay_other_chars=0.005,sleep=0.0001) 378 | 379 | -------------------------------------------------------------------------------- /teensy_comm/teensy_comm.ino: -------------------------------------------------------------------------------- 1 | // Author: Michael Scutari 2 | // Description: Read code from LORD Microstrain 3DM-CV7 IMU and FX29 Load Cell. Send data over serial with COBS encoding. 3 | // © 2024 General Robotics Lab, Thomas Lord Department of Mechanical Engineering, Duke University 4 | // License: MIT License 5 | 6 | // #define DEBUG 7 | // modified 8/11 8 | #define PACKETIZER_USE_INDEX_AS_DEFAULT 9 | #define PACKETIZER_USE_CRC_AS_DEFAULT 10 | 11 | // must be before the import of Packetizer 12 | #include // I2C 13 | #include // I2C multiplexer 14 | #include // serial with computer 15 | #include 16 | 17 | // I2C addresses 18 | #define LOADCELL_ADDR 0x28 19 | #define MULTIPLEXER_ADDR 0x70 20 | 21 | constexpr float GRAVITY = 9.80665; 22 | 23 | // CONSTANTS 24 | // IMU 25 | const int bufferSize = 256; 26 | byte buffer[bufferSize]; 27 | int bufferIndex = 0; 28 | bool ackReceived = false; // IMU acknowledgment for commands 29 | unsigned long commandTimeout = 1000; 30 | 31 | // Filter and sensor sample rates 32 | const uint16_t sensor_sample_rate = 500; // Hz 33 | const uint16_t filter_sample_rate = 500; // Hz 34 | 35 | //*--------------force sensor--------------------------------------------------------- 36 | // 37 | bool sampleForceSensors = true; 38 | 39 | // multiplexer 40 | PCA9548 multiplexer(MULTIPLEXER_ADDR); 41 | 42 | // sensor timing and data collection. 43 | unsigned long previousForceSensorCollectionTime = 0; // ms 44 | const long forceSensorCollectionInterval = 5; // ms //typical 5 ms 45 | //*--------------------------------------------------------------------------- 46 | 47 | // Packetizer 48 | // send and recv index 49 | const uint8_t recv_index = 0x12; 50 | const uint8_t send_index = 0x34; 51 | 52 | // Finite state machine 53 | enum State 54 | { 55 | WAIT_FOR_U, 56 | WAIT_FOR_E, 57 | WAIT_FOR_DESCRIPTOR, 58 | WAIT_FOR_LENGTH, 59 | READ_PACKET 60 | }; 61 | 62 | union FloatBytes { 63 | float f; 64 | byte b[4]; 65 | }__attribute__((packed)); 66 | 67 | // Data stream types 68 | enum DescriptorSet 69 | { 70 | BASE_COMMAND = 0x01, 71 | _3DM_COMMAND = 0x0C, 72 | FILTER_COMMAND = 0x0D, 73 | SENSOR_DATA = 0x80, 74 | FILTER_DATA = 0x82, 75 | SYSTEM_DATA = 0xA0, 76 | SHARED_DATA = 0xFF 77 | }; 78 | 79 | enum DataFieldDescriptors 80 | { 81 | FILTER_FILTER_STATUS = 0x10, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm 82 | FILTER_ATT_QUATERNION = 0x03, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_quaternion.htm?Highlight=0x03 83 | FILTER_ATT_EULER_ANGLES = 0x05, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/filter_data/data/mip_field_filter_euler_angles.htm 84 | FILTER_ATT_DCM = 0x04, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_dcm.htm 85 | FILTER_LINEAR_ACCEL = 0x0D, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/filter_data/data/mip_field_filter_linear_accel.htm 86 | FILTER_GRAVITY_VECTOR = 0x13, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/filter_data/data/mip_field_filter_gravity_vector.htm 87 | FILTER_ANGULAR_RATE = 0x0E, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/filter_data/data/mip_field_filter_comp_angular_rate.htm?Highlight=0x0E 88 | SENSOR_SCALED_ACCEL = 0x04, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/sensor_data/data/mip_field_sensor_scaled_accel.htm 89 | SENSOR_SCALED_GYRO = 0x05, // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Data/sensor_data/data/mip_field_sensor_scaled_gyro.htm 90 | }; 91 | 92 | enum CommandSendDescriptors 93 | { 94 | COMMAND_PING = 0x01, 95 | COMMAND_BUILT_IN_TEST = 0x05, 96 | COMMAND_GET_BASE_DATA_RATE = 0x0E, 97 | COMMAND_SET_TO_IDLE = 0x02, 98 | COMMAND_DEVICE_SETTINGS = 0x30, 99 | COMMAND_DATASTREAM_CONTROL = 0x11, 100 | COMMAND_DEFAULT_SETTINGS = 0x30, 101 | COMMAND_MESSAGE_FORMAT = 0x0F, 102 | COMMAND_RESUME_SAMPLING = 0x06, 103 | COMMAND_RESET_NAV_FILTER = 0x01, 104 | COMMAND_SET_HEADING = 0x03, 105 | COMMAND_AIDING_MEASUREMENT = 0x50, 106 | COMMAND_VEHICLE_TRANSFORM_EULER = 0x31, 107 | }; 108 | 109 | // Function Selectors // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/mip_common.html?Highlight=function%20selectors 110 | // Most settings commands support a function selector, which allows the setting to be changed, read back, saved, loaded, or reset to the default. Some commands don't support all 5 options. For most settings, no additional parameters are needed when the function selector is not 0x01. However certain commands need more information (for example, which subset of settings to read back). 111 | // 0x01 - Apply new setting 112 | // 0x02 - Read current setting 113 | // 0x03 - Save current setting to non-volatile memory 114 | // 0x04 - Load current setting from non-volatile memory 115 | // 0x05 - Restore factory default setting 116 | enum FunctionSelectors 117 | { 118 | SELECT_WRITE = 0x01, 119 | SELECT_READ = 0x02, 120 | SELECT_SAVE = 0x03, 121 | SELECT_LOAD = 0x04, 122 | SELECT_RESET = 0x05, 123 | }; 124 | 125 | enum CommandReceiveDescriptors 126 | { 127 | RECV_BUILT_IN_TEST = 0x83, 128 | RECV_AIDING_MEASURMENT = 0xD0, 129 | RECV_GET_BASE_DATA_RATE = 0x8E, 130 | RECV_DATASTEAM_CONTROL = 0x85, 131 | }; 132 | 133 | // Structs for storing packet information. 134 | struct PacketField 135 | { 136 | byte descriptor; 137 | byte length; 138 | std::vector data; 139 | }; 140 | 141 | struct IMUPacket 142 | { 143 | byte descriptorSet; 144 | byte length; 145 | std::vector fields; 146 | }; 147 | 148 | // for collecting data in one place and sending 149 | struct CombinedData 150 | { 151 | // load cell measurements 152 | int16_t force_measurement[4]; // hhhh 153 | 154 | // accelerometer data 155 | float lin_acc_raw[3]; // fff 156 | 157 | // gyroscope data 158 | float ang_vel_raw[3]; // fff 159 | 160 | // gravity vector estimation filter 161 | float gravity_vector[3]; // fff 162 | 163 | // // attitude rotation matrix 164 | // float rotation_matrix[9]; // fffffffff 165 | 166 | // quaternion attitude estimation filter 167 | float quaternion[4]; // ffff 168 | 169 | // gravity-compensated linear acceleration 170 | float lin_acc_filtered[3]; // fff 171 | 172 | // gravity-compensated angular velocity 173 | float ang_vel_filtered[3]; // fff 174 | 175 | // filter status, check domumentation 176 | uint16_t filter_status; // h 177 | // dynamic mode. check documentation 178 | uint16_t filter_dynamics_mode; // h 179 | // status flags. processed in python code. 180 | uint16_t filter_status_flags; // h 181 | // check which are fresh. 182 | uint16_t fresh; // h // 00000 (nothing), 00 (accel, gyro), 0000 (gravity vector, rotation matrix, quaternion, linear accel), 0 (status, dynamics mode, status flag), 0000 (force sensor 3, 2, 1, 0) 183 | } __attribute__((packed)); 184 | 185 | // Packetizer combined data object 186 | CombinedData combinedData; 187 | 188 | // IMU current packet information 189 | State currentState = WAIT_FOR_U; 190 | byte descriptor; 191 | int payload_length; 192 | 193 | //////////////////////////////////// 194 | // IMU variables (updated by IMU) // 195 | //////////////////////////////////// 196 | 197 | uint16_t filter_base_rate = 0; 198 | uint16_t sensor_base_rate = 0; 199 | 200 | // verifying channels are activate when enabling datastream 201 | bool sensor_channel_active = false; 202 | bool filter_channel_active = false; 203 | bool aiding_measurements_active = true; 204 | 205 | // for timing 206 | unsigned long startTime; 207 | 208 | void setup() 209 | { 210 | Serial.begin(1500000); 211 | Serial1.begin(921600); 212 | Wire.begin(); 213 | Wire.setClock(921600UL); 214 | 215 | // start all data as not fresh 216 | combinedData.fresh = 0; 217 | 218 | // Initialize the IMU with default settings. 219 | if (setToIdle()) 220 | { 221 | Serial.println("Successfully set to idle."); 222 | } 223 | else 224 | { 225 | Serial.println("Failed to set to idle."); 226 | } 227 | 228 | Serial.println(); 229 | 230 | if (loadDefaultSettings()) 231 | { 232 | Serial.println("Successfully loaded default settings."); 233 | } 234 | else 235 | { 236 | Serial.println("Failed to load default settings."); 237 | } 238 | 239 | Serial.println(); 240 | 241 | // Configure sensor data. 242 | if (getDataBaseRate(SENSOR_DATA)) 243 | { 244 | Serial.print("Sensor base data rate: "); 245 | Serial.println(sensor_base_rate); 246 | } 247 | else 248 | { 249 | Serial.println("Failed to get base data rate."); 250 | } 251 | 252 | Serial.println(); 253 | 254 | const uint16_t sensor_decimation = sensor_base_rate / sensor_sample_rate; 255 | 256 | // Set Message Format 257 | std::vector> sensorDescriptorsAndDecimators = { 258 | {SENSOR_SCALED_ACCEL, sensor_decimation}, // Accelerometer 259 | {SENSOR_SCALED_GYRO, sensor_decimation}, // Gyroscope 260 | // {0x0E, sensor_decimation} // Magnetometer 261 | }; 262 | 263 | if (setMessageFormat(SENSOR_DATA, sensorDescriptorsAndDecimators)) 264 | { 265 | Serial.println("Sensor data message format successfully updated."); 266 | } 267 | else 268 | { 269 | Serial.println("Failed to set message format."); 270 | } 271 | 272 | Serial.println(); 273 | 274 | if (enableDatastream(SENSOR_DATA)) 275 | { 276 | Serial.println("Successfully enabled sensor data."); 277 | } 278 | else 279 | { 280 | Serial.println("Failed to enable sensor datastream."); 281 | } 282 | 283 | // if (disableDatastream(SENSOR_DATA)) // HACK disble sensor data 284 | // { 285 | // Serial.println("Successfully disabled sensor data."); 286 | // } 287 | // else 288 | // { 289 | // Serial.println("Failed to disabled sensor datastream."); 290 | // } 291 | 292 | Serial.println(); 293 | 294 | if (verifyDatastream(SENSOR_DATA)) 295 | { 296 | Serial.println("Verified: Sensor datastream enabled."); 297 | } 298 | else 299 | { 300 | Serial.println("Verification failed: Sensor data not available."); 301 | } 302 | 303 | Serial.println(); 304 | 305 | // Configure filter data. 306 | 307 | if (resetNavigationFilter()) 308 | { 309 | Serial.println("Navigation filter reset successfully."); 310 | } 311 | else 312 | { 313 | Serial.println("Failed to reset navigation filter."); 314 | } 315 | 316 | if (setHeadingControl()) 317 | { // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Commands/filter_command/data/mip_filter_set_initial_heading.htm 318 | Serial.println("Heading control set successfully."); 319 | } 320 | else 321 | { 322 | Serial.println("Failed to set heading control."); 323 | } 324 | 325 | if (loadAllAidingMeasurement()) 326 | { // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Commands/filter_command/data/mip_filter_aiding_measurement_enable.htm 327 | Serial.println("Loaded all aiding mesurements successfully."); 328 | } 329 | else 330 | { 331 | Serial.println("Failed to load all aiding measurements."); 332 | } 333 | 334 | Serial.println(); 335 | 336 | if (sensorToVehicleTransform(0.0, 0.0, 0.0)) 337 | { 338 | Serial.println("Sensor to vehicle transform successfully updated."); 339 | } 340 | else 341 | { 342 | Serial.println("Failed to updated sensor to vehicle transform."); 343 | } 344 | 345 | Serial.println(); 346 | 347 | if (readSensorToVehicleTransform()) 348 | { 349 | Serial.println("Reading sensor to vehicle transform."); 350 | } 351 | else 352 | { 353 | Serial.println("Failed to updated sensor to vehicle transform."); 354 | } 355 | 356 | if (getDataBaseRate(FILTER_DATA)) 357 | { 358 | Serial.print("Filter base data rate: "); 359 | Serial.println(filter_base_rate); 360 | } 361 | else 362 | { 363 | Serial.println("Failed to get base data rate."); 364 | } 365 | 366 | Serial.println(); 367 | 368 | const uint16_t filter_decimation = filter_base_rate / filter_sample_rate; 369 | 370 | // Set Message Format 371 | std::vector> filterDescriptorsAndDecimators = { 372 | {FILTER_ATT_QUATERNION, filter_decimation}, // Attitude quaternion 373 | {FILTER_FILTER_STATUS, filter_decimation}, // Filter Status 374 | {FILTER_GRAVITY_VECTOR, filter_decimation}, // Gravity vector 375 | // {FILTER_ATT_DCM, filter_decimation}, // Attitude rotation matrix 376 | {FILTER_LINEAR_ACCEL, filter_decimation}, // Gravity-compensated linear acceleration, 377 | {FILTER_ANGULAR_RATE, filter_decimation}, // Filter-compensated gyro 378 | }; 379 | 380 | if (setMessageFormat(FILTER_DATA, filterDescriptorsAndDecimators)) 381 | { 382 | Serial.println("Filter data message format successfully updated."); 383 | } 384 | else 385 | { 386 | Serial.println("Failed to set message format."); 387 | } 388 | 389 | Serial.println(); 390 | 391 | if (enableDatastream(FILTER_DATA)) 392 | { 393 | Serial.println("Successfully enabled filter data."); 394 | } 395 | else 396 | { 397 | Serial.println("Failed to enable filter datastream."); 398 | } 399 | 400 | Serial.println(); 401 | 402 | if (verifyDatastream(FILTER_DATA)) 403 | { 404 | Serial.println("Verified: Filter datastream enabled."); 405 | } 406 | else 407 | { 408 | Serial.println("Verification failed: Filter data not available."); 409 | } 410 | 411 | Serial.println(); 412 | 413 | startTime = millis(); 414 | 415 | if (resumeSampling()) 416 | { 417 | Serial.println("IMU sampling resumed."); 418 | } 419 | else 420 | { 421 | Serial.println("Failed to resume IMU sampling."); 422 | } 423 | } 424 | 425 | void loop() 426 | { 427 | unsigned long currentMillis = millis(); 428 | 429 | // Read force sensor data at regular intervals 430 | if ((currentMillis - previousForceSensorCollectionTime >= forceSensorCollectionInterval) && sampleForceSensors) 431 | { 432 | previousForceSensorCollectionTime = currentMillis; 433 | for (int i = 0; i < 4; i++) 434 | { 435 | readLoadCell(i); 436 | } 437 | } 438 | 439 | noInterrupts(); // Disable interrupts 440 | if (shouldSendPacket()) { 441 | 442 | uint8_t* data = reinterpret_cast(&combinedData); 443 | Packetizer::send(Serial, send_index, data, sizeof(combinedData)); 444 | combinedData.fresh = 0; 445 | } 446 | interrupts(); // Re-enable interrupts 447 | } 448 | 449 | // Interrupt on receiving data with finite state machine. 450 | void serialEvent1() 451 | { 452 | while (Serial1.available()) 453 | { 454 | byte incomingByte = Serial1.read(); 455 | 456 | // Serial.println(incomingByte, HEX); // DEBUG 457 | 458 | switch (currentState) 459 | { 460 | case WAIT_FOR_U: 461 | if (incomingByte == 'u') 462 | { 463 | bufferIndex = 0; 464 | buffer[bufferIndex++] = incomingByte; 465 | currentState = WAIT_FOR_E; 466 | } 467 | break; 468 | 469 | case WAIT_FOR_E: 470 | if (incomingByte == 'e') 471 | { 472 | buffer[bufferIndex++] = incomingByte; 473 | currentState = WAIT_FOR_DESCRIPTOR; 474 | } 475 | else 476 | { 477 | currentState = WAIT_FOR_U; 478 | bufferIndex = 0; 479 | } 480 | break; 481 | 482 | case WAIT_FOR_DESCRIPTOR: 483 | buffer[bufferIndex++] = incomingByte; 484 | descriptor = incomingByte; 485 | currentState = WAIT_FOR_LENGTH; 486 | break; 487 | 488 | case WAIT_FOR_LENGTH: 489 | buffer[bufferIndex++] = incomingByte; 490 | payload_length = (int)incomingByte; 491 | if (payload_length > bufferSize - 6) 492 | { // Check if length is valid 493 | Serial.println("Invalid packet length"); 494 | currentState = WAIT_FOR_U; 495 | bufferIndex = 0; 496 | } 497 | else 498 | { 499 | currentState = READ_PACKET; 500 | } 501 | break; 502 | 503 | case READ_PACKET: 504 | if (bufferIndex < bufferSize) 505 | { 506 | buffer[bufferIndex++] = incomingByte; 507 | if (bufferIndex >= payload_length + 6) 508 | { 509 | processPacket(buffer, payload_length + 6); // Process the complete packet 510 | currentState = WAIT_FOR_U; 511 | } 512 | } 513 | else 514 | { 515 | Serial.println("Buffer overflow detected"); 516 | currentState = WAIT_FOR_U; 517 | } 518 | break; 519 | } 520 | } 521 | } 522 | 523 | void processPacket(byte *packet, int packet_length) 524 | { 525 | if (!verifyChecksum(packet, packet_length)) 526 | { 527 | Serial.println("Bad checksum."); 528 | ackReceived = false; 529 | return; 530 | } 531 | 532 | IMUPacket response_packet = parsePacket(packet, packet_length); 533 | 534 | // Handle different data and command response packets. 535 | if (response_packet.descriptorSet == BASE_COMMAND) 536 | { 537 | 538 | handleBaseCommand(response_packet); 539 | } 540 | else if (response_packet.descriptorSet == _3DM_COMMAND) 541 | { 542 | 543 | handle3DMCommand(response_packet); 544 | } 545 | else if (response_packet.descriptorSet == SENSOR_DATA) 546 | { 547 | 548 | handleSensorData(response_packet); 549 | } 550 | else if (response_packet.descriptorSet == FILTER_COMMAND) 551 | { 552 | 553 | handleFilterCommand(response_packet); 554 | } 555 | else if (response_packet.descriptorSet == FILTER_DATA) 556 | { 557 | 558 | handleFilterData(response_packet); 559 | } 560 | else 561 | { 562 | 563 | Serial.print("ERROR - Unrecognized packet descriptor set: "); 564 | Serial.println(response_packet.descriptorSet, HEX); 565 | } 566 | } 567 | 568 | void handleBaseCommand(IMUPacket packet) 569 | { 570 | ackReceived = fieldsContainACK(packet); 571 | for (const auto &field : packet.fields) 572 | { 573 | if (field.descriptor == COMMAND_BUILT_IN_TEST && field.data.size() == 6) 574 | { 575 | for (byte data : field.data) 576 | { 577 | printBinaryWithLeadingZeros(data); 578 | } 579 | Serial.println(); 580 | } 581 | } 582 | } 583 | 584 | void handle3DMCommand(IMUPacket packet) 585 | { 586 | ackReceived = fieldsContainACK(packet); 587 | 588 | if (!ackReceived) 589 | { 590 | return; 591 | } 592 | 593 | for (const auto &field : packet.fields) 594 | { 595 | 596 | if (field.descriptor == RECV_GET_BASE_DATA_RATE && field.data.size() == 3) 597 | { // getBaseDataRate 598 | if (field.data[0] == FILTER_DATA) 599 | { 600 | filter_base_rate = (field.data[1] << 8) | field.data[2]; 601 | } 602 | else if (field.data[0] == SENSOR_DATA) 603 | { 604 | sensor_base_rate = (field.data[1] << 8) | field.data[2]; 605 | } 606 | } 607 | else if (field.descriptor == RECV_DATASTEAM_CONTROL) 608 | { // Datastream control 609 | 610 | if (field.data[0] == SENSOR_DATA && field.data.size() == 2) 611 | { 612 | sensor_channel_active = field.data[1]; 613 | } 614 | else if (field.data[1] == FILTER_DATA && field.data.size() == 2) 615 | { 616 | filter_channel_active = field.data[1]; 617 | } 618 | } 619 | } 620 | } 621 | 622 | void handleFilterCommand(IMUPacket packet) 623 | { 624 | ackReceived = fieldsContainACK(packet); 625 | 626 | if (!ackReceived) 627 | { 628 | return; 629 | } 630 | 631 | // Serial.println("Received filter command."); 632 | 633 | for (const auto &field : packet.fields) 634 | { 635 | if (field.descriptor == RECV_AIDING_MEASURMENT && field.data.size() == 3) 636 | { 637 | if (field.data[0] == 0xFF && field.data[1] == 0xFF) 638 | { 639 | if (field.data[2] == 0x01) 640 | { 641 | aiding_measurements_active = true; 642 | } 643 | else 644 | { 645 | aiding_measurements_active = false; 646 | } 647 | } 648 | } 649 | } 650 | } 651 | 652 | // inline void convertAndStoreFloats(const std::vector& data, float* destination, int numFloats) { 653 | // FloatBytes fb; 654 | // for (int i = 0; i < numFloats; ++i) { 655 | // // Copy bytes into the union, assuming big-endian order in data 656 | // fb.b[0] = data[i * 4 + 3]; 657 | // fb.b[1] = data[i * 4 + 2]; 658 | // fb.b[2] = data[i * 4 + 1]; 659 | // fb.b[3] = data[i * 4]; 660 | 661 | // // Access the float value, now in the correct endianness 662 | // destination[i] = fb.f; 663 | // } 664 | // } 665 | 666 | void handleFilterData(IMUPacket packet) 667 | { 668 | for (const auto &field : packet.fields) 669 | { // linear acceleration 670 | if (field.descriptor == FILTER_LINEAR_ACCEL && field.length == 0x10) 671 | { 672 | 673 | bool valid = (bool)((field.data[12] << 8) | field.data[13]); 674 | if (valid) 675 | { 676 | // byte x[] = {field.data[3], field.data[2], field.data[1], field.data[0]}; 677 | // byte y[] = {field.data[7], field.data[6], field.data[5], field.data[4]}; 678 | // byte z[] = {field.data[11], field.data[10], field.data[9], field.data[8]}; 679 | // memcpy(&combinedData.lin_acc_filtered[0], x, sizeof(float)); 680 | // memcpy(&combinedData.lin_acc_filtered[1], y, sizeof(float)); 681 | // memcpy(&combinedData.lin_acc_filtered[2], z, sizeof(float)); 682 | 683 | 684 | // Reinterpret the entire gravity_vector array as an array of FloatBytes 685 | FloatBytes* fb = reinterpret_cast(combinedData.lin_acc_filtered); 686 | for (int i = 0; i < 3; ++i) { 687 | // Copy bytes, assuming big-endian order in field.data 688 | fb[i].b[0] = field.data[i * 4 + 3]; 689 | fb[i].b[1] = field.data[i * 4 + 2]; 690 | fb[i].b[2] = field.data[i * 4 + 1]; 691 | fb[i].b[3] = field.data[i * 4]; 692 | } 693 | // negate y and z 694 | // combinedData.lin_acc_filtered[0] = combinedData.lin_acc_filtered[0]; 695 | combinedData.lin_acc_filtered[1] = -combinedData.lin_acc_filtered[1]; 696 | combinedData.lin_acc_filtered[2] = -combinedData.lin_acc_filtered[2]; 697 | 698 | 699 | combinedData.fresh |= (1 << 5); 700 | } 701 | } 702 | // else if (field.descriptor == FILTER_ATT_DCM && field.length == 0x28) 703 | // { 704 | // bool valid = (bool)((field.data[36] << 8) | field.data[37]); 705 | // if (valid) 706 | // { 707 | // // // rotation matrix 708 | // FloatBytes fb[9]; 709 | // for (int i = 0; i < 9; ++i) { 710 | // // Copy bytes into the union, assuming big-endian order in field.data 711 | // fb[i].b[0] = field.data[i * 4 + 3]; 712 | // fb[i].b[1] = field.data[i * 4 + 2]; 713 | // fb[i].b[2] = field.data[i * 4 + 1]; 714 | // fb[i].b[3] = field.data[i * 4]; 715 | // } 716 | // combinedData.rotation_matrix[0] = fb[0].f; 717 | // combinedData.rotation_matrix[1] = -fb[3].f; 718 | // combinedData.rotation_matrix[2] = -fb[6].f; 719 | // combinedData.rotation_matrix[3] = -fb[1].f; 720 | // combinedData.rotation_matrix[4] = fb[4].f; 721 | // combinedData.rotation_matrix[5] = fb[7].f; 722 | // combinedData.rotation_matrix[6] = -fb[2].f; 723 | // combinedData.rotation_matrix[7] = fb[5].f; 724 | // combinedData.rotation_matrix[8] = fb[8].f; 725 | 726 | // combinedData.fresh |= (1 << 7); 727 | // } 728 | // } 729 | else if (field.descriptor == FILTER_GRAVITY_VECTOR && field.length == 0x10) 730 | { // gravity vector 731 | 732 | bool valid = (bool)((field.data[12] << 8) | field.data[13]); 733 | if (valid) 734 | { 735 | // byte x[] = {field.data[3], field.data[2], field.data[1], field.data[0]}; 736 | // byte y[] = {field.data[7], field.data[6], field.data[5], field.data[4]}; 737 | // byte z[] = {field.data[11], field.data[10], field.data[9], field.data[8]}; 738 | // memcpy(&combinedData.gravity_vector[0], x, sizeof(float)); 739 | // memcpy(&combinedData.gravity_vector[1], y, sizeof(float)); 740 | // memcpy(&combinedData.gravity_vector[2], z, sizeof(float)); 741 | 742 | // FloatBytes fb[3]; 743 | // for (int i = 0; i < 3; ++i) { 744 | // // Copy bytes into the union, assuming big-endian order in field.data 745 | // fb[i].b[0] = field.data[i * 4 + 3]; 746 | // fb[i].b[1] = field.data[i * 4 + 2]; 747 | // fb[i].b[2] = field.data[i * 4 + 1]; 748 | // fb[i].b[3] = field.data[i * 4]; 749 | // // Access the float value, now in the correct endianness 750 | // combinedData.gravity_vector[i] = fb[i].f/GRAVITY; // normalize to [g] 751 | // } 752 | 753 | // Reinterpret the entire gravity_vector array as an array of FloatBytes 754 | FloatBytes* fb = reinterpret_cast(combinedData.gravity_vector); 755 | for (int i = 0; i < 3; ++i) { 756 | // Copy bytes, assuming big-endian order in field.data 757 | fb[i].b[0] = field.data[i * 4 + 3]; 758 | fb[i].b[1] = field.data[i * 4 + 2]; 759 | fb[i].b[2] = field.data[i * 4 + 1]; 760 | fb[i].b[3] = field.data[i * 4]; 761 | // Access and normalize the float value in place 762 | // fb[i].f /= GRAVITY; 763 | } 764 | combinedData.gravity_vector[0] = - combinedData.gravity_vector[0]/GRAVITY; // negate x 765 | combinedData.gravity_vector[1] = combinedData.gravity_vector[1]/GRAVITY; 766 | combinedData.gravity_vector[2] = combinedData.gravity_vector[2]/GRAVITY; 767 | 768 | 769 | combinedData.fresh |= (1 << 8); 770 | } 771 | } 772 | else if (field.descriptor == FILTER_ATT_QUATERNION && field.length == 0x14) 773 | { // quaternion 774 | 775 | bool valid = (bool)((field.data[16] << 8) | field.data[17]); 776 | 777 | if (valid) 778 | { 779 | // byte w[] = {field.data[3], field.data[2], field.data[1], field.data[0]}; 780 | // byte x[] = {field.data[7], field.data[6], field.data[5], field.data[4]}; 781 | // byte y[] = {field.data[11], field.data[10], field.data[9], field.data[8]}; 782 | // byte z[] = {field.data[15], field.data[14], field.data[13], field.data[12]}; 783 | 784 | // memcpy(&combinedData.quaternion[0], w, sizeof(float)); 785 | // memcpy(&combinedData.quaternion[1], x, sizeof(float)); 786 | // memcpy(&combinedData.quaternion[2], y, sizeof(float)); 787 | // memcpy(&combinedData.quaternion[3], z, sizeof(float)); 788 | 789 | 790 | FloatBytes fb[4]; 791 | for (int i = 0; i < 4; ++i) { 792 | // Copy bytes into the union, assuming big-endian order in field.data 793 | fb[i].b[0] = field.data[i * 4 + 3]; 794 | fb[i].b[1] = field.data[i * 4 + 2]; 795 | fb[i].b[2] = field.data[i * 4 + 1]; 796 | fb[i].b[3] = field.data[i * 4]; 797 | } 798 | // fb in wzyz order, need to convert to xyzw and negate y and z 799 | combinedData.quaternion[0] = fb[1].f; 800 | combinedData.quaternion[1] = - fb[2].f; 801 | combinedData.quaternion[2] = - fb[3].f; 802 | combinedData.quaternion[3] = fb[0].f; 803 | 804 | 805 | combinedData.fresh |= (1 << 6); 806 | } 807 | } 808 | // else if (field.descriptor == FILTER_ATT_EULER_ANGLES && field.data.size() == 14) { // euler angles 809 | 810 | // byte x[] = { field.data[3], field.data[2], field.data[1], field.data[0] }; 811 | // byte y[] = { field.data[7], field.data[6], field.data[5], field.data[4] }; 812 | // byte z[] = { field.data[11], field.data[10], field.data[9], field.data[8] }; 813 | 814 | // bool valid = (bool)((field.data[12] << 8) | field.data[13]); 815 | 816 | // if (valid) { 817 | // Serial.println("ERROR: Operating in quaternion mode."); 818 | // } 819 | 820 | // } 821 | else if (field.descriptor == FILTER_FILTER_STATUS && field.data.size() == 6) 822 | { // filter status 823 | 824 | combinedData.filter_status = (field.data[0] << 8) | field.data[1]; 825 | combinedData.filter_dynamics_mode = (field.data[2] << 8) | field.data[3]; 826 | combinedData.filter_status_flags = (field.data[4] << 8) | field.data[5]; 827 | 828 | combinedData.fresh |= (1 << 4); 829 | } 830 | else if (field.descriptor == FILTER_ANGULAR_RATE && field.length == 0x10) 831 | { // scaled accelerometer values 832 | 833 | // byte x[] = {field.data[3], field.data[2], field.data[1], field.data[0]}; 834 | // byte y[] = {field.data[7], field.data[6], field.data[5], field.data[4]}; 835 | // byte z[] = {field.data[11], field.data[10], field.data[9], field.data[8]}; 836 | 837 | // memcpy(&combinedData.ang_vel_filtered[0], x, sizeof(float)); 838 | // memcpy(&combinedData.ang_vel_filtered[1], y, sizeof(float)); 839 | // memcpy(&combinedData.ang_vel_filtered[2], z, sizeof(float)); 840 | 841 | 842 | // Reinterpret the entire gravity_vector array as an array of FloatBytes 843 | FloatBytes* fb = reinterpret_cast(combinedData.ang_vel_filtered); 844 | for (int i = 0; i < 3; ++i) { 845 | // Copy bytes, assuming big-endian order in field.data 846 | fb[i].b[0] = field.data[i * 4 + 3]; 847 | fb[i].b[1] = field.data[i * 4 + 2]; 848 | fb[i].b[2] = field.data[i * 4 + 1]; 849 | fb[i].b[3] = field.data[i * 4]; 850 | } 851 | // negate y and z 852 | // combinedData.ang_vel_filtered[0] = combinedData.ang_vel_filtered[0]; 853 | combinedData.ang_vel_filtered[1] = -combinedData.ang_vel_filtered[1]; 854 | combinedData.ang_vel_filtered[2] = -combinedData.ang_vel_filtered[2]; 855 | 856 | 857 | combinedData.fresh |= (1 << 10); 858 | } 859 | 860 | else 861 | { 862 | Serial.print("Unrecognized data type! "); 863 | Serial.print("Descriptor: 0x"); 864 | Serial.print(field.descriptor, HEX); 865 | Serial.print(", Length: 0x"); 866 | Serial.println(field.length, HEX); 867 | } 868 | } 869 | } 870 | 871 | void handleSensorData(IMUPacket packet) 872 | { 873 | for (const auto &field : packet.fields) 874 | { 875 | if (field.descriptor == SENSOR_SCALED_ACCEL && field.data.size() == 12) 876 | { // scaled accelerometer values 877 | 878 | // byte x[] = {field.data[3], field.data[2], field.data[1], field.data[0]}; 879 | // byte y[] = {field.data[7], field.data[6], field.data[5], field.data[4]}; 880 | // byte z[] = {field.data[11], field.data[10], field.data[9], field.data[8]}; 881 | // memcpy(&combinedData.lin_acc_raw[0], x, sizeof(float)); 882 | // memcpy(&combinedData.lin_acc_raw[1], y, sizeof(float)); 883 | // memcpy(&combinedData.lin_acc_raw[2], z, sizeof(float)); 884 | // // convert unit from [g] to [m/s^2] 885 | // combinedData.lin_acc_raw[0]*=GRAVITY; 886 | // combinedData.lin_acc_raw[1]*=GRAVITY; 887 | // combinedData.lin_acc_raw[2]*=GRAVITY; 888 | 889 | // FloatBytes fb; 890 | // for (int i = 0; i < 3; ++i) { 891 | // // Copy bytes into the union, assuming big-endian order in field.data 892 | // fb.b[0] = field.data[i * 4 + 3]; 893 | // fb.b[1] = field.data[i * 4 + 2]; 894 | // fb.b[2] = field.data[i * 4 + 1]; 895 | // fb.b[3] = field.data[i * 4]; 896 | // // Access the float value, now in the correct endianness 897 | // combinedData.lin_acc_raw[i] = fb.f*GRAVITY; 898 | // } 899 | 900 | // Reinterpret the entire gravity_vector array as an array of FloatBytes 901 | FloatBytes* fb = reinterpret_cast(combinedData.lin_acc_raw); 902 | for (int i = 0; i < 3; ++i) { 903 | // Copy bytes, assuming big-endian order in field.data 904 | fb[i].b[0] = field.data[i * 4 + 3]; 905 | fb[i].b[1] = field.data[i * 4 + 2]; 906 | fb[i].b[2] = field.data[i * 4 + 1]; 907 | fb[i].b[3] = field.data[i * 4]; 908 | // Access and normalize the float value in place 909 | // fb[i].f /= GRAVITY; 910 | } 911 | // negate y and z 912 | combinedData.lin_acc_raw[0] = combinedData.lin_acc_raw[0]*GRAVITY; 913 | combinedData.lin_acc_raw[1] = -combinedData.lin_acc_raw[1]*GRAVITY; 914 | combinedData.lin_acc_raw[2] = -combinedData.lin_acc_raw[2]*GRAVITY; 915 | 916 | 917 | combinedData.fresh |= (1 << 10); 918 | } 919 | else if (field.descriptor == SENSOR_SCALED_GYRO && field.data.size() == 12) 920 | { // gyroscope 921 | 922 | // byte x[] = {field.data[3], field.data[2], field.data[1], field.data[0]}; 923 | // byte y[] = {field.data[7], field.data[6], field.data[5], field.data[4]}; 924 | // byte z[] = {field.data[11], field.data[10], field.data[9], field.data[8]}; 925 | 926 | // memcpy(&combinedData.ang_vel_raw[0], x, sizeof(float)); 927 | // memcpy(&combinedData.ang_vel_raw[1], y, sizeof(float)); 928 | // memcpy(&combinedData.ang_vel_raw[2], z, sizeof(float)); 929 | 930 | // FloatBytes fb; 931 | // for (int i = 0; i < 3; ++i) { 932 | // // Copy bytes into the union, assuming big-endian order in field.data 933 | // fb.b[0] = field.data[i * 4 + 3]; 934 | // fb.b[1] = field.data[i * 4 + 2]; 935 | // fb.b[2] = field.data[i * 4 + 1]; 936 | // fb.b[3] = field.data[i * 4]; 937 | // // Access the float value, now in the correct endianness 938 | // combinedData.ang_vel_raw[i] = fb.f; 939 | // } 940 | 941 | // Reinterpret the entire gravity_vector array as an array of FloatBytes 942 | FloatBytes* fb = reinterpret_cast(combinedData.ang_vel_raw); 943 | for (int i = 0; i < 3; ++i) { 944 | // Copy bytes, assuming big-endian order in field.data 945 | fb[i].b[0] = field.data[i * 4 + 3]; 946 | fb[i].b[1] = field.data[i * 4 + 2]; 947 | fb[i].b[2] = field.data[i * 4 + 1]; 948 | fb[i].b[3] = field.data[i * 4]; 949 | } 950 | // negate y and z 951 | // combinedData.ang_vel_raw[0] = combinedData.ang_vel_raw[0]; 952 | combinedData.ang_vel_raw[1] = -combinedData.ang_vel_raw[1]; 953 | combinedData.ang_vel_raw[2] = -combinedData.ang_vel_raw[2]; 954 | 955 | 956 | 957 | // convertAndStoreFloats(field.data, combinedData.ang_vel_raw, 3); 958 | 959 | combinedData.fresh |= (1 << 9); 960 | } 961 | else 962 | { 963 | Serial.println("Unrecognized data type."); 964 | } 965 | } 966 | } 967 | 968 | // Code for parsing buffer into IMUPacket struct. 969 | IMUPacket parsePacket(byte *buffer, int length) 970 | { 971 | IMUPacket packet; 972 | packet.descriptorSet = buffer[2]; 973 | packet.length = buffer[3]; 974 | int index = 4; 975 | 976 | #ifdef DEBUG 977 | Serial.println(); 978 | for (int i = 0; i < length; i++) 979 | { 980 | Serial.print(buffer[i], HEX); 981 | Serial.print(" "); 982 | } 983 | Serial.println(); 984 | 985 | Serial.println("Parsing Packet:"); 986 | Serial.print("Descriptor Set: "); 987 | Serial.print(packet.descriptorSet, HEX); 988 | Serial.print(", Length: "); 989 | Serial.println(packet.length, HEX); 990 | #endif 991 | 992 | while (index < length - 2) 993 | { // Subtract 2 for checksum 994 | PacketField field; 995 | field.length = buffer[index++]; 996 | field.descriptor = buffer[index++]; 997 | field.data.assign(buffer + index, buffer + index + field.length - 2); 998 | index += field.length - 2; 999 | packet.fields.push_back(field); 1000 | 1001 | #ifdef DEBUG 1002 | Serial.print("Length: "); 1003 | Serial.print(field.length, HEX); 1004 | Serial.print(", Field Descriptor: "); 1005 | Serial.print(field.descriptor, HEX); 1006 | Serial.print(", Data: "); 1007 | for (byte b : field.data) 1008 | { 1009 | Serial.print(b, HEX); 1010 | Serial.print(" "); 1011 | } 1012 | Serial.println(); 1013 | #endif 1014 | } 1015 | return packet; 1016 | } 1017 | 1018 | // Verify if fletcher checksum is received. 1019 | bool verifyChecksum(byte *packet, int packet_length) 1020 | { 1021 | uint16_t packet_checksum = (packet[packet_length - 2] << 8) | packet[packet_length - 1]; 1022 | uint16_t calculated_checksum = fletcher_checksum(packet, packet_length); 1023 | return packet_checksum == calculated_checksum; 1024 | } 1025 | 1026 | uint16_t fletcher_checksum(const uint8_t *packet, int packet_length) 1027 | { 1028 | const int checksum_length = packet_length - 2; 1029 | uint8_t checksum_MSB = 0; 1030 | uint8_t checksum_LSB = 0; 1031 | for (int i = 0; i < checksum_length; i++) 1032 | { 1033 | checksum_MSB += packet[i]; 1034 | checksum_LSB += checksum_MSB; 1035 | } 1036 | return ((uint16_t)checksum_MSB << 8) | (uint16_t)checksum_LSB; 1037 | } 1038 | 1039 | bool fieldsContainACK(IMUPacket packet) 1040 | { 1041 | for (const auto &field : packet.fields) 1042 | { 1043 | if (field.descriptor == 0xF1) 1044 | { // Example: ACK bits position 1045 | if (field.data[1] == 0x00) 1046 | { 1047 | return true; 1048 | } 1049 | else if (field.data[1] == 0x03) 1050 | { 1051 | Serial.println("Invalid parameter!"); 1052 | } 1053 | } 1054 | } 1055 | return false; 1056 | } 1057 | 1058 | bool checkACK() 1059 | { 1060 | unsigned long startTime = millis(); 1061 | while (millis() - startTime < commandTimeout) 1062 | { 1063 | noInterrupts(); // Disable interrupts 1064 | if (ackReceived) 1065 | { 1066 | ackReceived = false; 1067 | interrupts(); // Re-enable interrupts 1068 | return true; // Acknowledgment received 1069 | } 1070 | interrupts(); // Re-enable interrupts 1071 | yield(); // Yield to other processes 1072 | } 1073 | 1074 | Serial.println("Timeout occurred!"); 1075 | return false; // Timeout occurred 1076 | } 1077 | 1078 | // ADD Checksum to last two bytes of command. 1079 | void addChecksum(byte *command, int command_length) 1080 | { 1081 | uint16_t checksum = fletcher_checksum(command, command_length); 1082 | command[command_length - 2] = (byte)(checksum >> 8); // MSB of checksum 1083 | command[command_length - 1] = (byte)(checksum & 0xFF); // LSB of checksum 1084 | } 1085 | 1086 | // LIST OF POTENTIAL COMMANDS 1087 | bool ping() 1088 | { 1089 | int command_length = 8; 1090 | byte command[command_length] = {0x75, 0x65, BASE_COMMAND, 0x02, 0x02, COMMAND_PING, 0x00, 0x00}; 1091 | 1092 | addChecksum(command, command_length); 1093 | Serial1.write(command, command_length); 1094 | 1095 | return checkACK(); 1096 | } 1097 | 1098 | bool setToIdle() 1099 | { 1100 | int command_length = 8; 1101 | byte command[] = {0x75, 0x65, BASE_COMMAND, 0x02, 0x02, COMMAND_SET_TO_IDLE, 0x00, 0x00}; 1102 | 1103 | addChecksum(command, command_length); 1104 | Serial1.write(command, command_length); 1105 | 1106 | return checkACK(); 1107 | } 1108 | 1109 | bool loadDefaultSettings() 1110 | { 1111 | int command_length = 9; 1112 | byte command[] = {0x75, 0x65, _3DM_COMMAND, 0x03, 0x03, COMMAND_DEFAULT_SETTINGS, SELECT_LOAD, 0x00, 0x00}; 1113 | 1114 | addChecksum(command, command_length); 1115 | Serial1.write(command, command_length); 1116 | 1117 | return checkACK(); 1118 | } 1119 | 1120 | bool getDataBaseRate(byte dataDescriptorSet) 1121 | { 1122 | int command_length = 9; 1123 | byte command[] = {0x75, 0x65, _3DM_COMMAND, 0x03, 0x03, COMMAND_GET_BASE_DATA_RATE, dataDescriptorSet, 0x00, 0x00}; 1124 | 1125 | addChecksum(command, command_length); 1126 | Serial1.write(command, command_length); 1127 | 1128 | return checkACK(); 1129 | } 1130 | 1131 | 1132 | 1133 | // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Commands/3dm_command/data/mip_cmd_3dm_message_format.htm 1134 | bool setMessageFormat(byte descriptorSet, const std::vector> &descriptorsAndDecimators) 1135 | { 1136 | int numDescriptors = descriptorsAndDecimators.size(); 1137 | int command_length = 11 + numDescriptors * 3; 1138 | 1139 | std::vector command(command_length, 0); 1140 | command[0] = 0x75; 1141 | command[1] = 0x65; 1142 | command[2] = _3DM_COMMAND; 1143 | command[3] = 5 + numDescriptors * 3; 1144 | command[4] = 5 + numDescriptors * 3; 1145 | command[5] = COMMAND_MESSAGE_FORMAT; 1146 | command[6] = 0x01; 1147 | command[7] = descriptorSet; 1148 | command[8] = numDescriptors; 1149 | 1150 | for (int i = 0; i < numDescriptors; ++i) 1151 | { 1152 | 1153 | command[9 + i * 3] = descriptorsAndDecimators[i].first; 1154 | command[10 + i * 3] = (byte)(descriptorsAndDecimators[i].second >> 8); 1155 | command[11 + i * 3] = (byte)(descriptorsAndDecimators[i].second & 0xFF); 1156 | } 1157 | 1158 | command[12 + numDescriptors * 3] = 0x00; 1159 | command[13 + numDescriptors * 3] = 0x00; 1160 | 1161 | addChecksum(command.data(), command_length); 1162 | Serial1.write(command.data(), command_length); 1163 | 1164 | return checkACK(); 1165 | } 1166 | 1167 | bool verifyDatastream(byte dataDescriptorSet) 1168 | { // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Commands/3dm_command/data/mip_cmd_3dm_datastream_control.htm 1169 | int command_length = 10; 1170 | byte command[] = {0x75, 0x65, _3DM_COMMAND, 0x04, 0x04, COMMAND_DATASTREAM_CONTROL, SELECT_READ, dataDescriptorSet, 0x00, 0x00}; 1171 | 1172 | addChecksum(command, command_length); 1173 | Serial1.write(command, command_length); 1174 | 1175 | return checkACK(); 1176 | } 1177 | 1178 | bool enableDatastream(byte dataDescriptorSet) 1179 | { 1180 | int command_length = 11; 1181 | byte command[] = {0x75, 0x65, _3DM_COMMAND, 0x05, 0x05, COMMAND_DATASTREAM_CONTROL, SELECT_WRITE, dataDescriptorSet, 0x01, 0x00, 0x00}; 1182 | 1183 | addChecksum(command, command_length); 1184 | Serial1.write(command, command_length); 1185 | 1186 | if (checkACK()) 1187 | { 1188 | if (dataDescriptorSet == SENSOR_DATA) 1189 | { 1190 | sensor_channel_active = true; 1191 | return true; 1192 | } 1193 | else if (dataDescriptorSet == FILTER_DATA) 1194 | { 1195 | filter_channel_active = true; 1196 | return true; 1197 | } 1198 | else 1199 | { 1200 | Serial.println("Unrecognized data descriptor set."); 1201 | return false; 1202 | } 1203 | } 1204 | else 1205 | { 1206 | return false; 1207 | } 1208 | } 1209 | 1210 | bool disableDatastream(byte dataDescriptorSet) 1211 | { 1212 | int command_length = 11; 1213 | byte command[] = {0x75, 0x65, _3DM_COMMAND, 0x05, 0x05, COMMAND_DATASTREAM_CONTROL, SELECT_WRITE, dataDescriptorSet, 0x00, 0x00, 0x00}; 1214 | 1215 | addChecksum(command, command_length); 1216 | Serial1.write(command, command_length); 1217 | 1218 | if (checkACK()) 1219 | { 1220 | if (dataDescriptorSet == SENSOR_DATA) 1221 | { 1222 | sensor_channel_active = false; 1223 | return true; 1224 | } 1225 | else if (dataDescriptorSet == FILTER_DATA) 1226 | { 1227 | filter_channel_active = false; 1228 | return true; 1229 | } 1230 | else 1231 | { 1232 | Serial.println("Unrecognized data descriptor set."); 1233 | return false; 1234 | } 1235 | } 1236 | else 1237 | { 1238 | return false; 1239 | } 1240 | } 1241 | 1242 | bool resumeSampling() 1243 | { 1244 | int command_length = 8; 1245 | byte command[] = {0x75, 0x65, BASE_COMMAND, 0x02, 0x02, COMMAND_RESUME_SAMPLING, 0x00, 0x00}; 1246 | 1247 | addChecksum(command, command_length); 1248 | Serial1.write(command, command_length); 1249 | 1250 | return checkACK(); 1251 | } 1252 | 1253 | bool resetNavigationFilter() 1254 | { 1255 | // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Commands/filter_command/data/mip_filter_reset_filter.htm 1256 | int command_length = 8; 1257 | byte command[] = {0x75, 0x65, FILTER_COMMAND, 0x02, 0x02, COMMAND_RESET_NAV_FILTER, 0x00, 0x00}; 1258 | 1259 | addChecksum(command, command_length); 1260 | Serial1.write(command, command_length); 1261 | 1262 | return checkACK(); 1263 | } 1264 | 1265 | bool setHeadingControl() 1266 | { 1267 | int command_length = 12; 1268 | byte command[] = {0x75, 0x65, FILTER_COMMAND, 0x06, 0x06, COMMAND_SET_HEADING, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // set heading reference to 0 1269 | 1270 | addChecksum(command, command_length); 1271 | Serial1.write(command, command_length); 1272 | 1273 | return checkACK(); 1274 | } 1275 | 1276 | bool loadAllAidingMeasurement() 1277 | { // https://s3.amazonaws.com/files.microstrain.com/CV7+Online/external_content/dcp/Commands/filter_command/data/mip_filter_aiding_measurement_enable.htm 1278 | int command_length = 12; 1279 | byte command[] = {0x75, 0x65, FILTER_COMMAND, 0x06, 0x06, COMMAND_AIDING_MEASUREMENT, SELECT_LOAD, 0xFF, 0xFF, 0x01, 0x00, 0x00}; 1280 | 1281 | addChecksum(command, command_length); 1282 | Serial1.write(command, command_length); 1283 | 1284 | return checkACK(); 1285 | } 1286 | 1287 | bool sensorToVehicleTransform(float x, float y, float z) 1288 | { // Sensor to Vehicle Frame Transformation Euler 1289 | 1290 | // Split float32_ts into bytes and store them in big-endian order 1291 | byte *xBytes = (byte *)&x; 1292 | byte *yBytes = (byte *)&y; 1293 | byte *zBytes = (byte *)&z; 1294 | 1295 | int command_length = 21; 1296 | byte command[] = {0x75, 0x65, _3DM_COMMAND, 0x0F, 0x0F, COMMAND_VEHICLE_TRANSFORM_EULER, SELECT_WRITE, xBytes[3], xBytes[2], xBytes[1], xBytes[0], yBytes[3], yBytes[2], yBytes[1], yBytes[0], zBytes[3], zBytes[2], zBytes[1], zBytes[0], 0x00, 0x00}; 1297 | 1298 | addChecksum(command, command_length); // replaces zeroes at end of command with checksum 1299 | Serial1.write(command, command_length); 1300 | 1301 | return checkACK(); 1302 | } 1303 | 1304 | bool readSensorToVehicleTransform() 1305 | { // Sensor to Vehicle Frame Transformation Euler 1306 | 1307 | int command_length = 9; 1308 | byte command[] = {0x75, 0x65, _3DM_COMMAND, 0x03, 0x03, COMMAND_VEHICLE_TRANSFORM_EULER, SELECT_READ, 0x00, 0x00}; 1309 | 1310 | addChecksum(command, command_length); // replaces zeroes at end of command with checksum 1311 | Serial1.write(command, command_length); 1312 | 1313 | return checkACK(); 1314 | } 1315 | 1316 | bool runBuiltInTest() 1317 | { 1318 | int command_length = 8; 1319 | byte command[] = {0x75, 0x65, BASE_COMMAND, 0x02, 0x02, COMMAND_BUILT_IN_TEST, 0x00, 0x00}; 1320 | 1321 | addChecksum(command, command_length); 1322 | Serial1.write(command, command_length); 1323 | 1324 | return checkACK(); 1325 | } 1326 | 1327 | /////////////////////// 1328 | // LOAD CELL FUNCTIONS 1329 | /////////////////////// 1330 | 1331 | void readLoadCell(int index) 1332 | { 1333 | multiplexer.selectChannel(index); 1334 | Wire.requestFrom(LOADCELL_ADDR, 2); // Request 2 bytes of data 1335 | if (Wire.available() >= 2) 1336 | { 1337 | uint8_t data[2]; 1338 | data[0] = Wire.read(); // First byte of data 1339 | data[1] = Wire.read(); // Second byte of data 1340 | uint8_t bridge_status = data[0] >> 6; // Extract status bits, 2=old data, 0=new data 1341 | 1342 | combinedData.force_measurement[index] = (int16_t)(((data[0] & 0x3F) << 8) | data[1]); // Extract bridge data 1343 | if (bridge_status == 2) 1344 | { 1345 | // 8/20: do nothing 1346 | // // LOADCELL data is stale 1347 | // combinedData.fresh &= ~(1 << index); 1348 | } 1349 | else 1350 | { 1351 | // LOADCELL data is fresh 1352 | combinedData.fresh |= (1 << index); 1353 | } 1354 | } 1355 | else 1356 | { 1357 | // DEBUG 1358 | Serial.print("Loadcell no data: "); 1359 | Serial.println(index); 1360 | } 1361 | } 1362 | 1363 | /////////////////////// 1364 | // Other Functions 1365 | /////////////////////// 1366 | 1367 | bool shouldSendPacket() 1368 | { 1369 | // if load cell data fresh 1370 | uint16_t load_cell_mask = 0b0000000000001111; 1371 | if ((load_cell_mask & combinedData.fresh) == load_cell_mask) 1372 | return true; 1373 | 1374 | // h // 00000 (nothing), 00 (accel, gyro), 0000 (gravity vector, rotation matrix, quaternion, linear accel), 0 (status, dynamics mode, status flag), 0000 (force sensor 3, 2, 1, 0) 1375 | 1376 | // if sensor and filter are data fresh 1377 | // uint16_t filter_and_sensor_mask = 0b0000011111110000; 1378 | uint16_t filter_and_sensor_mask = 0b0000011101110000; // no rotation matrix 1379 | 1380 | if ((filter_and_sensor_mask & combinedData.fresh) == filter_and_sensor_mask) 1381 | return true; 1382 | 1383 | // // if sensor data fresh 1384 | // uint16_t sensor_mask = 0b0000011000000000; 1385 | // if ((sensor_mask & combinedData.fresh) == sensor_mask) return true; 1386 | 1387 | // // if sensor data fresh 1388 | // uint16_t filter_mask = 0b0000000111110000; 1389 | // if ((filter_mask & combinedData.fresh) == filter_mask) return true; 1390 | 1391 | return false; 1392 | } 1393 | 1394 | void printBinaryWithLeadingZeros(byte value) 1395 | { 1396 | for (int i = 15; i >= 0; i--) 1397 | { 1398 | Serial.print(bitRead(value, i)); 1399 | } 1400 | Serial.println(); 1401 | } 1402 | -------------------------------------------------------------------------------- /trajectory_PD_test.py: -------------------------------------------------------------------------------- 1 | # sudo chrt -f 99 $(which python) trajectory_PD_test.py 2 | 3 | from motor_controller import MotorController, CONTROL_MODE 4 | import time 5 | import numpy as np 6 | from publisher import DataPublisher 7 | from publisher import DataReceiver 8 | 9 | pi = np.pi 10 | 11 | 12 | motor = MotorController("enp3s0", CONTROL_MODE.CYCLIC_SYNC_VELOCITY, 20, 1000) 13 | motor.run() 14 | 15 | motor.set_max_torque(np.ones(10)*1000) 16 | # motor.set_max_torque(np.ones(10)*40) 17 | 18 | # motor.position_offset=motor.motor_pos_offset 19 | # motor.set_position_offset(motor.motor_pos_offset) 20 | 21 | motor.kp=np.ones(10)*60 22 | motor.kd=np.ones(10)*5 23 | 24 | # motor.kp=np.ones(10)*120 25 | # motor.kd=np.ones(10)*5 26 | 27 | # motor.kp=np.zeros(10) 28 | # motor.kd=np.zeros(10) 29 | 30 | time.sleep(0.1) 31 | 32 | dt = 1/200 33 | # dt = 1/400 34 | 35 | motor.control_mode_int8 = CONTROL_MODE.CYCLIC_SYNC_TORQUE 36 | 37 | decimation=4 38 | 39 | dof_pos_target = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) 40 | 41 | np.set_printoptions(formatter={'float': '{: 3.2f}'.format}) 42 | 43 | startTime = time.time() 44 | 45 | data_publisher = DataPublisher() 46 | 47 | # Create a receiver instance 48 | receiver = DataReceiver(port=9871, decoding="msgpack",broadcast=True) 49 | # Start continuous receiving in a thread 50 | receiver.receive_continuously() 51 | received_id = 0 52 | 53 | 54 | t = time.time() 55 | 56 | action_is_on = np.ones(10) 57 | 58 | should_publish = False 59 | 60 | for i in range(1000000): 61 | if receiver.data is not None: 62 | if receiver.data_id !=received_id: 63 | received_id = receiver.data_id 64 | if "dof_pos_target" in receiver.data: 65 | dof_pos_target = np.array(receiver.data["dof_pos_target"]) 66 | # print(dof_pos_target) 67 | if "action_is_on" in receiver.data: 68 | action_is_on = np.array(receiver.data["action_is_on"], dtype=np.float64) 69 | if "should_publish" in receiver.data: 70 | should_publish:bool = receiver.data["should_publish"] 71 | if "kp" in receiver.data: 72 | motor.kp = np.array(receiver.data["kp"], dtype=np.float64) 73 | if "kd" in receiver.data: 74 | motor.kd = np.array(receiver.data["kd"], dtype=np.float64) 75 | 76 | for _ in range(decimation): 77 | 78 | motor.use_position_pd = True 79 | motor.target_dof_position = dof_pos_target 80 | motor.torque_multiplier = action_is_on 81 | 82 | data = { 83 | "dof_pos":motor.dof_pos, 84 | "dof_vel":motor.dof_vel, 85 | "dof_vel_raw":motor._dof_vel_raw, 86 | "dof_current":motor.dof_current, 87 | "dof_force":motor.dof_force, 88 | "dof_force_target": motor.target_dof_torque_Nm, 89 | "dof_pos_target": dof_pos_target, 90 | "target_dof_torque_A":motor.target_dof_torque_A, 91 | "target_dof_torque_A_adjusted": motor.target_dof_torque_A_adjusted, 92 | "action_is_on":action_is_on, 93 | "t_ns": time.perf_counter_ns() 94 | # "motor_vel": motor.actual_velocity, 95 | # "motor_vel_measured": motor.actual_velocity_measured, 96 | } 97 | data_publisher.enable = True 98 | data_publisher.publish({"real":data}) 99 | 100 | # time.sleep(dt) 101 | elapsed = time.time() - t 102 | if elapsed < dt: 103 | time.sleep(dt - elapsed) 104 | t = time.time() 105 | 106 | 107 | --------------------------------------------------------------------------------