├── configs └── .gitkeep ├── checkpoints └── .gitkeep ├── requirements.txt ├── berkeley_humanoid_lite_lowlevel ├── __init__.py ├── policy │ ├── __init__.py │ ├── config.py │ ├── gamepad.py │ └── rl_controller.py ├── robot │ ├── __init__.py │ ├── bimanual.py │ ├── humanoid.py │ └── imu.py └── recoil │ ├── __init__.py │ ├── util.py │ ├── can.py │ ├── fixed16.py │ └── core.py ├── scripts ├── stop_can_transports.sh ├── start_can_transports.sh ├── check_connection.py ├── motor │ ├── ping.py │ ├── calibrate_electrical_offset.py │ ├── move_actuator.py │ ├── read_configurations.py │ └── configure_parameter.py ├── test_joystick.py ├── run_idle.py ├── run_locomotion.py ├── test_inference.py ├── test_imu.py ├── calibrate_joints.py └── robot │ ├── read_configurations.py │ └── write_configurations.py ├── ctests ├── test-torch.cpp ├── test-yaml.cpp ├── test-udp.cpp └── test-imu.cpp ├── Makefile ├── csrc ├── metal.h ├── main.cpp ├── imu.h ├── serializer.h ├── consts.h ├── udp.c ├── udp.h ├── socketcan.h ├── loop_function.h ├── imu.cpp ├── motor_controller.h ├── serializer.c ├── socketcan.cpp ├── real_humanoid.h ├── motor_controller_conf.h ├── motor_controller.cpp └── real_humanoid.cpp ├── pyproject.toml ├── CMakeLists.txt ├── README.md ├── .gitignore └── imu └── bno085_arduino └── bno085_arduino.ino /configs/.gitkeep: -------------------------------------------------------------------------------- 1 | store deployed config files here -------------------------------------------------------------------------------- /checkpoints/.gitkeep: -------------------------------------------------------------------------------- 1 | store deployed model checkpoints here -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pyserial 2 | python-can 3 | cc.udp 4 | onnxruntime 5 | inputs 6 | pynput 7 | tables 8 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/policy/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/robot/__init__.py: -------------------------------------------------------------------------------- 1 | from .bimanual import Bimanual 2 | from .humanoid import Humanoid 3 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/recoil/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, -T.K.-. 2 | 3 | from .core import * 4 | from .util import * 5 | -------------------------------------------------------------------------------- /scripts/stop_can_transports.sh: -------------------------------------------------------------------------------- 1 | sudo ip link set can0 down 2 | sudo ip link set can1 down 3 | sudo ip link set can2 down 4 | sudo ip link set can3 down 5 | 6 | -------------------------------------------------------------------------------- /scripts/start_can_transports.sh: -------------------------------------------------------------------------------- 1 | sudo ip link set can0 up type can bitrate 1000000 2 | sudo ip link set can1 up type can bitrate 1000000 3 | sudo ip link set can2 up type can bitrate 1000000 4 | sudo ip link set can3 up type can bitrate 1000000 5 | 6 | -------------------------------------------------------------------------------- /scripts/check_connection.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | from berkeley_humanoid_lite_lowlevel.robot import Humanoid 4 | 5 | 6 | robot = Humanoid() 7 | 8 | robot.check_connection() 9 | 10 | robot.stop() 11 | -------------------------------------------------------------------------------- /ctests/test-torch.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include 4 | #include 5 | 6 | int main() { 7 | torch::Tensor tensor = torch::rand({2, 3}); 8 | std::cout << tensor << std::endl; 9 | } 10 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: build rebuild run clean 2 | 3 | build: 4 | cmake -S ./ -B ./build/ 5 | cmake --build ./build/ --target main 6 | 7 | rebuild: 8 | cmake --build ./build/ --target clean 9 | cmake --build ./build/ --target main 10 | 11 | run: build 12 | ./build/main 13 | 14 | clean: 15 | rm -rf ./build/ -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/recoil/util.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, -T.K.-. 2 | 3 | import argparse 4 | 5 | 6 | def get_args(): 7 | parser = argparse.ArgumentParser() 8 | parser.add_argument("-c", "--channel", help="CAN transport channel", type=str, default="can0") 9 | parser.add_argument("-i", "--id", help="CAN device ID", type=int, default=1) 10 | return parser.parse_args() 11 | -------------------------------------------------------------------------------- /csrc/metal.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #pragma once 4 | 5 | #define SET_BITS(REG, BIT) ((REG) |= (BIT)) 6 | #define CLEAR_BITS(REG, BIT) ((REG) &= ~(BIT)) 7 | #define READ_BITS(REG, BIT) ((REG) & (BIT)) 8 | #define WRITE_BITS(REG, CLEARMASK, SETMASK) ((REG) = ((REG) & ~(CLEARMASK)) | (SETMASK)) 9 | -------------------------------------------------------------------------------- /ctests/test-yaml.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | int main() { 9 | const std::string config_path = "config.yaml"; 10 | 11 | YAML::Node config = YAML::LoadFile(config_path); 12 | 13 | printf("Joint names: %s\n", config["joint_names"][0].as().c_str()); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /scripts/motor/ping.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 4 | 5 | 6 | args = recoil.util.get_args() 7 | bus = recoil.Bus(channel=args.channel, bitrate=1000000) 8 | 9 | device_id = args.id 10 | 11 | status = bus.ping(device_id) 12 | 13 | if status: 14 | print("Motor is online") 15 | else: 16 | print("Motor is offline") 17 | 18 | bus.stop() 19 | -------------------------------------------------------------------------------- /csrc/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include 4 | #include "real_humanoid.h" 5 | 6 | 7 | RealHumanoid humanoid; 8 | 9 | 10 | void handle_keyboard_interrupt(int sig) { 11 | printf("\n
Caught signal %d\n", sig); 12 | humanoid.stop(); 13 | } 14 | 15 | int main() { 16 | printf(" Starting...\n"); 17 | signal(SIGINT, handle_keyboard_interrupt); 18 | 19 | humanoid.run(); 20 | 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /scripts/motor/calibrate_electrical_offset.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import time 4 | 5 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 6 | 7 | 8 | args = recoil.util.get_args() 9 | bus = recoil.Bus(channel=args.channel, bitrate=1000000) 10 | 11 | device_id = args.id 12 | 13 | bus.set_mode(device_id, recoil.Mode.CALIBRATION) 14 | 15 | # the motor should now perform the calibration sequence 16 | 17 | # wait for calibration to finish 18 | time.sleep(20) 19 | 20 | bus.stop() 21 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/recoil/can.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, -T.K.-. 2 | 3 | from .core import DataFrame 4 | 5 | 6 | class CANFrame(DataFrame): 7 | ID_STANDARD = 0 8 | ID_EXTENDED = 1 9 | 10 | DEVICE_ID_MSK = 0x7F 11 | FUNC_ID_POS = 7 12 | FUNC_ID_MSK = 0x0F << FUNC_ID_POS 13 | 14 | def __init__( 15 | self, 16 | device_id: int = 0, 17 | func_id: int | None = None, 18 | size: int = 0, 19 | data: bytes = b"" 20 | ): 21 | super().__init__(device_id, func_id, size, data) 22 | assert self.size <= 8 23 | -------------------------------------------------------------------------------- /scripts/test_joystick.py: -------------------------------------------------------------------------------- 1 | """ 2 | calibrate_joints.py 3 | 4 | Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 5 | 6 | Run this script after each power cycle to calibrate the encoder offset of each joint. 7 | """ 8 | 9 | from berkeley_humanoid_lite_lowlevel.robot import ROBOT 10 | 11 | while True: 12 | print( 13 | ROBOT.command_controller.commands["mode_switch"], 14 | ROBOT.command_controller.commands["velocity_x"], 15 | ROBOT.command_controller.commands["velocity_y"], 16 | ROBOT.command_controller.commands["velocity_yaw"] 17 | ) 18 | -------------------------------------------------------------------------------- /scripts/run_idle.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import numpy as np 4 | from cc.udp import UDP 5 | 6 | from berkeley_humanoid_lite_lowlevel.robot import Humanoid 7 | 8 | 9 | robot = Humanoid() 10 | # udp = UDP(send_addr=("172.28.0.5", 8000)) 11 | 12 | udp = UDP(("0.0.0.0", 11000), ("172.28.0.5", 11000)) 13 | 14 | try: 15 | while True: 16 | acs = np.zeros(12) 17 | obs = robot.step(acs) 18 | udp.send_numpy(obs) 19 | 20 | print(robot.joint_position_measured) 21 | 22 | except KeyboardInterrupt: 23 | robot.stop() 24 | 25 | print("Stopped.") 26 | -------------------------------------------------------------------------------- /csrc/imu.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "serializer.h" 11 | 12 | 13 | #define SYNC_1 0x75 14 | #define SYNC_2 0x65 15 | 16 | 17 | class IMU { 18 | public: 19 | float quaternion[4]; // quaternion, (w, x, y, z) 20 | float angular_velocity[3]; // angular velocity, (x, y, z), rad/s 21 | float gravity_vector[3]; // gravity vector, (x, y, z), m/s^2 22 | 23 | IMU(const char *path, speed_t baudrate); 24 | 25 | ~IMU(); 26 | 27 | ssize_t init(); 28 | 29 | void update_reading(); 30 | 31 | private: 32 | int fd; 33 | const char *path_; 34 | speed_t baudrate_; 35 | }; 36 | 37 | 38 | -------------------------------------------------------------------------------- /csrc/serializer.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #ifndef SERIALIZER_H 4 | #define SERIALIZER_H 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | #define ENCODING_END 0x0A 20 | #define ENCODING_ESC 0x0B 21 | #define ENCODING_ESC_END 0x1A 22 | #define ENCODING_ESC_ESC 0x1B 23 | 24 | int Serial_init(const char *path, speed_t baudrate); 25 | 26 | size_t Serial_receive(int fd, char *buffer); 27 | 28 | void Serial_transmit(int fd, char *buffer, size_t len); 29 | 30 | void Serial_stop(int fd); 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/recoil/fixed16.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, -T.K.-. 2 | 3 | import struct 4 | 5 | 6 | class Fixed16: 7 | """ 8 | A fixed-point 16-bit (Q8.8) number with 8 bits for the integer part and 8 bits for the fractional part. 9 | 10 | The range of the number is [-128.0, 127.9] with a resolution of 1/256. 11 | """ 12 | def __init__(self, value: float): 13 | self.value = max(-128.0, min(127.9, value)) 14 | 15 | def asFloat(self): 16 | return self.value 17 | 18 | def asBytes(self): 19 | int16_val = int(self.value * 256) 20 | return struct.pack(" 4 | #include 5 | #include 6 | #include 7 | 8 | #include "udp.h" 9 | #include "consts.h" 10 | 11 | #define N_OBSERVATIONS 50 12 | #define N_ACTIONS 14 13 | 14 | #define FREQUENCY 100 15 | 16 | 17 | float obs[N_OBSERVATIONS]; 18 | float acs[N_ACTIONS]; 19 | 20 | 21 | int main() { 22 | UDP udp; 23 | initialize_udp(&udp, "0.0.0.0", JOYSTICK_PORT, "127.0.0.1", JOYSTICK_PORT); 24 | 25 | while (true) { 26 | uint8_t udp_buffer[13]; 27 | ssize_t actual_bytes = recvfrom(udp.sockfd, udp_buffer, 13, MSG_WAITALL, NULL, NULL); 28 | printf("actual_bytes: %ld\n", actual_bytes); 29 | printf("udp_buffer: %d %.2f %.2f %.2f\n", udp_buffer[0], *(float *)(udp_buffer + 1), *(float *)(udp_buffer + 5), *(float *)(udp_buffer + 9)); 30 | } 31 | 32 | return 0; 33 | } -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools>=61.0"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [project] 6 | name = "berkeley_humanoid_lite_lowlevel" 7 | version = "2025.09.07" 8 | description = "Low-level control for Berkeley Humanoid Lite" 9 | authors = [ 10 | { name = "Berkeley Humanoid Project Developers" }, 11 | { name = "-T.K.-" }, 12 | ] 13 | maintainers = [ 14 | { name = "Berkeley Humanoid Project Developers" }, 15 | { name = "-T.K.-" }, 16 | ] 17 | readme = "README.md" 18 | requires-python = ">=3.10" 19 | license = "MIT" 20 | keywords = ["robotics", "lowlevel"] 21 | classifiers = [ 22 | "Natural Language :: English", 23 | "Programming Language :: Python :: 3.10", 24 | ] 25 | dependencies = [ 26 | "psutil", 27 | ] 28 | 29 | [project.urls] 30 | repository = "https://github.com/HybridRobotics/Berkeley-Humanoid-Lite-Lowlevel" 31 | 32 | [tool.setuptools] 33 | packages = ["berkeley_humanoid_lite_lowlevel"] 34 | zip-safe = false 35 | -------------------------------------------------------------------------------- /scripts/run_locomotion.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | from cc.udp import UDP 4 | from loop_rate_limiters import RateLimiter 5 | 6 | from berkeley_humanoid_lite_lowlevel.robot import Humanoid 7 | from berkeley_humanoid_lite_lowlevel.policy.rl_controller import RlController 8 | from berkeley_humanoid_lite_lowlevel.policy.config import Cfg 9 | 10 | 11 | # Load configuration 12 | cfg = Cfg.from_arguments() 13 | 14 | print(f"Policy frequency: {1 / cfg.policy_dt} Hz") 15 | 16 | udp = UDP(("0.0.0.0", 11000), ("172.28.0.5", 11000)) 17 | 18 | 19 | # Initialize and start policy controller 20 | controller = RlController(cfg) 21 | controller.load_policy() 22 | 23 | rate = RateLimiter(1 / cfg.policy_dt) 24 | 25 | robot = Humanoid() 26 | 27 | robot.enter_damping() 28 | 29 | obs = robot.reset() 30 | 31 | try: 32 | while True: 33 | actions = controller.update(obs) 34 | obs = robot.step(actions) 35 | udp.send_numpy(obs) 36 | 37 | rate.sleep() 38 | 39 | except KeyboardInterrupt: 40 | robot.stop() 41 | 42 | print("Stopped.") 43 | -------------------------------------------------------------------------------- /csrc/udp.c: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include "udp.h" 4 | 5 | 6 | ssize_t initialize_udp( 7 | UDP *udp, 8 | const char *recv_ip, const uint16_t recv_port, 9 | const char *send_ip, const uint16_t send_port 10 | ) { 11 | memset(udp, 0, sizeof(UDP)); 12 | memset(&udp->recv_addr, 0, sizeof(udp->recv_addr)); 13 | memset(&udp->send_addr, 0, sizeof(udp->send_addr)); 14 | 15 | udp->recv_addr.sin_family = AF_INET; 16 | udp->recv_addr.sin_addr.s_addr = inet_addr(recv_ip); 17 | udp->recv_addr.sin_port = htons(recv_port); 18 | 19 | udp->send_addr.sin_family = AF_INET; 20 | udp->send_addr.sin_addr.s_addr = inet_addr(send_ip); 21 | udp->send_addr.sin_port = htons(send_port); 22 | 23 | if ((udp->sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { 24 | printf("[Error] Error creating socket: %s\n", strerror(errno)); 25 | return -1; 26 | } 27 | 28 | if (bind(udp->sockfd, (struct sockaddr *)&udp->recv_addr, sizeof(udp->recv_addr)) < 0) { 29 | printf("[Error] Error binding socket: %s\n", strerror(errno)); 30 | return -1; 31 | } 32 | 33 | printf("[INFO] Server listening on %s:%d\n", recv_ip, recv_port); 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /scripts/test_inference.py: -------------------------------------------------------------------------------- 1 | 2 | import argparse 3 | from typing import Union 4 | 5 | import numpy as np 6 | from omegaconf import DictConfig, ListConfig, OmegaConf 7 | 8 | from berkeley_humanoid_lite_lowlevel.policy.rl_controller import RlController 9 | 10 | 11 | def parse_arguments() -> Union[DictConfig, ListConfig]: 12 | """ 13 | Parse command line arguments and load configuration file. 14 | 15 | Returns: 16 | Union[DictConfig, ListConfig]: Loaded configuration object 17 | """ 18 | parser = argparse.ArgumentParser(description="Policy Runner for Berkeley Humanoid Lite") 19 | parser.add_argument("--config", type=str, default="./configs/policy_humanoid.yaml", 20 | help="Path to the configuration file") 21 | args = parser.parse_args() 22 | 23 | print("Loading config file from ", args.config) 24 | 25 | with open(args.config, "r") as f: 26 | cfg = OmegaConf.load(f) 27 | 28 | return cfg 29 | 30 | 31 | # Load configuration 32 | cfg = parse_arguments() 33 | 34 | 35 | # Initialize and start policy controller 36 | controller = RlController(cfg) 37 | controller.load_policy() 38 | 39 | obs = np.zeros((63,), dtype=np.float32) # Example observation, adjust as needed 40 | 41 | actions = controller.update(obs) -------------------------------------------------------------------------------- /scripts/motor/move_actuator.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import time 4 | import numpy as np 5 | 6 | from loop_rate_limiters import RateLimiter 7 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 8 | 9 | 10 | args = recoil.util.get_args() 11 | bus = recoil.Bus(channel=args.channel, bitrate=1000000) 12 | 13 | device_id = args.id 14 | 15 | kp = 0.2 16 | kd = 0.005 17 | 18 | frequency = 1.0 # motion frequency is 1 Hz 19 | amplitude = 1.0 # motion amplitude is 1 rad 20 | 21 | rate = RateLimiter(frequency=200.0) 22 | 23 | 24 | bus.write_position_kp(device_id, kp) 25 | bus.write_position_kd(device_id, kd) 26 | bus.write_torque_limit(device_id, 0.2) 27 | 28 | bus.set_mode(device_id, recoil.Mode.POSITION) 29 | bus.feed(device_id) 30 | 31 | try: 32 | while True: 33 | target_angle = np.sin(2 * np.pi * frequency * time.time()) * amplitude 34 | 35 | measured_position, measured_velocity = bus.write_read_pdo_2(device_id, target_angle, 0.0) 36 | if measured_position is not None and measured_velocity is not None: 37 | print(f"Measured pos: {measured_position:.3f} \tvel: {measured_velocity:.3f}") 38 | 39 | rate.sleep() 40 | 41 | except KeyboardInterrupt: 42 | pass 43 | 44 | bus.set_mode(device_id, recoil.Mode.IDLE) 45 | bus.stop() 46 | -------------------------------------------------------------------------------- /scripts/test_imu.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import time 4 | import struct 5 | import scipy.spatial.transform as st 6 | 7 | import serial 8 | 9 | SYNC_1 = b'\x75' 10 | SYNC_2 = b'\x65' 11 | 12 | 13 | ser = serial.Serial("/dev/serial/by-path/pci-0000:00:14.0-usb-0:4.1:1.0", 1000000, timeout=0.001) 14 | 15 | try: 16 | while True: 17 | t = time.perf_counter() 18 | 19 | sync_1 = ser.read(1) 20 | if not sync_1 or sync_1 != SYNC_1: 21 | # print("sync_1 error", sync_1) 22 | continue 23 | 24 | sync_2 = ser.read(1) 25 | if not sync_2 or sync_2 != SYNC_2: 26 | # print("sync_2 error", sync_2) 27 | continue 28 | 29 | size = ser.read(2) 30 | data_buffer = ser.read(28) 31 | 32 | data = struct.unpack("f"*7, data_buffer) 33 | 34 | # print([round(d, 2) for d in data]) 35 | w, x, y, z, rx, ry, rz = data 36 | 37 | # roll pitch yaw 38 | euler = st.Rotation.from_quat([w, x, y, z], scalar_first=True).as_euler("xyz", degrees=True) 39 | 40 | print(f"x: {euler[0]:.2f} deg, y: {euler[1]:.2f} deg, z: {euler[2]:.2f} deg, freq: {1 / (time.perf_counter() - t):.2f} Hz", end="\r") 41 | 42 | except KeyboardInterrupt: 43 | print() 44 | print("Keyboard interrupt") 45 | 46 | ser.close() 47 | -------------------------------------------------------------------------------- /csrc/udp.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #ifndef UDP_H 4 | #define UDP_H 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | #define NANOSECOND_PER_SECOND 1000000000 22 | 23 | 24 | typedef struct { 25 | int sockfd; 26 | struct sockaddr_in recv_addr; 27 | struct sockaddr_in send_addr; 28 | } UDP; 29 | 30 | 31 | typedef struct { 32 | UDP udp; 33 | size_t n_obs; 34 | size_t n_acs; 35 | } PolicyComm; 36 | 37 | typedef struct { 38 | UDP udp; 39 | size_t n_obs; 40 | size_t n_acs; 41 | 42 | pthread_t thread_obs; 43 | pthread_t thread_acs; 44 | 45 | size_t frequency; 46 | 47 | float *obs; 48 | float *acs; 49 | } EnvironmentComm; 50 | 51 | 52 | ssize_t initialize_udp( 53 | UDP *udp, 54 | const char *recv_ip, const uint16_t recv_port, 55 | const char *send_ip, const uint16_t send_port 56 | ); 57 | 58 | ssize_t initialize_policy( 59 | PolicyComm *comm, 60 | const char *compute_ip, int compute_port, 61 | const char *env_ip, int env_port, 62 | size_t n_obs, size_t n_acs 63 | ); 64 | 65 | #ifdef __cplusplus 66 | } 67 | #endif 68 | 69 | #endif -------------------------------------------------------------------------------- /ctests/test-imu.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "imu.h" 11 | 12 | 13 | #define IMU_PATH "/dev/serial/by-path/pci-0000:00:14.0-usb-0:4:1.0" 14 | #define IMU_BAUDRATE B1000000 15 | 16 | 17 | int main() { 18 | IMU imu = IMU(IMU_PATH, IMU_BAUDRATE); 19 | ssize_t ret = imu.init(); 20 | if (ret < 0) { 21 | printf("Error initializing IMU: %s\n", strerror(errno)); 22 | return -1; 23 | } 24 | 25 | const sched_param sched{.sched_priority = 49}; 26 | pthread_setschedparam(pthread_self(), SCHED_FIFO, &sched); 27 | 28 | 29 | 30 | while (1) { 31 | struct timespec start_time, current_time; 32 | clock_gettime(CLOCK_MONOTONIC, &start_time); 33 | 34 | // imu.trigger_update(); 35 | imu.update_reading(); 36 | 37 | clock_gettime(CLOCK_MONOTONIC, ¤t_time); 38 | double elapsed_time = (current_time.tv_sec - start_time.tv_sec) + (current_time.tv_nsec - start_time.tv_nsec) / 1e9; 39 | // printf("Elapsed time: %.2f ms, Hz: %.2f\n", elapsed_time * 1e3, 1.0 / elapsed_time); 40 | 41 | printf("qpos: %f %f %f %f\n", imu.quaternion[0], imu.quaternion[1], imu.quaternion[2], imu.quaternion[3]); 42 | } 43 | 44 | 45 | 46 | printf("Hello, World!\n"); 47 | } 48 | 49 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/policy/config.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | from omegaconf import DictConfig, ListConfig, OmegaConf 4 | 5 | 6 | class Cfg(DictConfig): 7 | """Configuration class holding simulation and robot parameters.""" 8 | 9 | # === Policy configurations === 10 | policy_checkpoint_path: str 11 | 12 | # === Networking configurations === 13 | ip_robot_addr: str 14 | ip_policy_obs_port: int 15 | ip_host_addr: str 16 | ip_policy_acs_port: int 17 | 18 | # === Physics configurations === 19 | control_dt: float 20 | policy_dt: float 21 | physics_dt: float 22 | cutoff_freq: float 23 | 24 | # === Articulation configurations === 25 | num_joints: int 26 | joints: list[str] 27 | joint_kp: list[float] | float 28 | joint_kd: list[float] | float 29 | effort_limits: list[float] 30 | default_base_position: list[float] 31 | default_joint_positions: list[float] 32 | 33 | # === Observation configurations === 34 | num_observations: int 35 | history_length: int 36 | 37 | # === Command configurations === 38 | command_velocity: list[float] 39 | 40 | # === Action configurations === 41 | num_actions: int 42 | action_indices: list[int] 43 | action_scale: float 44 | action_limit_lower: float 45 | action_limit_upper: float 46 | 47 | @staticmethod 48 | def from_arguments() -> DictConfig | ListConfig: 49 | """ 50 | Parse command line arguments and load configuration file. 51 | 52 | Returns: 53 | DictConfig | ListConfig: Loaded configuration object 54 | """ 55 | parser = argparse.ArgumentParser(description="Policy Runner for Berkeley Humanoid Lite") 56 | parser.add_argument( 57 | "--config", 58 | type=str, 59 | default="./configs/policy_biped.yaml", 60 | help="Path to the configuration file", 61 | ) 62 | args = parser.parse_args() 63 | 64 | print("Loading config file from ", args.config) 65 | 66 | with open(args.config, "r") as f: 67 | cfg = OmegaConf.load(f) 68 | 69 | return cfg 70 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ================================================================ 2 | # cmake -S ./ -B ./build/ 3 | # cmake --build ./build/ --target all 4 | # ================================================================ 5 | 6 | cmake_minimum_required(VERSION 3.10) 7 | project(humanoid) 8 | 9 | set(CMAKE_BUILD_TYPE Debug) 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 11 | 12 | #set(TORCH_PATH "/home/tk/Documents/libtorch/") 13 | 14 | #find_package(Torch REQUIRED PATHS ${TORCH_PATH}) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}") 16 | 17 | include(FetchContent) 18 | 19 | FetchContent_Declare( 20 | yaml-cpp 21 | GIT_REPOSITORY https://github.com/jbeder/yaml-cpp.git 22 | GIT_TAG yaml-cpp-0.7.0 23 | ) 24 | FetchContent_MakeAvailable(yaml-cpp) 25 | 26 | 27 | # ================================ main application ================================ 28 | 29 | add_executable(main 30 | csrc/main.cpp 31 | csrc/udp.c 32 | csrc/serializer.c 33 | csrc/imu.cpp 34 | csrc/socketcan.cpp 35 | csrc/motor_controller.cpp 36 | csrc/real_humanoid.cpp 37 | ) 38 | 39 | set_property(TARGET main PROPERTY CXX_STANDARD 17) 40 | 41 | target_include_directories(main PRIVATE csrc) 42 | 43 | target_link_libraries(main yaml-cpp::yaml-cpp) 44 | 45 | 46 | # ================================ test applications ================================ 47 | 48 | # Torch 49 | add_executable(test-torch ctests/test-torch.cpp) 50 | target_link_libraries(test-torch "${TORCH_LIBRARIES}") 51 | set_property(TARGET test-torch PROPERTY CXX_STANDARD 17) 52 | 53 | 54 | # IMU 55 | add_executable(test-imu ctests/test-imu.cpp csrc/imu.cpp) 56 | target_include_directories(test-imu PRIVATE csrc) 57 | set_property(TARGET test-imu PROPERTY CXX_STANDARD 17) 58 | 59 | 60 | # UDP 61 | add_executable(test-udp ctests/test-udp.cpp csrc/udp.c) 62 | target_include_directories(test-udp PRIVATE csrc) 63 | set_property(TARGET test-udp PROPERTY CXX_STANDARD 17) 64 | 65 | 66 | # YAML 67 | add_executable(test-yaml ctests/test-yaml.cpp) 68 | target_include_directories(test-yaml PRIVATE csrc) 69 | target_link_libraries(test-yaml yaml-cpp::yaml-cpp) 70 | set_property(TARGET test-yaml PROPERTY CXX_STANDARD 17) 71 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Berkeley Humanoid Lite Low-level Control 2 | 3 | This submodule repository contains the low-level control code for the Berkeley Humanoid Lite robot. 4 | 5 | 6 | ## Installation 7 | 8 | ```bash 9 | sudo apt install net-tools can-utils 10 | ``` 11 | 12 | ```bash 13 | pip install -r requirements.txt 14 | ``` 15 | 16 | ## Getting Started 17 | 18 | ### Bring up the CAN interface 19 | 20 | The low-level computer connects to the joints via CAN. 21 | 22 | Run this script to initialize the CAN interface: 23 | 24 | ```bash 25 | sudo ./scripts/start_can_transports.sh 26 | ``` 27 | 28 | ### Verify CAN connection 29 | 30 | A Python script is provided to verify the CAN connection to all the joints of the robot: 31 | 32 | ```bash 33 | python3 ./berkeley_humanoid_lite_lowlevel/robot/check_connection.py 34 | ``` 35 | 36 | ### Launch the joystick receiver 37 | 38 | The low-level computer also receives commands from the joystick. 39 | 40 | To broadcast the joystick commands to other running nodes, run the following command: 41 | 42 | ```bash 43 | python ./berkeley_humanoid_lite_lowlevel/policy/udp_joystick.py 44 | ``` 45 | 46 | ### Joint Calibration 47 | 48 | Because the joint actuators only have single encoder on the motor shaft, we need to calibrate the zero position of the joints after each power cycle. 49 | 50 | Run the following command to start the calibration: 51 | 52 | ```bash 53 | python3 ./berkeley_humanoid_lite_lowlevel/robot/calibrate_joints.py 54 | ``` 55 | 56 | After the script is launched and running, manually move the robot joints to the mechanical position limits. After all the joints are moved, press `q` or the `B` button on the joystick to quit the calibration. 57 | 58 | The calibration data will be saved in the `./calibration.yaml` file. 59 | 60 | 61 | 62 | ### Run main controller 63 | 64 | The main controller is implemented in C. 65 | 66 | To run the controller, run the following command: 67 | 68 | ```bash 69 | make run 70 | ``` 71 | 72 | Press `LB` + `A` to enter RL init mode. Then, press `RB` + `A` to enter RL running mode. 73 | 74 | At any time, press `B` or the thumb buttons to exit RL mode. The joints will enter passive damping mode. 75 | 76 | Press `Ctrl` + `C` to terminate the controller. Upon first termination, the joints will enter passive damping mode. Press `Ctrl` + `C` again to completely stop the controller, which joints will return to unpowered idle state. 77 | 78 | -------------------------------------------------------------------------------- /scripts/calibrate_joints.py: -------------------------------------------------------------------------------- 1 | """ 2 | calibrate_joints.py 3 | 4 | Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 5 | 6 | Run this script after each power cycle to calibrate the encoder offset of each joint. 7 | """ 8 | 9 | import time 10 | 11 | import numpy as np 12 | import yaml 13 | 14 | from berkeley_humanoid_lite_lowlevel.robot import Humanoid 15 | 16 | 17 | robot = Humanoid() 18 | 19 | joint_axis_directions = np.array([ 20 | -1, +1, -1, 21 | -1, 22 | -1, +1, 23 | -1, +1, +1, 24 | +1, 25 | +1, +1 26 | ]) 27 | 28 | 29 | ideal_values = np.array([ 30 | np.deg2rad(-(10)), 31 | np.deg2rad(+(33.75)), 32 | np.deg2rad(+(56.25)), 33 | np.deg2rad(+(0)), 34 | np.deg2rad(-(45)), 35 | np.deg2rad(-(15)), 36 | 37 | np.deg2rad(+(10)), 38 | np.deg2rad(-(33.75)), 39 | np.deg2rad(+(56.25)), 40 | np.deg2rad(+(0)), 41 | np.deg2rad(-(45)), 42 | np.deg2rad(+(15)), 43 | ]) 44 | 45 | 46 | print("initial readings:") 47 | limit_readings = np.array([joint[0].read_position_measured(joint[1]) for joint in robot.joints]) * joint_axis_directions 48 | print([f"{reading:.2f}" for reading in limit_readings]) 49 | 50 | while robot.command_controller.commands.get("mode_switch") != 1: 51 | joint_readings = np.array([joint[0].read_position_measured(joint[1]) for joint in robot.joints]) * joint_axis_directions 52 | 53 | limit_readings[0] = min(limit_readings[0], joint_readings[0]) 54 | limit_readings[1] = max(limit_readings[1], joint_readings[1]) 55 | limit_readings[2] = max(limit_readings[2], joint_readings[2]) 56 | limit_readings[3] = min(limit_readings[3], joint_readings[3]) 57 | limit_readings[4] = min(limit_readings[4], joint_readings[4]) 58 | limit_readings[5] = min(limit_readings[5], joint_readings[5]) 59 | 60 | limit_readings[6] = max(limit_readings[6], joint_readings[6]) 61 | limit_readings[7] = min(limit_readings[7], joint_readings[7]) 62 | limit_readings[8] = max(limit_readings[8], joint_readings[8]) 63 | limit_readings[9] = min(limit_readings[9], joint_readings[9]) 64 | limit_readings[10] = min(limit_readings[10], joint_readings[10]) 65 | limit_readings[11] = max(limit_readings[11], joint_readings[11]) 66 | 67 | print(time.time(), [f"{reading:.2f}" for reading in limit_readings]) 68 | 69 | time.sleep(0.05) 70 | 71 | 72 | print("final readings at the limits:") 73 | print([f"{limit:.4f}" for limit in limit_readings]) 74 | 75 | print("offsets:") 76 | print([f"{offset:.4f}" for offset in (limit_readings - ideal_values)]) 77 | 78 | calibration_data = { 79 | "position_offsets": [float(offset) for offset in (limit_readings - ideal_values)], 80 | } 81 | 82 | with open("calibration.yaml", "w") as f: 83 | yaml.dump(calibration_data, f) 84 | 85 | robot.stop() 86 | -------------------------------------------------------------------------------- /csrc/socketcan.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 3/3/21. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | // Multi-threading 43 | #include 44 | #include 45 | 46 | 47 | class SocketCan 48 | { 49 | private: 50 | ifreq interface_request_{}; 51 | sockaddr_can address_{}; 52 | pthread_t receiver_thread_id_{}; 53 | 54 | public: 55 | /** 56 | * CAN socket file descriptor 57 | */ 58 | int sock_fd_ = -1; 59 | 60 | SocketCan() = default; 61 | ~SocketCan(); 62 | 63 | /** \brief Open and bind socket. 64 | * 65 | * \param interface bus's name(example: can0). 66 | * 67 | * \returns \c true if it successfully open and bind socket. 68 | */ 69 | bool open(const std::string& interface); 70 | /** \brief Close and unbind socket. 71 | * 72 | */ 73 | void close(); 74 | /** \brief Returns whether the socket is open or closed. 75 | * 76 | * \returns \c True if socket has opened. 77 | */ 78 | bool isOpen() const; 79 | /** \brief Sends the referenced frame to the bus. 80 | * 81 | * \param frame referenced frame which you want to send. 82 | */ 83 | void write(can_frame* frame) const; 84 | /** \brief Starts a new thread, that will wait for socket events. 85 | * 86 | */ 87 | can_frame read(); 88 | 89 | }; 90 | -------------------------------------------------------------------------------- /scripts/motor/read_configurations.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import json 4 | import time 5 | 6 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 7 | 8 | 9 | args = recoil.util.get_args() 10 | bus = recoil.Bus(channel=args.channel, bitrate=1000000) 11 | 12 | device_id = args.id 13 | 14 | print(f"Reading configuration from actuator #{args.id}") 15 | 16 | config = { 17 | "position_controller": {}, 18 | "current_controller": {}, 19 | "powerstage": {}, 20 | "motor": {}, 21 | "encoder": {}, 22 | } 23 | 24 | config["device_id"] = bus._read_parameter_u32(device_id, recoil.Parameter.DEVICE_ID) 25 | config["firmware_version"] = hex(bus._read_parameter_u32(device_id, recoil.Parameter.FIRMWARE_VERSION)) 26 | config["watchdog_timeout"] = bus._read_parameter_u32(device_id, recoil.Parameter.WATCHDOG_TIMEOUT) 27 | config["fast_frame_frequency"] = bus.read_fast_frame_frequency(device_id) 28 | 29 | config["position_controller"]["gear_ratio"] = bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_GEAR_RATIO) 30 | config["position_controller"]["position_kp"] = bus.read_position_kp(device_id) 31 | config["position_controller"]["position_ki"] = bus.read_position_ki(device_id) 32 | config["position_controller"]["velocity_kp"] = bus.read_velocity_kp(device_id) 33 | config["position_controller"]["velocity_ki"] = bus.read_velocity_ki(device_id) 34 | config["position_controller"]["torque_limit"] = bus.read_torque_limit(device_id) 35 | config["position_controller"]["velocity_limit"] = bus.read_velocity_limit(device_id) 36 | config["position_controller"]["position_limit_upper"] = bus.read_position_limit_upper(device_id) 37 | config["position_controller"]["position_limit_lower"] = bus.read_position_limit_lower(device_id) 38 | config["position_controller"]["position_offset"] = bus.read_position_offset(device_id) 39 | config["position_controller"]["torque_filter_alpha"] = bus.read_torque_filter_alpha(device_id) 40 | 41 | config["current_controller"]["i_limit"] = bus.read_current_limit(device_id) 42 | config["current_controller"]["i_kp"] = bus.read_current_kp(device_id) 43 | config["current_controller"]["i_ki"] = bus.read_current_ki(device_id) 44 | 45 | config["powerstage"]["undervoltage_threshold"] = bus._read_parameter_f32(device_id, recoil.Parameter.POWERSTAGE_UNDERVOLTAGE_THRESHOLD) 46 | config["powerstage"]["overvoltage_threshold"] = bus._read_parameter_f32(device_id, recoil.Parameter.POWERSTAGE_OVERVOLTAGE_THRESHOLD) 47 | config["powerstage"]["bus_voltage_filter_alpha"] = bus.read_bus_voltage_filter_alpha(device_id) 48 | 49 | config["motor"]["pole_pairs"] = bus.read_motor_pole_pairs(device_id) 50 | config["motor"]["torque_constant"] = bus.read_motor_torque_constant(device_id) 51 | config["motor"]["phase_order"] = bus.read_motor_phase_order(device_id) 52 | config["motor"]["max_calibration_current"] = bus.read_motor_calibration_current(device_id) 53 | 54 | config["encoder"]["cpr"] = bus.read_encoder_cpr(device_id) 55 | config["encoder"]["position_offset"] = bus.read_encoder_position_offset(device_id) 56 | config["encoder"]["velocity_filter_alpha"] = bus.read_encoder_velocity_filter_alpha(device_id) 57 | config["encoder"]["flux_offset"] = bus.read_encoder_flux_offset(device_id) 58 | 59 | 60 | time.sleep(0.1) 61 | 62 | 63 | with open("motor_configuration.json", "w") as f: 64 | json.dump(config, f, indent=4) 65 | 66 | bus.stop() 67 | 68 | print("Done") 69 | -------------------------------------------------------------------------------- /scripts/robot/read_configurations.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import json 4 | import time 5 | 6 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 7 | from berkeley_humanoid_lite_lowlevel.robot import Humanoid 8 | 9 | 10 | robot_configuration = {} 11 | 12 | robot = Humanoid() 13 | 14 | robot.check_connection() 15 | 16 | for entry in robot.joints: 17 | bus, joint_id, joint_name = entry 18 | 19 | print(f"Reading configuration for {joint_name}") 20 | 21 | config = { 22 | "position_controller": {}, 23 | "current_controller": {}, 24 | "powerstage": {}, 25 | "motor": {}, 26 | "encoder": {}, 27 | } 28 | 29 | config["device_id"] = bus._read_parameter_u32(joint_id, recoil.Parameter.DEVICE_ID) 30 | config["firmware_version"] = hex(bus._read_parameter_u32(joint_id, recoil.Parameter.FIRMWARE_VERSION)) 31 | config["watchdog_timeout"] = bus._read_parameter_u32(joint_id, recoil.Parameter.WATCHDOG_TIMEOUT) 32 | config["fast_frame_frequency"] = bus.read_fast_frame_frequency(joint_id) 33 | 34 | config["position_controller"]["gear_ratio"] = bus._read_parameter_f32(joint_id, recoil.Parameter.POSITION_CONTROLLER_GEAR_RATIO) 35 | config["position_controller"]["position_kp"] = bus.read_position_kp(joint_id) 36 | config["position_controller"]["position_ki"] = bus.read_position_ki(joint_id) 37 | config["position_controller"]["velocity_kp"] = bus.read_velocity_kp(joint_id) 38 | config["position_controller"]["velocity_ki"] = bus.read_velocity_ki(joint_id) 39 | config["position_controller"]["torque_limit"] = bus.read_torque_limit(joint_id) 40 | config["position_controller"]["velocity_limit"] = bus.read_velocity_limit(joint_id) 41 | config["position_controller"]["position_limit_upper"] = bus.read_position_limit_upper(joint_id) 42 | config["position_controller"]["position_limit_lower"] = bus.read_position_limit_lower(joint_id) 43 | config["position_controller"]["position_offset"] = bus.read_position_offset(joint_id) 44 | config["position_controller"]["torque_filter_alpha"] = bus.read_torque_filter_alpha(joint_id) 45 | 46 | config["current_controller"]["i_limit"] = bus.read_current_limit(joint_id) 47 | config["current_controller"]["i_kp"] = bus.read_current_kp(joint_id) 48 | config["current_controller"]["i_ki"] = bus.read_current_ki(joint_id) 49 | 50 | config["powerstage"]["undervoltage_threshold"] = bus._read_parameter_f32(joint_id, recoil.Parameter.POWERSTAGE_UNDERVOLTAGE_THRESHOLD) 51 | config["powerstage"]["overvoltage_threshold"] = bus._read_parameter_f32(joint_id, recoil.Parameter.POWERSTAGE_OVERVOLTAGE_THRESHOLD) 52 | config["powerstage"]["bus_voltage_filter_alpha"] = bus.read_bus_voltage_filter_alpha(joint_id) 53 | 54 | config["motor"]["pole_pairs"] = bus.read_motor_pole_pairs(joint_id) 55 | config["motor"]["torque_constant"] = bus.read_motor_torque_constant(joint_id) 56 | config["motor"]["phase_order"] = bus.read_motor_phase_order(joint_id) 57 | config["motor"]["max_calibration_current"] = bus.read_motor_calibration_current(joint_id) 58 | 59 | config["encoder"]["cpr"] = bus.read_encoder_cpr(joint_id) 60 | config["encoder"]["position_offset"] = bus.read_encoder_position_offset(joint_id) 61 | config["encoder"]["velocity_filter_alpha"] = bus.read_encoder_velocity_filter_alpha(joint_id) 62 | config["encoder"]["flux_offset"] = bus.read_encoder_flux_offset(joint_id) 63 | 64 | robot_configuration[joint_name] = config 65 | time.sleep(0.1) 66 | 67 | 68 | with open("robot_configuration.json", "w") as f: 69 | json.dump(robot_configuration, f, indent=4) 70 | 71 | robot.stop(joint_id) 72 | 73 | print("Done") 74 | -------------------------------------------------------------------------------- /csrc/loop_function.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | class LoopFunc { 17 | public: 18 | LoopFunc(const std::string &name, float period, std::function func, int bind_cpu = -1, bool detach = false, int priority = -1, bool log = false) 19 | : _name(name), _period(period), _func(func), _bind_cpu(bind_cpu), _running(false), _detach(detach), _priority(priority), _log(log) {} 20 | 21 | void start() { 22 | _running = true; 23 | if (_log) { 24 | log("[Loop Start] name: " + _name + ", period: " + format_period_ms() + " ms, freq: " + format_freq_hz() + " Hz" + (_bind_cpu != -1 ? ", run at cpu: " + std::to_string(_bind_cpu) : ", cpu unspecified") + (_priority != -1 ? ", priority: " + std::to_string(_priority) : "")); 25 | } 26 | _thread = std::thread(&LoopFunc::loop, this); 27 | if (_bind_cpu != -1) { 28 | set_thread_affinity(_thread.native_handle(), _bind_cpu); 29 | } 30 | if (_priority != -1) { 31 | set_thread_priority(_thread.native_handle(), _priority); 32 | } 33 | if (_detach) { 34 | _thread.detach(); 35 | } 36 | } 37 | 38 | void shutdown() { 39 | _running = false; 40 | if (_thread.joinable()) { 41 | printf("Joining thread %s\n", _name.c_str()); 42 | _thread.join(); 43 | } 44 | log("[Loop End] name: " + _name); 45 | } 46 | 47 | private: 48 | std::string _name; 49 | float _period; 50 | std::function _func; 51 | int _bind_cpu; 52 | std::atomic _running; 53 | std::thread _thread; 54 | bool _detach; 55 | int _priority; 56 | bool _log; 57 | void loop() { 58 | while (_running) { 59 | const auto target_time = std::chrono::high_resolution_clock::now() + std::chrono::duration(_period); 60 | _func(); 61 | const auto finished_time = std::chrono::high_resolution_clock::now(); 62 | if (finished_time < target_time) { 63 | std::this_thread::sleep_for(target_time - finished_time); 64 | } else { 65 | if (_log) { 66 | auto elapsed = std::chrono::duration_cast(finished_time - target_time).count(); 67 | printf("[ERROR] : Task %s use %d us more to complete\n", _name.c_str(), static_cast(elapsed)); 68 | } 69 | } 70 | } 71 | } 72 | 73 | std::string format_period_ms() const { 74 | std::ostringstream stream; 75 | stream << std::fixed << std::setprecision(0) << _period * 1000; 76 | return stream.str(); 77 | } 78 | 79 | std::string format_freq_hz() const { 80 | std::ostringstream stream; 81 | stream << std::fixed << std::setprecision(0) << 1 / _period; 82 | return stream.str(); 83 | } 84 | 85 | void log(const std::string &message) { 86 | static std::mutex logMutex; 87 | std::lock_guard lock(logMutex); 88 | std::cout << message << std::endl; 89 | } 90 | 91 | void set_thread_affinity(std::thread::native_handle_type thread_handle, int cpu_id) { 92 | cpu_set_t cpuset; 93 | CPU_ZERO(&cpuset); 94 | CPU_SET(cpu_id, &cpuset); 95 | if (pthread_setaffinity_np(thread_handle, sizeof(cpu_set_t), &cpuset) != 0) { 96 | std::ostringstream oss; 97 | oss << "Error setting thread affinity: CPU " << cpu_id << " may not be valid or accessible."; 98 | throw std::runtime_error(oss.str()); 99 | } 100 | } 101 | 102 | void set_thread_priority(std::thread::native_handle_type thread_handle, int priority) { 103 | sched_param sched{}; 104 | sched.sched_priority = priority; 105 | 106 | if (pthread_setschedparam(thread_handle, SCHED_FIFO, &sched) != 0) { 107 | std::cerr << "Failed to set thread priority for worker [" << _name << "]: " << strerror(errno) << std::endl; 108 | } 109 | 110 | } 111 | }; 112 | 113 | -------------------------------------------------------------------------------- /csrc/imu.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include "imu.h" 4 | 5 | IMU::IMU(const char *path, speed_t baudrate) { 6 | path_ = path; 7 | baudrate_ = baudrate; 8 | } 9 | 10 | IMU::~IMU() { 11 | close(fd); 12 | } 13 | 14 | ssize_t IMU::init() { 15 | // imu->fd = Serial_init(path, baudrate); 16 | fd = open(path_, O_RDWR); 17 | 18 | if (fd < 0) { 19 | printf("[ERROR] : Error %i from open: %s\n", errno, strerror(errno)); 20 | return -1; 21 | } 22 | 23 | struct termios tty; 24 | memset(&tty, 0, sizeof tty); 25 | 26 | if (tcgetattr(fd, &tty) != 0) { 27 | printf("[ERROR] : Error %i from tcgetattr: %s\n", errno, strerror(errno)); 28 | return -1; 29 | } 30 | 31 | tty.c_cflag &= ~PARENB; // Disable parity 32 | tty.c_cflag &= ~CSTOPB; // 1 stop bit 33 | tty.c_cflag &= ~CSIZE; // Clear the mask 34 | tty.c_cflag |= CS8; // 8 data bits 35 | tty.c_cflag &= ~CRTSCTS; // No hardware flow control 36 | tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines 37 | 38 | tty.c_lflag &= ~ICANON; // Disable canonical mode 39 | tty.c_lflag &= ~ECHO; // Disable echo 40 | tty.c_lflag &= ~ECHOE; // Disable erasure 41 | tty.c_lflag &= ~ECHONL; // Disable new-line echo 42 | tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP 43 | tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl 44 | tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); // Disable any special handling of received bytes 45 | 46 | tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars) 47 | tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed 48 | 49 | tty.c_cc[VTIME] = 1; // Wait for up to 0.1s (1 deciseconds), returning as soon as any data is received. 50 | tty.c_cc[VMIN] = 0; 51 | 52 | if (tcsetattr(fd, TCSANOW, &tty) != 0) { 53 | printf("[ERROR] : Error %i from cfsetattr: %s\n", errno, strerror(errno)); 54 | return -1; 55 | } 56 | 57 | if (cfsetospeed(&tty, baudrate_) != 0) { 58 | printf("[ERROR] : Error %i from cfsetospeed: %s\n", errno, strerror(errno)); 59 | return -1; 60 | } 61 | 62 | printf("[INFO] : Serial port %s initialized\n", path_); 63 | 64 | if (fd < 0) { 65 | printf("[ERROR] : Error %i from open: %s\n", errno, strerror(errno)); 66 | return -1; 67 | } 68 | 69 | quaternion[0] = 1.0; 70 | quaternion[1] = 0.0; 71 | quaternion[2] = 0.0; 72 | quaternion[3] = 0.0; 73 | 74 | angular_velocity[0] = 0.0; 75 | angular_velocity[1] = 0.0; 76 | angular_velocity[2] = 0.0; 77 | 78 | gravity_vector[0] = 0.0; 79 | gravity_vector[1] = 0.0; 80 | gravity_vector[2] = 0.0; 81 | 82 | // wait for the IMU to start for 0.1 seconds 83 | usleep(100000); 84 | 85 | return 0; 86 | } 87 | 88 | void IMU::update_reading() { 89 | uint8_t sync_byte; 90 | size_t bytes_read; 91 | 92 | bytes_read = read(fd, &sync_byte, 1); 93 | if (bytes_read < 1 || sync_byte != SYNC_1) { 94 | printf("Sync byte 1 error: %s\n", strerror(errno)); 95 | return; 96 | } 97 | 98 | bytes_read = read(fd, &sync_byte, 1); 99 | if (bytes_read < 1 || sync_byte != SYNC_2) { 100 | printf("Sync byte 2 error: %s\n", strerror(errno)); 101 | return; 102 | } 103 | 104 | uint16_t size; 105 | bytes_read = read(fd, &size, sizeof(size)); 106 | if (bytes_read < 2) { 107 | printf("Size error: %s\n", strerror(errno)); 108 | return; 109 | } 110 | 111 | float uart_buffer[7]; 112 | bytes_read = read(fd, uart_buffer, size); 113 | if (bytes_read < size) { 114 | printf("Error reading: %s, expected %d bytes, got %ld bytes\n", strerror(errno), size, bytes_read); 115 | return; 116 | } 117 | 118 | quaternion[0] = uart_buffer[0]; 119 | quaternion[1] = uart_buffer[1]; 120 | quaternion[2] = uart_buffer[2]; 121 | quaternion[3] = uart_buffer[3]; 122 | 123 | angular_velocity[0] = uart_buffer[4]; 124 | angular_velocity[1] = uart_buffer[5]; 125 | angular_velocity[2] = uart_buffer[6]; 126 | } 127 | -------------------------------------------------------------------------------- /csrc/motor_controller.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include "socketcan.h" 9 | #include "motor_controller_conf.h" 10 | 11 | 12 | #define DEVICE_ID_MSK 0b1111111 13 | #define FUNC_ID_POS 7 14 | #define FUNC_ID_MSK (0b1111 << FUNC_ID_POS) 15 | 16 | 17 | // Helper functions for CAN frames 18 | inline uint32_t make_can_id(uint8_t device_id, uint8_t func_id) { 19 | return ((uint32_t)func_id << FUNC_ID_POS) | device_id; 20 | } 21 | 22 | inline uint8_t get_device_id(uint32_t can_id) { 23 | return can_id & DEVICE_ID_MSK; 24 | } 25 | 26 | 27 | class MotorController { 28 | public: 29 | MotorController(SocketCan *bus, size_t device_id); 30 | 31 | void ping(); 32 | void feed(); 33 | void set_mode(uint8_t mode); 34 | void load_settings_from_flash(); 35 | void store_settings_to_flash(); 36 | 37 | uint32_t read_fast_frame_frequency(); 38 | void write_fast_frame_frequency(uint32_t frequency); 39 | float read_gear_ratio(); 40 | void write_gear_ratio(float ratio); 41 | float read_position_kp(); 42 | void write_position_kp(float kp); 43 | float read_position_kd(); 44 | void write_position_kd(float kd); 45 | float read_position_ki(); 46 | void write_position_ki(float ki); 47 | float read_velocity_kp(); 48 | void write_velocity_kp(float kp); 49 | float read_velocity_ki(); 50 | void write_velocity_ki(float ki); 51 | float read_torque_limit(); 52 | void write_torque_limit(float torque); 53 | float read_velocity_limit(); 54 | void write_velocity_limit(float limit); 55 | float read_position_limit_lower(); 56 | void write_position_limit_lower(float limit); 57 | float read_position_limit_upper(); 58 | void write_position_limit_upper(float limit); 59 | float read_position_offset(); 60 | void write_position_offset(float offset); 61 | float read_torque_target(); 62 | void write_torque_target(float torque); 63 | float read_torque_measured(); 64 | float read_velocity_measured(); 65 | float read_position_target(); 66 | void write_position_target(float position); 67 | float read_position_measured(); 68 | float read_torque_filter_alpha(); 69 | void write_torque_filter_alpha(float alpha); 70 | float read_current_limit(); 71 | void write_current_limit(float current); 72 | float read_current_kp(); 73 | void write_current_kp(float kp); 74 | float read_current_ki(); 75 | void write_current_ki(float ki); 76 | float read_bus_voltage_filter_alpha(); 77 | void write_bus_voltage_filter_alpha(float alpha); 78 | float read_motor_torque_constant(); 79 | void write_motor_torque_constant(float torque_constant); 80 | int read_motor_phase_order(); 81 | void write_motor_phase_order(int order); 82 | float read_encoder_velocity_filter_alpha(); 83 | void write_encoder_velocity_filter_alpha(float alpha); 84 | void write_pdo_2(); 85 | void read_pdo_2(); 86 | 87 | void set_current_bandwidth(float bandwidth_hz, float phase_resistance, float phase_inductance); 88 | void set_torque_bandwidth(float bandwidth_hz, float position_loop_rate); 89 | void set_bus_voltage_bandwidth(float bandwidth_hz, float bus_voltage_update_rate); 90 | void set_encoder_velocity_bandwidth(float bandwidth_hz, float encoder_update_rate); 91 | 92 | float get_measured_position(); 93 | float get_measured_velocity(); 94 | void set_target_position(float position); 95 | void set_target_velocity(float velocity); 96 | 97 | private: 98 | SocketCan *bus; 99 | size_t device_id; 100 | 101 | float position_measured; 102 | float velocity_measured; 103 | 104 | float position_target; 105 | float velocity_target; 106 | 107 | float read_parameter_f32(Parameter param_id); 108 | int32_t read_parameter_i32(Parameter param_id); 109 | uint32_t read_parameter_u32(Parameter param_id); 110 | void write_parameter_f32(Parameter param_id, float value); 111 | void write_parameter_i32(Parameter param_id, int32_t value); 112 | void write_parameter_u32(Parameter param_id, uint32_t value); 113 | }; 114 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # UV 98 | # Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | #uv.lock 102 | 103 | # poetry 104 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 105 | # This is especially recommended for binary packages to ensure reproducibility, and is more 106 | # commonly ignored for libraries. 107 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 108 | #poetry.lock 109 | 110 | # pdm 111 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 112 | #pdm.lock 113 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 114 | # in version control. 115 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 116 | .pdm.toml 117 | .pdm-python 118 | .pdm-build/ 119 | 120 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 121 | __pypackages__/ 122 | 123 | # Celery stuff 124 | celerybeat-schedule 125 | celerybeat.pid 126 | 127 | # SageMath parsed files 128 | *.sage.py 129 | 130 | # Environments 131 | .env 132 | .venv 133 | env/ 134 | venv/ 135 | ENV/ 136 | env.bak/ 137 | venv.bak/ 138 | 139 | # Spyder project settings 140 | .spyderproject 141 | .spyproject 142 | 143 | # Rope project settings 144 | .ropeproject 145 | 146 | # mkdocs documentation 147 | /site 148 | 149 | # mypy 150 | .mypy_cache/ 151 | .dmypy.json 152 | dmypy.json 153 | 154 | # Pyre type checker 155 | .pyre/ 156 | 157 | # pytype static type analyzer 158 | .pytype/ 159 | 160 | # Cython debug symbols 161 | cython_debug/ 162 | 163 | # PyCharm 164 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 165 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 166 | # and can be added to the global gitignore or merged into this file. For a more nuclear 167 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 168 | #.idea/ 169 | 170 | # Ruff stuff: 171 | .ruff_cache/ 172 | 173 | # PyPI configuration file 174 | .pypirc 175 | 176 | # VSCode 177 | .vscode/ 178 | 179 | # Model checkpoints and config files 180 | checkpoints/* 181 | !checkpoints/.gitkeep 182 | configs/* 183 | !configs/.gitkeep -------------------------------------------------------------------------------- /csrc/serializer.c: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include "serializer.h" 4 | 5 | 6 | int Serial_init(const char *path, speed_t baudrate) { 7 | int fd = open(path, O_RDWR); 8 | 9 | if (fd < 0) { 10 | printf("[ERROR] : Error %i from open: %s\n", errno, strerror(errno)); 11 | } 12 | 13 | struct termios tty; 14 | memset(&tty, 0, sizeof tty); 15 | 16 | if (tcgetattr(fd, &tty) != 0) { 17 | printf("[ERROR] : Error %i from tcgetattr: %s\n", errno, strerror(errno)); 18 | } 19 | 20 | tty.c_cflag &= ~PARENB; // Disable parity 21 | tty.c_cflag &= ~CSTOPB; // 1 stop bit 22 | tty.c_cflag &= ~CSIZE; // Clear the mask 23 | tty.c_cflag |= CS8; // 8 data bits 24 | tty.c_cflag &= ~CRTSCTS; // No hardware flow control 25 | tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines 26 | 27 | tty.c_lflag &= ~ICANON; // Disable canonical mode 28 | tty.c_lflag &= ~ECHO; // Disable echo 29 | tty.c_lflag &= ~ECHOE; // Disable erasure 30 | tty.c_lflag &= ~ECHONL; // Disable new-line echo 31 | tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP 32 | tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl 33 | tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); // Disable any special handling of received bytes 34 | 35 | tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars) 36 | tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed 37 | 38 | tty.c_cc[VTIME] = 10; // Wait for up to 0.2s (1 deciseconds), returning as soon as any data is received. 39 | tty.c_cc[VMIN] = 0; 40 | 41 | if (tcsetattr(fd, TCSANOW, &tty) != 0) { 42 | printf("[ERROR] : Error %i from cfsetattr: %s\n", errno, strerror(errno)); 43 | } 44 | 45 | if (cfsetospeed(&tty, baudrate) != 0) { 46 | printf("[ERROR] : Error %i from cfsetospeed: %s\n", errno, strerror(errno)); 47 | } 48 | 49 | printf("[INFO] : Serial port %s initialized\n", path); 50 | return fd; 51 | } 52 | 53 | size_t Serial_receive(int fd, char *buffer) { 54 | size_t index = 0; 55 | char c = 0; 56 | ssize_t len; 57 | 58 | len = read(fd, &c, 1); 59 | 60 | while (len > 0 && c != ENCODING_END) { 61 | if (c == ENCODING_ESC) { 62 | len = read(fd, &c, 1); 63 | if (c == ENCODING_ESC_END) { 64 | buffer[index] = ENCODING_END; 65 | } else if (c == ENCODING_ESC_ESC) { 66 | buffer[index] = ENCODING_ESC; 67 | } else { 68 | buffer[index] = c; 69 | } 70 | } else { 71 | buffer[index] = c; 72 | } 73 | index += 1; 74 | len = read(fd, &c, 1); 75 | } 76 | 77 | tcflush(fd, TCIFLUSH); 78 | 79 | return index; 80 | } 81 | 82 | void Serial_transmit(int fd, char *buffer, size_t len) { 83 | size_t index = 0; 84 | ssize_t len_written; 85 | 86 | while (index < len) { 87 | char c = buffer[index]; 88 | if (c == 0xC0) { 89 | char escape; 90 | escape = ENCODING_END; 91 | len_written = write(fd, &escape, 1); 92 | if (len_written < 0) { 93 | printf("[ERROR] : Error %i from write: %s\n", errno, strerror(errno)); 94 | } 95 | escape = ENCODING_ESC_END; 96 | len_written = write(fd, &escape, 1); 97 | if (len_written < 0) { 98 | printf("[ERROR] : Error %i from write: %s\n", errno, strerror(errno)); 99 | } 100 | } else if (c == 0xDB) { 101 | char escape; 102 | escape = ENCODING_ESC; 103 | len_written = write(fd, &escape, 1); 104 | if (len_written < 0) { 105 | printf("[ERROR] : Error %i from write: %s\n", errno, strerror(errno)); 106 | } 107 | escape = ENCODING_ESC_ESC; 108 | len_written = write(fd, &escape, 1); 109 | if (len_written < 0) { 110 | printf("[ERROR] : Error %i from write: %s\n", errno, strerror(errno)); 111 | } 112 | } else { 113 | len_written = write(fd, &c, 1); 114 | if (len_written < 0) { 115 | printf("[ERROR] : Error %i from write: %s\n", errno, strerror(errno)); 116 | } 117 | } 118 | index += 1; 119 | } 120 | char end = ENCODING_END; 121 | len_written = write(fd, &end, 1); 122 | if (len_written < 0) { 123 | printf("[ERROR] : Error %i from write: %s\n", errno, strerror(errno)); 124 | } 125 | 126 | tcflush(fd, TCOFLUSH); 127 | } 128 | 129 | void Serial_stop(int fd) { 130 | close(fd); 131 | } 132 | -------------------------------------------------------------------------------- /imu/bno085_arduino/bno085_arduino.ino: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | #define DEBUG_MODE 0 9 | 10 | #define UART_BAUDRATE 1000000 11 | 12 | #define BNO085_ADDRESS 0x4B 13 | 14 | 15 | typedef struct { 16 | uint8_t sync_1; 17 | uint8_t sync_2; 18 | uint16_t size; 19 | 20 | float rw; 21 | float rx; 22 | float ry; 23 | float rz; 24 | 25 | float vx; 26 | float vy; 27 | float vz; 28 | } IMUReadings; 29 | 30 | IMUReadings readings; 31 | 32 | Adafruit_BNO08x bno08x(-1); 33 | 34 | sh2_SensorValue_t sensor_value; 35 | 36 | uint8_t gyro_updated = 0; 37 | uint8_t quat_updated = 0; 38 | 39 | 40 | void setReports() { 41 | /* 42 | Reading `SH2_GYRO_INTEGRATED_RV` and `SH2_GYROSCOPE_CALIBRATED` ensures around 200 ~ 500 Hz update rate 43 | */ 44 | 45 | // target 200 Hz update rate 46 | uint32_t report_interval = 5000; 47 | 48 | // Top frequency is reported to be 1000Hz (but freq is somewhat variable) 49 | if (!bno08x.enableReport(SH2_GYRO_INTEGRATED_RV, report_interval)) { 50 | Serial.println("[ERROR] Could not enable stabilized remote vector"); 51 | } 52 | // Top frequency is about 250Hz but this report is more accurate 53 | // if (!bno08x.enableReport(SH2_ARVR_STABILIZED_RV, report_interval)) { 54 | // Serial.println("[ERROR] Could not enable stabilized remote vector"); 55 | // } 56 | 57 | if (!bno08x.enableReport(SH2_GYROSCOPE_CALIBRATED, report_interval)) { 58 | Serial.println("[ERROR] Could not enable gyroscope"); 59 | } 60 | // if (!bno08x.enableReport(SH2_GRAVITY, report_interval)) { 61 | // Serial.println("[ERROR] Could not enable gravity vector"); 62 | // } 63 | } 64 | 65 | void setup(void) { 66 | Serial.begin(UART_BAUDRATE); 67 | 68 | // while (!Serial) { 69 | // delay(10); 70 | // } 71 | 72 | // wait for the chip to bootup 73 | delay(200); 74 | 75 | Wire.setClock(400000); 76 | 77 | if (!bno08x.begin_I2C(BNO085_ADDRESS)) { 78 | Serial.println("[ERROR] Failed to find BNO08x chip"); 79 | while (1) {delay(10);} 80 | } 81 | 82 | 83 | pinMode(10, OUTPUT); 84 | pinMode(11, OUTPUT); 85 | 86 | // status LED 87 | pinMode(3, OUTPUT); 88 | 89 | setReports(); 90 | 91 | delay(200); 92 | 93 | readings.sync_1 = 0x75; 94 | readings.sync_2 = 0x65; 95 | readings.size = 28; 96 | } 97 | 98 | void loop() { 99 | if (bno08x.wasReset()) { 100 | // Serial.print("[ERROR] sensor was reset "); 101 | setReports(); 102 | } 103 | 104 | if (bno08x.getSensorEvent(&sensor_value)) { 105 | switch (sensor_value.sensorId) { 106 | // case SH2_ARVR_STABILIZED_RV: 107 | // readings.rw = sensor_value.un.arvrStabilizedRV.real; 108 | // readings.rx = sensor_value.un.arvrStabilizedRV.i; 109 | // readings.ry = sensor_value.un.arvrStabilizedRV.j; 110 | // readings.rz = sensor_value.un.arvrStabilizedRV.k; 111 | // break; 112 | 113 | case SH2_GYRO_INTEGRATED_RV: 114 | // faster (more noise?) 115 | // coordinate transform from Y-forward (w, x, y, z) to X-forward (w_, x_, y_, z_) = (w, y, -x, z) 116 | readings.rw = sensor_value.un.gyroIntegratedRV.real; 117 | readings.rx = sensor_value.un.gyroIntegratedRV.j; 118 | readings.ry = -sensor_value.un.gyroIntegratedRV.i; 119 | readings.rz = sensor_value.un.gyroIntegratedRV.k; 120 | // digitalWrite(10, HIGH); 121 | quat_updated = 1; 122 | break; 123 | 124 | case SH2_GYROSCOPE_CALIBRATED: 125 | // coordinate transform from Y-forward (rx, ry, rz) to X-forward (rx_, ry_, rz_) = (ry, -rx, rz) 126 | readings.vx = sensor_value.un.gyroscope.y; 127 | readings.vy = -sensor_value.un.gyroscope.x; 128 | readings.vz = sensor_value.un.gyroscope.z; 129 | // digitalWrite(11, HIGH); 130 | gyro_updated = 1; 131 | break; 132 | 133 | // case SH2_GRAVITY: 134 | // readings.gx = sensor_value.un.gravity.x; 135 | // readings.gy = sensor_value.un.gravity.y; 136 | // readings.gz = sensor_value.un.gravity.z; 137 | // break; 138 | } 139 | } 140 | 141 | if (gyro_updated && quat_updated) { 142 | digitalWrite(3, HIGH); 143 | Serial.write((uint8_t *)&readings, sizeof(readings)); 144 | gyro_updated = 0; 145 | quat_updated = 0; 146 | // digitalWrite(10, LOW); 147 | // digitalWrite(11, LOW); 148 | digitalWrite(3, LOW); 149 | } 150 | 151 | } 152 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/policy/gamepad.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | """ 4 | Gamepad Controller Module for Berkeley Humanoid Lite 5 | 6 | This module implements UDP-based controllers for the Berkeley Humanoid Lite robot, 7 | supporting both gamepad and keyboard input devices. It handles command broadcasting 8 | over UDP for robot control modes and movement velocities. 9 | """ 10 | 11 | import threading 12 | from typing import Dict 13 | 14 | from inputs import get_gamepad 15 | 16 | 17 | class XInputEntry: 18 | """ 19 | Constants for gamepad button and axis mappings. 20 | 21 | This class defines the standard mapping for various gamepad controls, 22 | including analog sticks, triggers, d-pad, and buttons. 23 | """ 24 | AXIS_X_L = "ABS_X" 25 | AXIS_Y_L = "ABS_Y" 26 | AXIS_TRIGGER_L = "ABS_Z" 27 | AXIS_X_R = "ABS_RX" 28 | AXIS_Y_R = "ABS_RY" 29 | AXIS_TRIGGER_R = "ABS_RZ" 30 | 31 | BTN_HAT_X = "ABS_HAT0X" 32 | BTN_HAT_Y = "ABS_HAT0Y" 33 | 34 | BTN_A = "BTN_SOUTH" 35 | BTN_B = "BTN_EAST" 36 | BTN_X = "BTN_NORTH" 37 | BTN_Y = "BTN_WEST" 38 | BTN_BUMPER_L = "BTN_TL" 39 | BTN_BUMPER_R = "BTN_TR" 40 | BTN_THUMB_L = "BTN_THUMBL" 41 | BTN_THUMB_R = "BTN_THUMBR" 42 | BTN_BACK = "BTN_SELECT" 43 | BTN_START = "BTN_START" 44 | 45 | 46 | class Se2Gamepad: 47 | def __init__(self, 48 | stick_sensitivity: float = 1.0, 49 | dead_zone: float = 0.01, 50 | ) -> None: 51 | self.stick_sensitivity = stick_sensitivity 52 | self.dead_zone = dead_zone 53 | 54 | self._stopped = threading.Event() 55 | self._run_forever_thread = None 56 | 57 | self.reset() 58 | 59 | self.commands = { 60 | "velocity_x": 0.0, 61 | "velocity_y": 0.0, 62 | "velocity_yaw": 0.0, 63 | "mode_switch": 0, 64 | } 65 | 66 | def reset(self) -> None: 67 | self._states = {key: 0 for key in XInputEntry.__dict__.values()} 68 | 69 | def stop(self) -> None: 70 | print("Gamepad stopping...") 71 | self._stopped.set() 72 | # self._run_forever_thread.join() 73 | 74 | def run(self) -> None: 75 | self._run_forever_thread = threading.Thread(target=self.run_forever) 76 | self._run_forever_thread.start() 77 | 78 | def run_forever(self) -> None: 79 | while not self._stopped.is_set(): 80 | self.advance() 81 | 82 | def advance(self) -> None: 83 | events = get_gamepad() 84 | 85 | # update all events from the joystick 86 | for event in events: 87 | self._states[event.code] = event.state 88 | 89 | self._update_command_buffer() 90 | 91 | def _update_command_buffer(self) -> Dict[str, float]: 92 | velocity_x = self._states.get(XInputEntry.AXIS_Y_L) 93 | velocity_y = self._states.get(XInputEntry.AXIS_X_R) 94 | velocity_yaw = self._states.get(XInputEntry.AXIS_X_L) 95 | 96 | if velocity_x is not None: 97 | self.commands["velocity_x"] = velocity_x / -32768.0 98 | if velocity_y is not None: 99 | self.commands["velocity_y"] = velocity_y / -32768.0 100 | if velocity_yaw is not None: 101 | self.commands["velocity_yaw"] = velocity_yaw / -32768.0 102 | 103 | mode_switch = 0 104 | 105 | # Enter RL control mode (A + Right Bumper) 106 | if self._states.get(XInputEntry.BTN_A) and self._states.get(XInputEntry.BTN_BUMPER_R): 107 | mode_switch = 3 108 | 109 | # Enter init mode (A + Left Bumper) 110 | if self._states.get(XInputEntry.BTN_A) and self._states.get(XInputEntry.BTN_BUMPER_L): 111 | mode_switch = 2 112 | 113 | # Enter idle mode (B or Left/Right Thumbstick) 114 | if self._states.get(XInputEntry.BTN_X) or self._states.get(XInputEntry.BTN_THUMB_L) or self._states.get(XInputEntry.BTN_THUMB_R): 115 | mode_switch = 1 116 | 117 | self.commands["mode_switch"] = mode_switch 118 | 119 | 120 | if __name__ == "__main__": 121 | command_controller = Se2Gamepad() 122 | command_controller.run() 123 | 124 | try: 125 | while True: 126 | print(f"""{command_controller.commands.get("velocity_x"):.2f}, {command_controller.commands.get("velocity_y"):.2f}, {command_controller.commands.get("velocity_yaw"):.2f}""") 127 | pass 128 | except KeyboardInterrupt: 129 | print("Keyboard interrupt") 130 | 131 | command_controller.stop() 132 | -------------------------------------------------------------------------------- /scripts/motor/configure_parameter.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 4 | 5 | 6 | args = recoil.util.get_args() 7 | bus = recoil.Bus(channel=args.channel, bitrate=1000000) 8 | 9 | device_id = args.id 10 | 11 | 12 | def configure_fast_frame_rate(bus: recoil.Bus, device_id: int, fast_frame_rate: int): 13 | print("Rate (before):\t", bus._read_parameter_u32(device_id, recoil.Parameter.FAST_FRAME_FREQUENCY)) 14 | bus._write_parameter_u32(device_id, recoil.Parameter.FAST_FRAME_FREQUENCY, fast_frame_rate) 15 | print("Rate (updated):\t", bus._read_parameter_u32(device_id, recoil.Parameter.FAST_FRAME_FREQUENCY)) 16 | 17 | 18 | def configure_gear_ratio(bus: recoil.Bus, device_id: int, gear_ratio: float): 19 | print("Gear Ratio (before):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_GEAR_RATIO)) 20 | bus._write_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_GEAR_RATIO, -gear_ratio) 21 | print("Gear Ratio (updated):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_GEAR_RATIO)) 22 | 23 | 24 | def configure_position_pd(bus: recoil.Bus, device_id: int, kp: float, kd: float): 25 | print("Kp (before):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_KP)) 26 | bus._write_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_KP, kp) 27 | print("Kp (updated):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_KP)) 28 | 29 | print("Kd (before):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_VELOCITY_KP)) 30 | bus._write_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_VELOCITY_KP, kd) 31 | print("Kd (updated):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_VELOCITY_KP)) 32 | 33 | 34 | def configure_torque_limit(bus: recoil.Bus, device_id: int, torque_limit: float): 35 | print("Torque Limit (before):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_TORQUE_LIMIT)) 36 | bus._write_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_TORQUE_LIMIT, torque_limit) 37 | print("Torque Limit (updated):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_TORQUE_LIMIT)) 38 | 39 | 40 | def configure_phase_order(bus: recoil.Bus, device_id: int, phase_order: int): 41 | print("Phase order (before):\t", bus._read_parameter_i32(device_id, recoil.Parameter.MOTOR_PHASE_ORDER)) 42 | bus._write_parameter_i32(device_id, recoil.Parameter.MOTOR_PHASE_ORDER, phase_order) 43 | print("Phase order (updated):\t", bus._read_parameter_i32(device_id, recoil.Parameter.MOTOR_PHASE_ORDER)) 44 | 45 | 46 | def configure_position_limit(bus: recoil.Bus, device_id: int, lower_limit: float, upper_limit: float): 47 | print("Position Lower Limit (before):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_LIMIT_LOWER)) 48 | bus._write_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_LIMIT_LOWER, lower_limit) 49 | print("Position Lower Limit (updated):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_LIMIT_LOWER)) 50 | 51 | print("Position Upper Limit (before):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_LIMIT_UPPER)) 52 | bus._write_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_LIMIT_UPPER, upper_limit) 53 | print("Position Upper Limit (updated):\t", bus._read_parameter_f32(device_id, recoil.Parameter.POSITION_CONTROLLER_POSITION_LIMIT_UPPER)) 54 | 55 | 56 | def configure_current_bandwidth(bus: recoil.Bus, device_id: int, bandwidth: float, kp: float = 0.0001, ki: float = 0.000001): 57 | print("Previous current kp:\t", bus._read_parameter_f32(device_id, recoil.Parameter.CURRENT_CONTROLLER_I_KP)) 58 | print("Previous current ki:\t", bus._read_parameter_f32(device_id, recoil.Parameter.CURRENT_CONTROLLER_I_KI)) 59 | 60 | bus.set_current_bandwidth(bandwidth, kp, ki) 61 | 62 | print("New current kp:\t", bus._read_parameter_f32(device_id, recoil.Parameter.CURRENT_CONTROLLER_I_KP)) 63 | print("New current ki:\t", bus._read_parameter_f32(device_id, recoil.Parameter.CURRENT_CONTROLLER_I_KI)) 64 | 65 | 66 | def store_to_flash(bus: recoil.Bus, device_id: int): 67 | bus.store_setting_to_flash(device_id) 68 | print("Settings stored to flash!") 69 | 70 | 71 | # configure_fast_frame_rate(motor, 0) 72 | 73 | # configure_gear_ratio(motor, 15) 74 | 75 | # configure_position_pd(motor, 20, 1) 76 | 77 | # configure_torque_limit(motor, 2) 78 | 79 | # configure_phase_order(motor, -1) 80 | 81 | # pi = math.pi 82 | 83 | # d90 = 0.5 * pi 84 | # d60 = 1./3. * math.pi 85 | # d45 = 0.25 * math.pi 86 | # d30 = 1./6. * math.pi 87 | # d10 = 0.5 * pi/9 88 | 89 | # configure_position_limit(motor, -d90, d90) 90 | 91 | # configure_current_bandwidth(motor, 100) 92 | 93 | 94 | # store_to_flash(motor) 95 | 96 | 97 | bus.stop() 98 | -------------------------------------------------------------------------------- /csrc/socketcan.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 3/3/21. 36 | // 37 | #include "socketcan.h" 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | 45 | /* ref: 46 | * https://github.com/JCube001/socketcan-demo 47 | * http://blog.mbedded.ninja/programming/operating-systems/linux/how-to-use-socketcan-with-c-in-linux 48 | * https://github.com/linux-can/can-utils/blob/master/candump.c 49 | */ 50 | 51 | SocketCan::~SocketCan() 52 | { 53 | if (isOpen()) 54 | close(); 55 | } 56 | 57 | bool SocketCan::open(const std::string &interface) 58 | { 59 | // Request a socket 60 | sock_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); 61 | if (sock_fd_ == -1) 62 | { 63 | std::cerr << "Error: Unable to create a CAN socket" << std::endl; 64 | return false; 65 | } 66 | char name[16] = {}; // avoid stringop-truncation 67 | strncpy(name, interface.c_str(), interface.size()); 68 | strncpy(interface_request_.ifr_name, name, IFNAMSIZ); 69 | // Get the index of the network interface 70 | if (ioctl(sock_fd_, SIOCGIFINDEX, &interface_request_) == -1) 71 | { 72 | std::cerr << "Unable to select CAN interface " << name << ": I/O control error" << std::endl; 73 | // Invalidate unusable socket 74 | close(); 75 | return false; 76 | } 77 | // Bind the socket to the network interface 78 | address_.can_family = AF_CAN; 79 | address_.can_ifindex = interface_request_.ifr_ifindex; 80 | const int rc = bind(sock_fd_, reinterpret_cast(&address_), 81 | sizeof(address_)); 82 | if (rc == -1) 83 | { 84 | std::cerr << "Failed to bind socket to " << name << " network interface" << std::endl; 85 | close(); 86 | return false; 87 | } 88 | return true; 89 | } 90 | 91 | void SocketCan::close() 92 | { 93 | if (!isOpen()) 94 | return; 95 | // Close the file descriptor for our socket 96 | ::close(sock_fd_); 97 | sock_fd_ = -1; 98 | } 99 | 100 | bool SocketCan::isOpen() const { return (sock_fd_ != -1); } 101 | 102 | void SocketCan::write(can_frame *frame) const 103 | { 104 | if (!isOpen()) 105 | { 106 | std::cerr << "Unable to write: Socket " << interface_request_.ifr_name << " not open" << std::endl; 107 | return; 108 | } 109 | if (::write(sock_fd_, frame, sizeof(can_frame)) == -1) 110 | std::cerr << "Unable to write: The " << interface_request_.ifr_name << " tx buffer may be full" << std::endl; 111 | } 112 | 113 | can_frame SocketCan::read() 114 | { 115 | 116 | // Holds the set of descriptors, that 'select' shall monitor 117 | fd_set descriptors; 118 | // Highest file descriptor in set 119 | int maxfd = sock_fd_; 120 | // How long 'select' shall wait before returning with timeout 121 | timeval timeout{}; 122 | // Buffer to store incoming frame 123 | can_frame rx_frame{}; 124 | // Run until termination signal received 125 | timeout.tv_sec = 1; // Should be set each loop 126 | // Clear descriptor set 127 | FD_ZERO(&descriptors); 128 | // Add socket descriptor 129 | FD_SET(sock_fd_, &descriptors); 130 | // Wait until timeout or activity on any descriptor 131 | if (select(maxfd + 1, &descriptors, nullptr, nullptr, &timeout)) 132 | { 133 | ::read(sock_fd_, &rx_frame, CAN_MTU); 134 | return rx_frame; 135 | } else { 136 | printf("[ERROR] : Timeout reading from socket\n"); 137 | return {}; 138 | } 139 | } 140 | -------------------------------------------------------------------------------- /scripts/robot/write_configurations.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import json 4 | import time 5 | 6 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 7 | from berkeley_humanoid_lite_lowlevel.robot import Humanoid 8 | 9 | 10 | robot_configuration = json.load(open("robot_configuration.json")) 11 | 12 | robot = Humanoid() 13 | 14 | delay_t = 0.1 15 | 16 | store_to_flash = True 17 | 18 | 19 | for entry in robot.joints: 20 | bus, joint_id, joint_name = entry 21 | print(f"Pinging {joint_name} ... ", end="\t") 22 | result = bus.ping(joint_id) 23 | print(f"success: {result}") 24 | 25 | firmware_version = hex(bus._read_parameter_u32(joint_id, recoil.Parameter.FIRMWARE_VERSION)) 26 | print(f" firmware version: {firmware_version}") 27 | 28 | time.sleep(0.1) 29 | 30 | 31 | for entry in robot.joints: 32 | bus, joint_id, joint_name = entry 33 | 34 | print(f"Writing configuration for {joint_name}") 35 | 36 | config = robot_configuration.get(joint_name) 37 | if not config: 38 | raise ValueError(f"No configuration found for {joint_name}") 39 | 40 | val = config["fast_frame_frequency"] 41 | print(f" setting fast frame frequency to {val}") 42 | bus.write_fast_frame_frequency(joint_id, val) 43 | time.sleep(delay_t) 44 | 45 | val = config["position_controller"]["gear_ratio"] 46 | print(f" setting gear ratio to {val}") 47 | bus._write_parameter_f32(joint_id, recoil.Parameter.POSITION_CONTROLLER_GEAR_RATIO, val) 48 | time.sleep(delay_t) 49 | 50 | val = config["position_controller"]["position_kp"] 51 | print(f" setting KP to {val}") 52 | bus.write_position_kp(joint_id, val) 53 | time.sleep(delay_t) 54 | 55 | val = config["position_controller"]["position_ki"] 56 | print(f" setting KI to {val}") 57 | bus.write_position_ki(joint_id, val) 58 | time.sleep(delay_t) 59 | 60 | val = config["position_controller"]["velocity_kp"] 61 | print(f" setting KD to {val}") 62 | bus.write_velocity_kp(joint_id, val) 63 | time.sleep(delay_t) 64 | 65 | val = config["position_controller"]["velocity_ki"] 66 | print(f" setting velocity KI to {val}") 67 | bus.write_velocity_ki(joint_id, val) 68 | time.sleep(delay_t) 69 | 70 | val = config["position_controller"]["torque_limit"] 71 | print(f" setting torque limit to {val}") 72 | bus.write_torque_limit(joint_id, val) 73 | time.sleep(delay_t) 74 | 75 | val = config["position_controller"]["velocity_limit"] 76 | print(f" setting velocity limit to {val}") 77 | bus.write_velocity_limit(joint_id, val) 78 | time.sleep(delay_t) 79 | 80 | val = config["position_controller"]["position_limit_lower"] 81 | print(f" setting position limit lower to {val}") 82 | bus.write_position_limit_lower(joint_id, val) 83 | time.sleep(delay_t) 84 | 85 | val = config["position_controller"]["position_limit_upper"] 86 | print(f" setting position limit upper to {val}") 87 | bus.write_position_limit_upper(joint_id, val) 88 | time.sleep(delay_t) 89 | 90 | val = config["position_controller"]["position_offset"] 91 | print(f" setting position offset to {val}") 92 | bus.write_position_offset(joint_id, val) 93 | time.sleep(delay_t) 94 | 95 | val = config["position_controller"]["torque_filter_alpha"] 96 | print(f" setting torque filter alpha to {val}") 97 | bus.write_torque_filter_alpha(joint_id, val) 98 | time.sleep(delay_t) 99 | 100 | val = config["current_controller"]["i_limit"] 101 | print(f" setting current limit to {val}") 102 | bus.write_current_limit(joint_id, val) 103 | time.sleep(delay_t) 104 | 105 | val = config["current_controller"]["i_kp"] 106 | print(f" setting current Kp to {val}") 107 | bus.write_current_kp(joint_id, val) 108 | time.sleep(delay_t) 109 | 110 | val = config["current_controller"]["i_ki"] 111 | print(f" setting current Ki to {val}") 112 | bus.write_current_ki(joint_id, val) 113 | time.sleep(delay_t) 114 | 115 | val = config["motor"]["pole_pairs"] 116 | print(f" setting pole pairs to {val}") 117 | bus.write_motor_pole_pairs(joint_id, val) 118 | time.sleep(delay_t) 119 | 120 | val = config["motor"]["torque_constant"] 121 | print(f" setting torque constant to {val}") 122 | bus.write_motor_torque_constant(joint_id, val) 123 | time.sleep(delay_t) 124 | 125 | val = config["motor"]["phase_order"] 126 | print(f" setting phase order to {val}") 127 | bus.write_motor_phase_order(joint_id, val) 128 | time.sleep(delay_t) 129 | 130 | val = config["motor"]["max_calibration_current"] 131 | print(f" setting max calibration current to {val}") 132 | bus.write_motor_calibration_current(joint_id, val) 133 | time.sleep(delay_t) 134 | 135 | val = config["encoder"]["cpr"] 136 | print(f" setting cpr to {val}") 137 | bus.write_encoder_cpr(joint_id, val) 138 | time.sleep(delay_t) 139 | 140 | val = config["encoder"]["position_offset"] 141 | print(f" setting position offset to {val}") 142 | bus.write_encoder_position_offset(joint_id, val) 143 | time.sleep(delay_t) 144 | 145 | val = config["encoder"]["velocity_filter_alpha"] 146 | print(f" setting velocity filter alpha to {val}") 147 | bus.write_encoder_velocity_filter_alpha(joint_id, val) 148 | time.sleep(delay_t) 149 | 150 | val = config["encoder"]["flux_offset"] 151 | print(f" setting flux offset to {val}") 152 | bus.write_encoder_flux_offset(joint_id, val) 153 | time.sleep(delay_t) 154 | 155 | if store_to_flash: 156 | print(" storing to flash") 157 | bus.store_settings_to_flash(joint_id) 158 | time.sleep(0.2) 159 | 160 | time.sleep(0.5) 161 | 162 | 163 | robot.stop() 164 | -------------------------------------------------------------------------------- /csrc/real_humanoid.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #pragma once 4 | 5 | 6 | #include 7 | #include 8 | 9 | #include "consts.h" 10 | #include "loop_function.h" 11 | #include "motor_controller.h" 12 | #include "imu.h" 13 | #include "udp.h" 14 | #include "socketcan.h" 15 | 16 | 17 | 18 | 19 | #define N_LOWLEVEL_STATES (4+3+12+12+1+3) 20 | #define N_LOWLEVEL_COMMANDS 12 21 | 22 | 23 | 24 | static inline float deg2rad(float deg) { 25 | return deg * M_PI / 180.0f; 26 | } 27 | 28 | 29 | enum ControllerState { 30 | STATE_ERROR = 0, 31 | STATE_IDLE = 1, 32 | STATE_RL_INIT, 33 | STATE_RL_RUNNING, 34 | STATE_GETUP, 35 | STATE_HOLD, 36 | STATE_GETDOWN, 37 | }; 38 | 39 | 40 | /** 41 | * Joint order: 42 | * 0: left_hip_roll 43 | * 1: left_hip_yaw 44 | * 2: left_hip_pitch 45 | * 3: left_knee_pitch 46 | * 4: left_ankle_pitch 47 | * 5: left_ankle_roll 48 | * 6: right_hip_roll 49 | * 7: right_hip_yaw 50 | * 8: right_hip_pitch 51 | * 9: right_knee_pitch 52 | * 10: right_ankle_pitch 53 | * 11: right_ankle_roll 54 | */ 55 | enum JointOrder { 56 | LEG_LEFT_HIP_ROLL_JOINT = 0, 57 | LEG_LEFT_HIP_YAW_JOINT = 1, 58 | LEG_LEFT_HIP_PITCH_JOINT = 2, 59 | LEG_LEFT_KNEE_PITCH_JOINT = 3, 60 | LEG_LEFT_ANKLE_PITCH_JOINT = 4, 61 | LEG_LEFT_ANKLE_ROLL_JOINT = 5, 62 | LEG_RIGHT_HIP_ROLL_JOINT = 6, 63 | LEG_RIGHT_HIP_YAW_JOINT = 7, 64 | LEG_RIGHT_HIP_PITCH_JOINT = 8, 65 | LEG_RIGHT_KNEE_PITCH_JOINT = 9, 66 | LEG_RIGHT_ANKLE_PITCH_JOINT = 10, 67 | LEG_RIGHT_ANKLE_ROLL_JOINT = 11, 68 | }; 69 | // enum JointOrder { 70 | // ARM_LEFT_SHOULDER_PITCH_JOINT = 0, 71 | // ARM_LEFT_SHOULDER_ROLL_JOINT = 1, 72 | // ARM_LEFT_SHOULDER_YAW_JOINT = 2, 73 | // ARM_LEFT_ELBOW_PITCH_JOINT = 3, 74 | // ARM_LEFT_ELBOW_ROLL_JOINT = 4, 75 | // ARM_RIGHT_SHOULDER_PITCH_JOINT = 5, 76 | // ARM_RIGHT_SHOULDER_ROLL_JOINT = 6, 77 | // ARM_RIGHT_SHOULDER_YAW_JOINT = 7, 78 | // ARM_RIGHT_ELBOW_PITCH_JOINT = 8, 79 | // ARM_RIGHT_ELBOW_ROLL_JOINT = 9, 80 | // LEG_LEFT_HIP_ROLL_JOINT = 10, 81 | // LEG_LEFT_HIP_YAW_JOINT = 11, 82 | // LEG_LEFT_HIP_PITCH_JOINT = 12, 83 | // LEG_LEFT_KNEE_PITCH_JOINT = 13, 84 | // LEG_LEFT_ANKLE_PITCH_JOINT = 14, 85 | // LEG_LEFT_ANKLE_ROLL_JOINT = 15, 86 | // LEG_RIGHT_HIP_ROLL_JOINT = 16, 87 | // LEG_RIGHT_HIP_YAW_JOINT = 17, 88 | // LEG_RIGHT_HIP_PITCH_JOINT = 18, 89 | // LEG_RIGHT_KNEE_PITCH_JOINT = 19, 90 | // LEG_RIGHT_ANKLE_PITCH_JOINT = 20, 91 | // LEG_RIGHT_ANKLE_ROLL_JOINT = 21, 92 | // }; 93 | 94 | 95 | class RealHumanoid { 96 | public: 97 | float position_target[N_JOINTS]; 98 | float position_measured[N_JOINTS]; 99 | float velocity_measured[N_JOINTS]; 100 | 101 | float starting_positions[N_JOINTS]; 102 | 103 | float position_offsets[N_JOINTS] = {0}; 104 | 105 | float joint_kp[N_JOINTS] = {0}; 106 | float joint_kd[N_JOINTS] = {0}; 107 | float torque_limit[N_JOINTS] = {0}; 108 | 109 | float rl_init_positions[N_JOINTS] = { 110 | 0.0, 0.0, -0.2, 111 | 0.4, 112 | -0.3, 0.0, 113 | 0.0, 0.0, -0.2, 114 | 0.4, 115 | -0.3, 0.0 116 | }; 117 | 118 | 119 | float joint_axis_directions[N_JOINTS] = { 120 | -1, 1, -1, 121 | -1, 122 | -1, 1, 123 | -1, 1, 1, 124 | 1, 125 | 1, 1 126 | }; 127 | 128 | RealHumanoid(); 129 | ~RealHumanoid(); 130 | 131 | /** 132 | * Stop the humanoid low-level control. 133 | * 134 | * This method will join all the threads. On first execution, it will set motor to damping 135 | * mode; on second execution, it will set motor to idle mode and exits the main loop. 136 | */ 137 | void stop(); 138 | 139 | /** 140 | * Start the humanoid low-level control. 141 | * 142 | * This method will initialize the low-level components of the robot, create the udp 143 | * communication threads, and start the control loop. 144 | */ 145 | void run(); 146 | 147 | void run_calibration(); 148 | 149 | private: 150 | /* Low-level components */ 151 | uint8_t stopped = 0; 152 | ControllerState state = STATE_IDLE; 153 | ControllerState next_state = STATE_IDLE; 154 | 155 | float config_control_dt_ = 0.; 156 | float config_policy_dt_ = 0.; 157 | 158 | float stick_command_velocity_x_ = 0.0; 159 | float stick_command_velocity_y_ = 0.0; 160 | float stick_command_velocity_yaw_ = 0.0; 161 | 162 | std::string config_udp_host_addr_; 163 | 164 | /* devices */ 165 | IMU *imu; 166 | SocketCan left_arm_bus; 167 | SocketCan right_arm_bus; 168 | SocketCan left_leg_bus; 169 | SocketCan right_leg_bus; 170 | 171 | std::array, N_JOINTS> joint_ptrs; 172 | 173 | 174 | uint8_t control_loop_count = 0; 175 | 176 | float init_percentage = 0.0; 177 | 178 | /* Threading */ 179 | std::shared_ptr loop_control; 180 | std::shared_ptr loop_udp_recv; 181 | std::shared_ptr loop_keyboard; 182 | std::shared_ptr loop_joystick; 183 | std::shared_ptr loop_imu; 184 | 185 | /* UDP stuff */ 186 | UDP udp; 187 | float lowlevel_commands[N_LOWLEVEL_COMMANDS]; 188 | float lowlevel_states[N_LOWLEVEL_STATES]; 189 | 190 | UDP udp_joystick; 191 | 192 | /* Policy stuff */ 193 | // torch::Tensor policy_observations; 194 | // torch::Tensor policy_actions; 195 | 196 | 197 | void initialize(); 198 | 199 | /** 200 | * The control loop that communicates with the hardware. 201 | * 202 | * This loop will run at 100 Hz. 203 | * 204 | * On each loop iteration, it will perform the following: 205 | * 1. collects the action terms from action array set by either UDP communication or the policy. 206 | * 2. triggers an IMU update. 207 | * 3. receives the data from IMU. 208 | * 4. sends the target positions to joints and reads the meaured positions, with some delay between each joint. 209 | * 5. populates the observation array. 210 | */ 211 | void control_loop(); 212 | 213 | void keyboard_loop(); 214 | 215 | void joystick_loop(); 216 | 217 | void imu_loop(); 218 | 219 | void process_actions(); 220 | 221 | void process_observations(); 222 | 223 | void policy_forward(); 224 | 225 | void udp_recv(); 226 | 227 | void update_imu(); 228 | 229 | void update_joints(); 230 | }; 231 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/robot/bimanual.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import struct 4 | import time 5 | 6 | import serial 7 | import numpy as np 8 | 9 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 10 | 11 | 12 | class Bimanual: 13 | def __init__(self): 14 | 15 | self.left_arm_transport = recoil.Bus("can0") 16 | self.right_arm_transport = recoil.Bus("can1") 17 | self.gripper = serial.Serial("/dev/ttyUSB0", 115200) 18 | 19 | self.joints = [ 20 | (self.left_arm_transport, 1, "left_shoulder_pitch_joint"), 21 | (self.left_arm_transport, 3, "left_shoulder_roll_joint"), 22 | (self.left_arm_transport, 5, "left_shoulder_yaw_joint"), 23 | (self.left_arm_transport, 7, "left_elbow_joint"), 24 | (self.left_arm_transport, 9, "left_wrist_yaw_joint"), 25 | 26 | (self.right_arm_transport, 2, "right_shoulder_pitch_joint"), 27 | (self.right_arm_transport, 4, "right_shoulder_roll_joint"), 28 | (self.right_arm_transport, 6, "right_shoulder_yaw_joint"), 29 | (self.right_arm_transport, 8, "right_elbow_joint"), 30 | (self.right_arm_transport, 10, "right_wrist_yaw_joint"), 31 | ] 32 | 33 | self.joint_axis_directions = np.array([ 34 | +1, +1, -1, -1, -1, 35 | -1, +1, -1, +1, -1, 36 | +1, +1, # gripper 37 | ], dtype=np.float32) 38 | 39 | self.position_offsets = np.array([ 40 | 0.0, 0.0, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 0.0, 0.0, 0.0, 42 | 0.2, 0.2, # gripper 43 | ], dtype=np.float32) 44 | 45 | self.joint_position_target = np.zeros(len(self.joints), dtype=np.float32) 46 | self.joint_position_measured = np.zeros(len(self.joints), dtype=np.float32) 47 | self.joint_velocity_measured = np.zeros(len(self.joints), dtype=np.float32) 48 | self.gripper_left_target = 0.5 49 | self.gripper_right_target = 0.5 50 | 51 | def start(self, kp=20, kd=2, torque_limit=1): 52 | self.joint_kp = np.zeros((len(self.joints),), dtype=np.float32) 53 | self.joint_kd = np.zeros((len(self.joints),), dtype=np.float32) 54 | self.torque_limit = np.zeros((len(self.joints),), dtype=np.float32) 55 | 56 | self.joint_kp[:] = kp 57 | self.joint_kd[:] = kd 58 | self.torque_limit[:] = torque_limit 59 | 60 | for i, entry in enumerate(self.joints): 61 | bus, device_id, joint_name = entry 62 | 63 | print(f"Initializing joint {joint_name}:") 64 | print(f" kp: {self.joint_kp[i]}, kd: {self.joint_kd[i]}, torque limit: {self.torque_limit[i]}") 65 | 66 | # Set the joint mode to idle 67 | bus.set_mode(device_id, recoil.Mode.IDLE) 68 | time.sleep(0.001) 69 | bus.write_position_kp(device_id, self.joint_kp[i]) 70 | time.sleep(0.001) 71 | bus.write_position_kd(device_id, self.joint_kd[i]) 72 | time.sleep(0.001) 73 | bus.write_torque_limit(device_id, self.torque_limit[i]) 74 | time.sleep(0.001) 75 | bus.feed(device_id) 76 | bus.set_mode(device_id, recoil.Mode.POSITION) 77 | 78 | self.position_offsets[i] = bus.read_position_measured(device_id) * self.joint_axis_directions[i] 79 | 80 | print("Motors enabled") 81 | print(self.position_offsets) 82 | 83 | def stop(self): 84 | for entry in self.joints: 85 | bus, device_id, _ = entry 86 | bus.set_mode(device_id, recoil.Mode.DAMPING) 87 | 88 | print("Entered damping mode. Press Ctrl+C again to exit.\n") 89 | 90 | try: 91 | while True: 92 | pass 93 | except KeyboardInterrupt: 94 | print("Exiting damping mode.") 95 | 96 | for entry in self.joints: 97 | bus, device_id, _ = entry 98 | bus.set_mode(device_id, recoil.Mode.IDLE) 99 | 100 | self.left_arm_transport.stop() 101 | self.right_arm_transport.stop() 102 | 103 | def get_observations(self) -> np.ndarray: 104 | return np.concatenate([ 105 | self.joint_position_measured[:], 106 | [self.gripper_left_target, self.gripper_right_target], 107 | ]) 108 | 109 | def update_joint_group(self, joint_id_l, joint_id_r): 110 | # adjust direction and offset of target values 111 | position_target_l = (self.joint_position_target[joint_id_l] + self.position_offsets[joint_id_l]) * self.joint_axis_directions[joint_id_l] 112 | position_target_r = (self.joint_position_target[joint_id_r] + self.position_offsets[joint_id_r]) * self.joint_axis_directions[joint_id_r] 113 | 114 | self.joints[joint_id_l][0].transmit_pdo_2(self.joints[joint_id_l][1], position_target=position_target_l, velocity_target=0.0) 115 | self.joints[joint_id_r][0].transmit_pdo_2(self.joints[joint_id_r][1], position_target=position_target_r, velocity_target=0.0) 116 | 117 | position_measured_l, velocity_measured_l = self.joints[joint_id_l][0].receive_pdo_2(self.joints[joint_id_l][1]) 118 | position_measured_r, velocity_measured_r = self.joints[joint_id_r][0].receive_pdo_2(self.joints[joint_id_r][1]) 119 | 120 | # adjust direction and offset of target values 121 | if position_measured_l is not None: 122 | self.joint_position_measured[joint_id_l] = (position_measured_l * self.joint_axis_directions[joint_id_l]) - self.position_offsets[joint_id_l] 123 | if velocity_measured_l is not None: 124 | self.joint_velocity_measured[joint_id_l] = velocity_measured_l * self.joint_axis_directions[joint_id_l] 125 | if position_measured_r is not None: 126 | self.joint_position_measured[joint_id_r] = (position_measured_r * self.joint_axis_directions[joint_id_r]) - self.position_offsets[joint_id_r] 127 | if velocity_measured_r is not None: 128 | self.joint_velocity_measured[joint_id_r] = velocity_measured_r * self.joint_axis_directions[joint_id_r] 129 | 130 | def update_joints(self): 131 | 132 | # communicate with actuators 133 | self.update_joint_group(0, 5) 134 | self.update_joint_group(1, 6) 135 | self.update_joint_group(2, 7) 136 | self.update_joint_group(3, 8) 137 | self.update_joint_group(4, 9) 138 | 139 | # communicate with gripper 140 | # 0.2: open 141 | # 0.85: closed 142 | # 0.9: tightly closed 143 | gripper_left_raw_value = 0.2 + self.gripper_left_target * 0.6 144 | gripper_right_raw_value = 0.2 + self.gripper_right_target * 0.6 145 | data = struct.pack(" np.ndarray: 29 | pass 30 | 31 | 32 | class TorchPolicy(Policy): 33 | """ 34 | PyTorch policy inference runner. 35 | 36 | Loads and executes PyTorch models for robot control policies. 37 | """ 38 | def __init__(self, checkpoint_path: str, device: str = "cpu"): 39 | self.device = device 40 | self.model: torch.nn.Module = torch.load(checkpoint_path, map_location=self.device) 41 | self.model.eval() 42 | 43 | def forward(self, observations: np.ndarray) -> np.ndarray: 44 | observations_tensor = torch.from_numpy(observations).unsqueeze(0).to(self.device) 45 | actions_tensor = self.model(observations_tensor) 46 | return actions_tensor.detach().cpu().squeeze(0).numpy() 47 | 48 | 49 | class OnnxPolicy(Policy): 50 | """ 51 | ONNX policy inference runner 52 | 53 | Loads and executes ONNX models for robot control policies. 54 | """ 55 | def __init__(self, checkpoint_path: str): 56 | self.model: ort.InferenceSession = ort.InferenceSession(checkpoint_path) 57 | 58 | input_shape = self.model.get_inputs()[0].shape 59 | try: 60 | self.model.run(None, {"obs": np.zeros(input_shape, dtype=np.float32)})[0] 61 | self.key = "obs" 62 | except Exception as e: 63 | print(e) 64 | self.key = "onnx::Gemm_0" 65 | 66 | def forward(self, observations: np.ndarray) -> np.ndarray: 67 | return np.array(self.model.run(None, {self.key: observations})[0]) 68 | 69 | 70 | class RlController: 71 | """ 72 | A class to run trained policies for the Berkeley Humanoid Lite robot. 73 | 74 | This class handles the execution of trained policies (PyTorch or ONNX format), 75 | processes robot observations, and sends control commands via UDP communication. 76 | """ 77 | 78 | @staticmethod 79 | def quat_rotate_inverse(q: np.ndarray, v: np.ndarray) -> np.ndarray: 80 | """ 81 | Rotate a vector by the inverse of a quaternion. 82 | 83 | Args: 84 | q (np.ndarray): Quaternion [w, x, y, z] 85 | v (np.ndarray): Vector to rotate 86 | 87 | Returns: 88 | np.ndarray: Rotated vector 89 | """ 90 | q_w = q[0] 91 | q_vec = q[1:4] 92 | a = v * (2.0 * q_w ** 2 - 1.0) 93 | b = np.cross(q_vec, v) * q_w * 2.0 94 | c = q_vec * (np.dot(q_vec, v)) * 2.0 95 | return a - b + c 96 | 97 | def __init__(self, cfg: Union[DictConfig, ListConfig]): 98 | """ 99 | Initialize the PolicyRunner. 100 | 101 | Args: 102 | cfg (Union[DictConfig, ListConfig]): Configuration object containing policy parameters 103 | """ 104 | self.cfg = cfg 105 | 106 | # for data logging 107 | self.data_log = [] 108 | self.counter = 0 109 | 110 | # Initialize robot state buffers 111 | self.command_velocity = np.array(self.cfg.command_velocity, dtype=np.float32) 112 | 113 | if self.cfg.num_actions == self.cfg.num_joints: 114 | self.default_joint_positions = np.array(self.cfg.default_joint_positions, dtype=np.float32) 115 | else: 116 | self.default_joint_positions = np.array(self.cfg.default_joint_positions[10:], dtype=np.float32) 117 | 118 | self.gravity_vector = np.array([0., 0., -1.], dtype=np.float32) 119 | 120 | # Initialize observation and action buffers 121 | self.policy_observations = np.zeros((1, self.cfg.num_observations * (self.cfg.history_length + 1)), dtype=np.float32) 122 | self.policy_actions = np.zeros((1, self.cfg.num_actions), dtype=np.float32) 123 | self.prev_actions = np.zeros((self.cfg.num_actions,), dtype=np.float32) 124 | 125 | def load_policy(self) -> None: 126 | """ 127 | Load the policy model (PyTorch or ONNX) 128 | """ 129 | model_checkpoint_path = self.cfg.policy_checkpoint_path 130 | 131 | # Determine policy format and load appropriate model 132 | if ".pt" in model_checkpoint_path: 133 | torch.set_printoptions(precision=2) 134 | self.policy = TorchPolicy(model_checkpoint_path) 135 | print("Using Torch runner") 136 | 137 | elif ".onnx" in model_checkpoint_path: 138 | self.policy = OnnxPolicy(model_checkpoint_path) 139 | print("Using ONNX runner") 140 | 141 | else: 142 | raise ValueError("Unrecognized policy format") 143 | 144 | def update(self, robot_observations: np.ndarray) -> np.ndarray: 145 | """ 146 | Run the policy execution loop. 147 | 148 | This method: 149 | 1. Loads the policy model (PyTorch or ONNX) 150 | 2. Sets up UDP communication 151 | 3. Runs the main control loop 152 | 4. Logs experiment data 153 | 154 | Args: 155 | robot_observations (np.ndarray): Observations from the robot low-level controller 156 | 157 | Returns: 158 | np.ndarray: Actions to send to the robot 159 | """ 160 | 161 | # Parse UDP observations 162 | robot_base_quat = robot_observations[0:4] 163 | robot_base_ang_vel = robot_observations[4:7] 164 | robot_joint_pos = robot_observations[7:7 + self.cfg.num_actions] - self.default_joint_positions 165 | robot_joint_vel = robot_observations[7 + self.cfg.num_actions:7 + self.cfg.num_actions * 2] 166 | # robot_mode = robot_observations[7 + self.cfg.num_actions * 2] 167 | command_velocity = robot_observations[7 + self.cfg.num_actions * 2 + 1:7 + self.cfg.num_actions * 2 + 4] 168 | 169 | # Process observations 170 | base_ang_vel = robot_base_ang_vel 171 | projected_gravity = self.quat_rotate_inverse(robot_base_quat, self.gravity_vector) 172 | joint_pos = robot_joint_pos 173 | joint_vel = robot_joint_vel 174 | 175 | # Update observation buffer 176 | self.policy_observations[:] = np.concatenate([ 177 | self.policy_observations[0, self.cfg.num_observations:], 178 | command_velocity, 179 | base_ang_vel, 180 | projected_gravity, 181 | joint_pos, 182 | joint_vel, 183 | self.prev_actions 184 | ], axis=0) 185 | 186 | # Execute policy 187 | self.policy_actions[:] = self.policy.forward(self.policy_observations) 188 | 189 | # Process and scale actions 190 | policy_actions_clipped = np.clip(self.policy_actions[0], 191 | self.cfg.action_limit_lower, 192 | self.cfg.action_limit_upper) 193 | self.prev_actions[:] = policy_actions_clipped 194 | 195 | policy_actions_scaled = policy_actions_clipped * self.cfg.action_scale + self.default_joint_positions 196 | 197 | # # Log data 198 | # data_log.append(np.concatenate([[time.time()], policy_observations.flatten()]).tolist()) 199 | # counter += 1 200 | 201 | # # Save experiment data 202 | # with open("data_log.json", "w") as f: 203 | # json.dump(data_log, f) 204 | # print("Written experiment data to ./data_log.json") 205 | 206 | return policy_actions_scaled 207 | -------------------------------------------------------------------------------- /csrc/motor_controller_conf.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #pragma once 4 | 5 | 6 | /** ======== Controller Settings ======== **/ 7 | 8 | /** 9 | * Firmware Version: 10 | * The firmware version is represented as a 32-bit hexadecimal number. 11 | * It follows the format: (MAJOR [7:4]) . (MINOR [3:2]) . (PATCH [1:0]). 12 | * For example, 0x00010005 represents version 1.0.5 of the firmware. 13 | */ 14 | // YYYYmmdd 15 | #define FIRMWARE_VERSION 0x20250226 16 | 17 | 18 | /** ======== Controller State Definitions ======== **/ 19 | 20 | /** 21 | * @brief Mode definition. 22 | */ 23 | typedef enum { 24 | // these are three safe modes 25 | MODE_DISABLED = 0x00U, 26 | MODE_IDLE = 0x01U, // operational 27 | 28 | // these are special modes 29 | MODE_DAMPING = 0x02U, // stopped 30 | MODE_CALIBRATION = 0x05U, 31 | 32 | // these are closed-loop modes 33 | MODE_CURRENT = 0x10U, 34 | MODE_TORQUE = 0x11U, 35 | MODE_VELOCITY = 0x12U, 36 | MODE_POSITION = 0x13U, 37 | 38 | // these are open-loop modes 39 | MODE_VABC_OVERRIDE = 0x20U, 40 | MODE_VALPHABETA_OVERRIDE = 0x21U, 41 | MODE_VQD_OVERRIDE = 0x22U, 42 | 43 | MODE_DEBUG = 0x80U, // pre-operational 44 | } Mode; 45 | 46 | /** 47 | * @brief ErrorCode definition. 48 | */ 49 | typedef enum { 50 | ERROR_NO_ERROR = 0b0000000000000000U, 51 | ERROR_GENERAL = 0b0000000000000001U, 52 | ERROR_ESTOP = 0b0000000000000010U, 53 | ERROR_INITIALIZATION_ERROR = 0b0000000000000100U, 54 | ERROR_CALIBRATION_ERROR = 0b0000000000001000U, 55 | ERROR_POWERSTAGE_ERROR = 0b0000000000010000U, 56 | ERROR_INVALID_MODE = 0b0000000000100000U, 57 | ERROR_WATCHDOG_TIMEOUT = 0b0000000001000000U, 58 | ERROR_OVER_VOLTAGE = 0b0000000010000000U, 59 | ERROR_OVER_CURRENT = 0b0000000100000000U, 60 | ERROR_OVER_TEMPERATURE = 0b0000001000000000U, 61 | ERROR_CAN_RX_FAULT = 0b0000010000000000U, 62 | ERROR_CAN_TX_FAULT = 0b0000100000000000U, 63 | ERROR_I2C_FAULT = 0b0001000000000000U, 64 | } ErrorCode; 65 | 66 | /** ======== CAN Packet Definitions ======== **/ 67 | /** 68 | * @brief CAN FrameFunction definition. 69 | */ 70 | typedef enum { 71 | FUNC_NMT = 0b0000U, 72 | FUNC_SYNC_EMCY = 0b0001U, 73 | FUNC_TIME = 0b0010U, 74 | FUNC_TRANSMIT_PDO_1 = 0b0011U, 75 | FUNC_RECEIVE_PDO_1 = 0b0100U, 76 | FUNC_TRANSMIT_PDO_2 = 0b0101U, 77 | FUNC_RECEIVE_PDO_2 = 0b0110U, 78 | FUNC_TRANSMIT_PDO_3 = 0b0111U, 79 | FUNC_RECEIVE_PDO_3 = 0b1000U, 80 | FUNC_TRANSMIT_PDO_4 = 0b1001U, 81 | FUNC_RECEIVE_PDO_4 = 0b1010U, 82 | FUNC_TRANSMIT_SDO = 0b1011U, 83 | FUNC_RECEIVE_SDO = 0b1100U, 84 | FUNC_FLASH = 0b1101U, 85 | FUNC_HEARTBEAT = 0b1110U, 86 | } FrameFunction; 87 | 88 | /** 89 | * @brief CAN Parameter ID definition. 90 | */ 91 | typedef enum { 92 | PARAM_DEVICE_ID = 0x000U, 93 | PARAM_FIRMWARE_VERSION = 0x004U, 94 | PARAM_WATCHDOG_TIMEOUT = 0x008U, 95 | PARAM_FAST_FRAME_FREQUENCY = 0x00CU, 96 | PARAM_MODE = 0x010U, 97 | PARAM_ERROR = 0x014U, 98 | PARAM_POSITION_CONTROLLER_UPDATE_COUNTER = 0x018U, 99 | PARAM_POSITION_CONTROLLER_GEAR_RATIO = 0x01CU, 100 | PARAM_POSITION_CONTROLLER_POSITION_KP = 0x020U, 101 | PARAM_POSITION_CONTROLLER_POSITION_KI = 0x024U, 102 | PARAM_POSITION_CONTROLLER_VELOCITY_KP = 0x028U, 103 | PARAM_POSITION_CONTROLLER_VELOCITY_KI = 0x02CU, 104 | PARAM_POSITION_CONTROLLER_TORQUE_LIMIT = 0x030U, 105 | PARAM_POSITION_CONTROLLER_VELOCITY_LIMIT = 0x034U, 106 | PARAM_POSITION_CONTROLLER_POSITION_LIMIT_LOWER = 0x038U, 107 | PARAM_POSITION_CONTROLLER_POSITION_LIMIT_UPPER = 0x03CU, 108 | PARAM_POSITION_CONTROLLER_POSITION_OFFSET = 0x040U, 109 | PARAM_POSITION_CONTROLLER_TORQUE_TARGET = 0x044U, 110 | PARAM_POSITION_CONTROLLER_TORQUE_MEASURED = 0x048U, 111 | PARAM_POSITION_CONTROLLER_TORQUE_SETPOINT = 0x04CU, 112 | PARAM_POSITION_CONTROLLER_VELOCITY_TARGET = 0x050U, 113 | PARAM_POSITION_CONTROLLER_VELOCITY_MEASURED = 0x054U, 114 | PARAM_POSITION_CONTROLLER_VELOCITY_SETPOINT = 0x058U, 115 | PARAM_POSITION_CONTROLLER_POSITION_TARGET = 0x05CU, 116 | PARAM_POSITION_CONTROLLER_POSITION_MEASURED = 0x060U, 117 | PARAM_POSITION_CONTROLLER_POSITION_SETPOINT = 0x064U, 118 | PARAM_POSITION_CONTROLLER_POSITION_INTEGRATOR = 0x068U, 119 | PARAM_POSITION_CONTROLLER_VELOCITY_INTEGRATOR = 0x06CU, 120 | PARAM_POSITION_CONTROLLER_TORQUE_FILTER_ALPHA = 0x070U, 121 | PARAM_CURRENT_CONTROLLER_I_LIMIT = 0x074U, 122 | PARAM_CURRENT_CONTROLLER_I_KP = 0x078U, 123 | PARAM_CURRENT_CONTROLLER_I_KI = 0x07CU, 124 | PARAM_CURRENT_CONTROLLER_I_A_MEASURED = 0x080U, 125 | PARAM_CURRENT_CONTROLLER_I_B_MEASURED = 0x084U, 126 | PARAM_CURRENT_CONTROLLER_I_C_MEASURED = 0x088U, 127 | PARAM_CURRENT_CONTROLLER_V_A_SETPOINT = 0x08CU, 128 | PARAM_CURRENT_CONTROLLER_V_B_SETPOINT = 0x090U, 129 | PARAM_CURRENT_CONTROLLER_V_C_SETPOINT = 0x094U, 130 | PARAM_CURRENT_CONTROLLER_I_ALPHA_MEASURED = 0x098U, 131 | PARAM_CURRENT_CONTROLLER_I_BETA_MEASURED = 0x09CU, 132 | PARAM_CURRENT_CONTROLLER_V_ALPHA_SETPOINT = 0x0A0U, 133 | PARAM_CURRENT_CONTROLLER_V_BETA_SETPOINT = 0x0A4U, 134 | PARAM_CURRENT_CONTROLLER_V_Q_TARGET = 0x0A8U, 135 | PARAM_CURRENT_CONTROLLER_V_D_TARGET = 0x0ACU, 136 | PARAM_CURRENT_CONTROLLER_V_Q_SETPOINT = 0x0B0U, 137 | PARAM_CURRENT_CONTROLLER_V_D_SETPOINT = 0x0B4U, 138 | PARAM_CURRENT_CONTROLLER_I_Q_TARGET = 0x0B8U, 139 | PARAM_CURRENT_CONTROLLER_I_D_TARGET = 0x0BCU, 140 | PARAM_CURRENT_CONTROLLER_I_Q_MEASURED = 0x0C0U, 141 | PARAM_CURRENT_CONTROLLER_I_D_MEASURED = 0x0C4U, 142 | PARAM_CURRENT_CONTROLLER_I_Q_SETPOINT = 0x0C8U, 143 | PARAM_CURRENT_CONTROLLER_I_D_SETPOINT = 0x0CCU, 144 | PARAM_CURRENT_CONTROLLER_I_Q_INTEGRATOR = 0x0D0U, 145 | PARAM_CURRENT_CONTROLLER_I_D_INTEGRATOR = 0x0D4U, 146 | PARAM_POWERSTAGE_HTIM = 0x0D8U, 147 | PARAM_POWERSTAGE_HADC1 = 0x0DCU, 148 | PARAM_POWERSTAGE_HADC2 = 0x0E0U, 149 | PARAM_POWERSTAGE_ADC_READING_RAW = 0x0E4U, 150 | PARAM_POWERSTAGE_ADC_READING_OFFSET = 0x0ECU, 151 | PARAM_POWERSTAGE_UNDERVOLTAGE_THRESHOLD = 0x0F4U, 152 | PARAM_POWERSTAGE_OVERVOLTAGE_THRESHOLD = 0x0F8U, 153 | PARAM_POWERSTAGE_BUS_VOLTAGE_FILTER_ALPHA = 0x0FCU, 154 | PARAM_POWERSTAGE_BUS_VOLTAGE_MEASURED = 0x100U, 155 | PARAM_MOTOR_POLE_PAIRS = 0x104U, 156 | PARAM_MOTOR_TORQUE_CONSTANT = 0x108U, 157 | PARAM_MOTOR_PHASE_ORDER = 0x10CU, 158 | PARAM_MOTOR_MAX_CALIBRATION_CURRENT = 0x110U, 159 | PARAM_ENCODER_HI2C = 0x114U, 160 | PARAM_ENCODER_I2C_BUFFER = 0x118U, 161 | PARAM_ENCODER_I2C_UPDATE_COUNTER = 0x11CU, 162 | PARAM_ENCODER_CPR = 0x120U, 163 | PARAM_ENCODER_POSITION_OFFSET = 0x124U, 164 | PARAM_ENCODER_VELOCITY_FILTER_ALPHA = 0x128U, 165 | PARAM_ENCODER_POSITION_RAW = 0x12CU, 166 | PARAM_ENCODER_N_ROTATIONS = 0x130U, 167 | PARAM_ENCODER_POSITION = 0x134U, 168 | PARAM_ENCODER_VELOCITY = 0x138U, 169 | PARAM_ENCODER_FLUX_OFFSET = 0x13CU, 170 | PARAM_ENCODER_FLUX_OFFSET_TABLE = 0x140U, 171 | } Parameter; 172 | -------------------------------------------------------------------------------- /csrc/motor_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include "motor_controller.h" 4 | #include "motor_controller_conf.h" 5 | 6 | 7 | MotorController::MotorController(SocketCan *bus, size_t device_id) : bus(bus), device_id(device_id) { 8 | } 9 | 10 | 11 | void MotorController::ping() { 12 | printf("Pinging motor controller %d\n", (int)device_id); 13 | 14 | // send ping command 15 | can_frame tx_frame; 16 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_PDO_1); 17 | tx_frame.len = 0; 18 | 19 | bus->write(&tx_frame); 20 | 21 | // wait for response 22 | can_frame rx_frame = bus->read(); 23 | 24 | // if (get_device_id(rx_frame.can_id) == device_id && get_func_id(rx_frame.can_id) == FUNC_PING) { 25 | // printf("Received ping response from motor joint %d\n", (int)joint->device_id); 26 | // break; 27 | // } 28 | } 29 | 30 | void MotorController::feed() { 31 | can_frame tx_frame; 32 | tx_frame.can_id = make_can_id(device_id, FUNC_HEARTBEAT); 33 | tx_frame.len = 0; 34 | bus->write(&tx_frame); 35 | } 36 | 37 | void MotorController::set_mode(uint8_t mode) { 38 | can_frame tx_frame; 39 | tx_frame.can_id = make_can_id(device_id, FUNC_NMT); 40 | tx_frame.len = 2; 41 | *((uint8_t *)(tx_frame.data)) = mode; 42 | *((uint8_t *)(tx_frame.data + 1)) = device_id; 43 | bus->write(&tx_frame); 44 | } 45 | 46 | void MotorController::load_settings_from_flash() { 47 | can_frame tx_frame; 48 | tx_frame.can_id = make_can_id(device_id, FUNC_FLASH); 49 | tx_frame.len = 1; 50 | *((uint8_t *)(tx_frame.data)) = 2; 51 | bus->write(&tx_frame); 52 | } 53 | 54 | void MotorController::store_settings_to_flash() { 55 | can_frame tx_frame; 56 | tx_frame.can_id = make_can_id(device_id, FUNC_FLASH); 57 | tx_frame.len = 1; 58 | *((uint8_t *)(tx_frame.data)) = 1; 59 | bus->write(&tx_frame); 60 | } 61 | 62 | float MotorController::read_parameter_f32(Parameter param_id) { 63 | can_frame tx_frame; 64 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_SDO); 65 | tx_frame.len = 3; 66 | *((uint8_t *)(tx_frame.data)) = 0x02 << 5; 67 | *((uint16_t *)(tx_frame.data + 1)) = param_id; 68 | bus->write(&tx_frame); 69 | 70 | can_frame rx_frame = bus->read(); 71 | if (get_device_id(rx_frame.can_id) == device_id) { 72 | return *((float *)(rx_frame.data)); 73 | } 74 | return 0; 75 | } 76 | 77 | int32_t MotorController::read_parameter_i32(Parameter param_id) { 78 | can_frame tx_frame; 79 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_SDO); 80 | tx_frame.len = 3; 81 | *((uint8_t *)(tx_frame.data)) = 0x02 << 5; 82 | *((uint16_t *)(tx_frame.data + 1)) = param_id; 83 | bus->write(&tx_frame); 84 | 85 | can_frame rx_frame = bus->read(); 86 | if (get_device_id(rx_frame.can_id) == device_id) { 87 | return *((int32_t *)(rx_frame.data)); 88 | } 89 | return 0; 90 | } 91 | 92 | uint32_t MotorController::read_parameter_u32(Parameter param_id) { 93 | can_frame tx_frame; 94 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_SDO); 95 | tx_frame.len = 3; 96 | *((uint8_t *)(tx_frame.data)) = 0x02 << 5; 97 | *((uint16_t *)(tx_frame.data + 1)) = param_id; 98 | bus->write(&tx_frame); 99 | 100 | can_frame rx_frame = bus->read(); 101 | if (get_device_id(rx_frame.can_id) == device_id) { 102 | return *((uint32_t *)(rx_frame.data)); 103 | } 104 | return 0; 105 | } 106 | 107 | void MotorController::write_parameter_f32(Parameter param_id, float value) { 108 | can_frame tx_frame; 109 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_SDO); 110 | tx_frame.len = 8; 111 | *((uint8_t *)(tx_frame.data)) = 0x01 << 5; 112 | *((uint16_t *)(tx_frame.data + 1)) = param_id; 113 | *((uint8_t *)(tx_frame.data + 3)) = 0; 114 | *((float *)(tx_frame.data + 4)) = value; 115 | bus->write(&tx_frame); 116 | } 117 | 118 | void MotorController::write_parameter_i32(Parameter param_id, int32_t value) { 119 | can_frame tx_frame; 120 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_SDO); 121 | tx_frame.len = 7; 122 | *((uint8_t *)(tx_frame.data)) = 0x02 << 5; 123 | *((uint16_t *)(tx_frame.data + 1)) = param_id; 124 | *((uint8_t *)(tx_frame.data + 3)) = 0; 125 | *((int32_t *)(tx_frame.data + 4)) = value; 126 | bus->write(&tx_frame); 127 | } 128 | 129 | void MotorController::write_parameter_u32(Parameter param_id, uint32_t value) { 130 | can_frame tx_frame; 131 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_SDO); 132 | tx_frame.len = 7; 133 | *((uint8_t *)(tx_frame.data)) = 0x02 << 5; 134 | *((uint16_t *)(tx_frame.data + 1)) = param_id; 135 | *((uint8_t *)(tx_frame.data + 3)) = 0; 136 | *((uint32_t *)(tx_frame.data + 4)) = value; 137 | bus->write(&tx_frame); 138 | } 139 | 140 | uint32_t MotorController::read_fast_frame_frequency() { 141 | return read_parameter_f32(PARAM_FAST_FRAME_FREQUENCY); 142 | } 143 | 144 | void MotorController::write_fast_frame_frequency(uint32_t frequency) { 145 | write_parameter_f32(PARAM_FAST_FRAME_FREQUENCY, frequency); 146 | } 147 | 148 | float MotorController::read_gear_ratio() { 149 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_GEAR_RATIO); 150 | } 151 | 152 | void MotorController::write_gear_ratio(float ratio) { 153 | write_parameter_f32(PARAM_POSITION_CONTROLLER_GEAR_RATIO, ratio); 154 | } 155 | 156 | float MotorController::read_position_kp() { 157 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_KP); 158 | } 159 | 160 | void MotorController::write_position_kp(float kp) { 161 | write_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_KP, kp); 162 | } 163 | 164 | float MotorController::read_position_kd() { 165 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_KP); 166 | } 167 | 168 | void MotorController::write_position_kd(float kd) { 169 | write_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_KP, kd); 170 | } 171 | 172 | float MotorController::read_position_ki() { 173 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_KI); 174 | } 175 | 176 | void MotorController::write_position_ki(float ki) { 177 | write_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_KI, ki); 178 | } 179 | 180 | float MotorController::read_velocity_kp() { 181 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_KP); 182 | } 183 | 184 | void MotorController::write_velocity_kp(float kp) { 185 | write_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_KP, kp); 186 | } 187 | 188 | float MotorController::read_velocity_ki() { 189 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_KI); 190 | } 191 | 192 | void MotorController::write_velocity_ki(float ki) { 193 | write_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_KI, ki); 194 | } 195 | 196 | float MotorController::read_torque_limit() { 197 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_TORQUE_LIMIT); 198 | } 199 | 200 | void MotorController::write_torque_limit(float torque) { 201 | write_parameter_f32(PARAM_POSITION_CONTROLLER_TORQUE_LIMIT, torque); 202 | } 203 | 204 | float MotorController::read_velocity_limit() { 205 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_LIMIT); 206 | } 207 | 208 | void MotorController::write_velocity_limit(float limit) { 209 | write_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_LIMIT, limit); 210 | } 211 | 212 | float MotorController::read_position_limit_lower() { 213 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_LIMIT_LOWER); 214 | } 215 | 216 | void MotorController::write_position_limit_lower(float limit) { 217 | write_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_LIMIT_LOWER, limit); 218 | } 219 | 220 | float MotorController::read_position_limit_upper() { 221 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_LIMIT_UPPER); 222 | } 223 | 224 | void MotorController::write_position_limit_upper(float limit) { 225 | write_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_LIMIT_UPPER, limit); 226 | } 227 | 228 | float MotorController::read_position_offset() { 229 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_OFFSET); 230 | } 231 | 232 | void MotorController::write_position_offset(float offset) { 233 | write_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_OFFSET, offset); 234 | } 235 | 236 | float MotorController::read_torque_target() { 237 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_TORQUE_TARGET); 238 | } 239 | 240 | void MotorController::write_torque_target(float torque) { 241 | write_parameter_f32(PARAM_POSITION_CONTROLLER_TORQUE_TARGET, torque); 242 | } 243 | 244 | float MotorController::read_torque_measured() { 245 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_TORQUE_MEASURED); 246 | } 247 | 248 | float MotorController::read_velocity_measured() { 249 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_VELOCITY_MEASURED); 250 | } 251 | 252 | float MotorController::read_position_target() { 253 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_TARGET); 254 | } 255 | 256 | void MotorController::write_position_target(float position) { 257 | write_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_TARGET, position); 258 | } 259 | 260 | float MotorController::read_position_measured() { 261 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_POSITION_MEASURED); 262 | } 263 | 264 | float MotorController::read_torque_filter_alpha() { 265 | return read_parameter_f32(PARAM_POSITION_CONTROLLER_TORQUE_FILTER_ALPHA); 266 | } 267 | 268 | void MotorController::write_torque_filter_alpha(float alpha) { 269 | write_parameter_f32(PARAM_POSITION_CONTROLLER_TORQUE_FILTER_ALPHA, alpha); 270 | } 271 | 272 | float MotorController::read_current_limit() { 273 | return read_parameter_f32(PARAM_CURRENT_CONTROLLER_I_LIMIT); 274 | } 275 | 276 | void MotorController::write_current_limit(float current) { 277 | write_parameter_f32(PARAM_CURRENT_CONTROLLER_I_LIMIT, current); 278 | } 279 | 280 | float MotorController::read_current_kp() { 281 | return read_parameter_f32(PARAM_CURRENT_CONTROLLER_I_KP); 282 | } 283 | 284 | void MotorController::write_current_kp(float kp) { 285 | write_parameter_f32(PARAM_CURRENT_CONTROLLER_I_KP, kp); 286 | } 287 | 288 | float MotorController::read_current_ki() { 289 | return read_parameter_f32(PARAM_CURRENT_CONTROLLER_I_KI); 290 | } 291 | 292 | void MotorController::write_current_ki(float ki) { 293 | write_parameter_f32(PARAM_CURRENT_CONTROLLER_I_KI, ki); 294 | } 295 | 296 | float MotorController::read_bus_voltage_filter_alpha() { 297 | return read_parameter_f32(PARAM_POWERSTAGE_BUS_VOLTAGE_FILTER_ALPHA); 298 | } 299 | 300 | void MotorController::write_bus_voltage_filter_alpha(float alpha) { 301 | write_parameter_f32(PARAM_POWERSTAGE_BUS_VOLTAGE_FILTER_ALPHA, alpha); 302 | } 303 | 304 | float MotorController::read_motor_torque_constant() { 305 | return read_parameter_f32(PARAM_MOTOR_TORQUE_CONSTANT); 306 | } 307 | 308 | void MotorController::write_motor_torque_constant(float torque_constant) { 309 | write_parameter_f32(PARAM_MOTOR_TORQUE_CONSTANT, torque_constant); 310 | } 311 | 312 | int MotorController::read_motor_phase_order() { 313 | return read_parameter_i32(PARAM_MOTOR_PHASE_ORDER); 314 | } 315 | 316 | float MotorController::read_encoder_velocity_filter_alpha() { 317 | return read_parameter_f32(PARAM_ENCODER_VELOCITY_FILTER_ALPHA); 318 | } 319 | 320 | void MotorController::write_encoder_velocity_filter_alpha(float alpha) { 321 | write_parameter_f32(PARAM_ENCODER_VELOCITY_FILTER_ALPHA, alpha); 322 | } 323 | 324 | void MotorController::set_current_bandwidth(float bandwidth_hz, float phase_resistance, float phase_inductance) { 325 | float kp = bandwidth_hz * 2.0 * M_PI * phase_inductance; 326 | float ki = phase_resistance / phase_inductance; 327 | write_current_kp(kp); 328 | write_current_ki(ki); 329 | } 330 | 331 | void MotorController::set_torque_bandwidth(float bandwidth_hz, float position_loop_rate) { 332 | float alpha = fmin(fmax(1. - exp(-2. * M_PI * (bandwidth_hz / position_loop_rate)), 0.), 1.); 333 | write_torque_filter_alpha(alpha); 334 | } 335 | 336 | void MotorController::set_bus_voltage_bandwidth(float bandwidth_hz, float bus_voltage_update_rate) { 337 | float alpha = fmin(fmax(1. - exp(-2. * M_PI * (bandwidth_hz / bus_voltage_update_rate)), 0.), 1.); 338 | write_bus_voltage_filter_alpha(alpha); 339 | } 340 | 341 | void MotorController::set_encoder_velocity_bandwidth(float bandwidth_hz, float encoder_update_rate) { 342 | float alpha = fmin(fmax(1. - exp(-2. * M_PI * (bandwidth_hz / encoder_update_rate)), 0.), 1.); 343 | write_encoder_velocity_filter_alpha(alpha); 344 | } 345 | 346 | 347 | void MotorController::read_pdo_2() { 348 | can_frame rx_frame = bus->read(); 349 | if (get_device_id(rx_frame.can_id) == device_id) { 350 | this->position_measured = *((float *)rx_frame.data + 0); 351 | this->velocity_measured = *((float *)rx_frame.data + 1); 352 | } 353 | } 354 | 355 | void MotorController::write_pdo_2() { 356 | can_frame tx_frame; 357 | tx_frame.can_id = make_can_id(device_id, FUNC_RECEIVE_PDO_2); 358 | tx_frame.len = 8; 359 | *((float *)tx_frame.data + 0) = this->position_target; 360 | *((float *)tx_frame.data + 1) = this->velocity_target; 361 | 362 | bus->write(&tx_frame); 363 | } 364 | 365 | float MotorController::get_measured_position() { 366 | return this->position_measured; 367 | } 368 | 369 | float MotorController::get_measured_velocity() { 370 | return this->velocity_measured; 371 | } 372 | 373 | void MotorController::set_target_position(float position) { 374 | this->position_target = position; 375 | } 376 | 377 | void MotorController::set_target_velocity(float velocity) { 378 | this->velocity_target = velocity; 379 | } 380 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/robot/humanoid.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import time 4 | 5 | from omegaconf import OmegaConf 6 | import numpy as np 7 | 8 | import berkeley_humanoid_lite_lowlevel.recoil as recoil 9 | from berkeley_humanoid_lite_lowlevel.robot.imu import SerialImu, Baudrate 10 | from berkeley_humanoid_lite_lowlevel.policy.gamepad import Se2Gamepad 11 | 12 | 13 | class State: 14 | INVALID = 0 15 | IDLE = 1 16 | RL_INIT = 2 17 | RL_RUNNING = 3 18 | 19 | 20 | def linear_interpolate(start: np.ndarray, end: np.ndarray, percentage: float) -> np.ndarray: 21 | percentage = min(max(percentage, 0.0), 1.0) 22 | target = start * (1. - percentage) + end * percentage 23 | return target 24 | 25 | 26 | class Humanoid: 27 | def __init__(self): 28 | 29 | # self.left_arm_transport = recoil.Bus("can0") 30 | # self.right_arm_transport = recoil.Bus("can1") 31 | # self.left_leg_transport = recoil.Bus("can2") 32 | # self.right_leg_transport = recoil.Bus("can3") 33 | 34 | self.left_leg_transport = recoil.Bus("can0") 35 | self.right_leg_transport = recoil.Bus("can1") 36 | 37 | self.joints = [ 38 | # (self.left_arm_transport, 1, "left_shoulder_pitch_joint" ), # noqa: E241 39 | # (self.left_arm_transport, 3, "left_shoulder_roll_joint" ), # noqa: E241 40 | # (self.left_arm_transport, 5, "left_shoulder_yaw_joint" ), # noqa: E241 41 | # (self.left_arm_transport, 7, "left_elbow_pitch_joint" ), # noqa: E241 42 | # (self.left_arm_transport, 9, "left_wrist_yaw_joint" ), # noqa: E241 43 | 44 | # (self.right_arm_transport, 2, "right_shoulder_pitch_joint" ), # noqa: E241 45 | # (self.right_arm_transport, 4, "right_shoulder_roll_joint" ), # noqa: E241 46 | # (self.right_arm_transport, 6, "right_shoulder_yaw_joint" ), # noqa: E241 47 | # (self.right_arm_transport, 8, "right_elbow_pitch_joint" ), # noqa: E241 48 | # (self.right_arm_transport, 10, "right_wrist_yaw_joint" ), # noqa: E241 49 | 50 | (self.left_leg_transport, 1, "left_hip_roll_joint" ), # noqa: E241 51 | (self.left_leg_transport, 3, "left_hip_yaw_joint" ), # noqa: E241 52 | (self.left_leg_transport, 5, "left_hip_pitch_joint" ), # noqa: E241 53 | (self.left_leg_transport, 7, "left_knee_pitch_joint" ), # noqa: E241 54 | (self.left_leg_transport, 11, "left_ankle_pitch_joint" ), # noqa: E241 55 | (self.left_leg_transport, 13, "left_ankle_roll_joint" ), # noqa: E241 56 | 57 | (self.right_leg_transport, 2, "right_hip_roll_joint" ), # noqa: E241 58 | (self.right_leg_transport, 4, "right_hip_yaw_joint" ), # noqa: E241 59 | (self.right_leg_transport, 6, "right_hip_pitch_joint" ), # noqa: E241 60 | (self.right_leg_transport, 8, "right_knee_pitch_joint" ), # noqa: E241 61 | (self.right_leg_transport, 12, "right_ankle_pitch_joint" ), # noqa: E241 62 | (self.right_leg_transport, 14, "right_ankle_roll_joint" ), # noqa: E241 63 | ] 64 | 65 | self.imu = SerialImu(baudrate=Baudrate.BAUD_460800) 66 | self.imu.run_forever() 67 | 68 | # Start joystick thread 69 | self.command_controller = Se2Gamepad() 70 | self.command_controller.run() 71 | 72 | self.state = State.IDLE 73 | self.next_state = State.IDLE 74 | 75 | self.rl_init_positions = np.array([ 76 | 0.0, 0.0, -0.2, 77 | 0.4, 78 | -0.3, 0.0, 79 | 0.0, 0.0, -0.2, 80 | 0.4, 81 | -0.3, 0.0 82 | ], dtype=np.float32) 83 | 84 | self.joint_axis_directions = np.array([ 85 | -1, 1, -1, 86 | -1, 87 | -1, 1, 88 | -1, 1, 1, 89 | 1, 90 | 1, 1 91 | ], dtype=np.float32) 92 | 93 | self.position_offsets = np.array([ 94 | 0.0, 0.0, 0.0, 95 | 0.0, 96 | 0.0, 0.0, 97 | 0.0, 0.0, 0.0, 98 | 0.0, 99 | 0.0, 0.0 100 | ], dtype=np.float32) 101 | 102 | self.n_lowlevel_states = 4 + 3 + 12 + 12 + 1 + 3 103 | self.lowlevel_states = np.zeros(self.n_lowlevel_states, dtype=np.float32) 104 | 105 | self.joint_velocity_target = np.zeros(len(self.joints), dtype=np.float32) 106 | self.joint_position_target = np.zeros(len(self.joints), dtype=np.float32) 107 | self.joint_position_measured = np.zeros(len(self.joints), dtype=np.float32) 108 | self.joint_velocity_measured = np.zeros(len(self.joints), dtype=np.float32) 109 | 110 | # used for RL initialization controller 111 | self.init_percentage = 0.0 112 | self.starting_positions = np.zeros_like(self.joint_position_target, dtype=np.float32) 113 | 114 | config_path = "calibration.yaml" 115 | with open(config_path, "r") as f: 116 | config = OmegaConf.load(f) 117 | position_offsets = np.array(config.get("position_offsets", None)) 118 | assert position_offsets.shape[0] == len(self.joints) 119 | self.position_offsets[:] = position_offsets 120 | 121 | def enter_damping(self): 122 | self.joint_kp = np.zeros((len(self.joints),), dtype=np.float32) 123 | self.joint_kd = np.zeros((len(self.joints),), dtype=np.float32) 124 | self.torque_limit = np.zeros((len(self.joints),), dtype=np.float32) 125 | 126 | self.joint_kp[:] = 20 127 | self.joint_kd[:] = 2 128 | self.torque_limit[:] = 4 129 | 130 | for i, entry in enumerate(self.joints): 131 | bus, device_id, joint_name = entry 132 | 133 | print(f"Initializing joint {joint_name}:") 134 | print(f" kp: {self.joint_kp[i]}, kd: {self.joint_kd[i]}, torque limit: {self.torque_limit[i]}") 135 | 136 | # Set the joint mode to idle 137 | bus.set_mode(device_id, recoil.Mode.IDLE) 138 | time.sleep(0.001) 139 | bus.write_position_kp(device_id, self.joint_kp[i]) 140 | time.sleep(0.001) 141 | bus.write_position_kd(device_id, self.joint_kd[i]) 142 | time.sleep(0.001) 143 | bus.write_torque_limit(device_id, self.torque_limit[i]) 144 | time.sleep(0.001) 145 | bus.feed(device_id) 146 | bus.set_mode(device_id, recoil.Mode.DAMPING) 147 | 148 | print("Motors enabled") 149 | 150 | def stop(self): 151 | self.imu.stop() 152 | self.command_controller.stop() 153 | 154 | for entry in self.joints: 155 | bus, device_id, _ = entry 156 | bus.set_mode(device_id, recoil.Mode.DAMPING) 157 | 158 | print("Entered damping mode. Press Ctrl+C again to exit.\n") 159 | 160 | try: 161 | while True: 162 | pass 163 | except KeyboardInterrupt: 164 | print("Exiting damping mode.") 165 | 166 | for entry in self.joints: 167 | bus, device_id, _ = entry 168 | bus.set_mode(device_id, recoil.Mode.IDLE) 169 | 170 | # self.left_arm_transport.stop() 171 | # self.right_arm_transport.stop() 172 | self.left_leg_transport.stop() 173 | self.right_leg_transport.stop() 174 | 175 | def get_observations(self) -> np.ndarray: 176 | imu_quaternion = self.lowlevel_states[0:4] 177 | imu_angular_velocity = self.lowlevel_states[4:7] 178 | joint_positions = self.lowlevel_states[7:19] 179 | joint_velocities = self.lowlevel_states[19:31] 180 | mode = self.lowlevel_states[31:32] 181 | velocity_commands = self.lowlevel_states[32:35] 182 | 183 | imu_quaternion[:] = self.imu.quaternion[:] 184 | 185 | # IMU returns angular velocity in deg/s, we need rad/s 186 | imu_angular_velocity[:] = np.deg2rad(self.imu.angular_velocity[:]) 187 | 188 | joint_positions[:] = self.joint_position_measured[:] 189 | joint_velocities[:] = self.joint_velocity_measured[:] 190 | 191 | mode[0] = self.command_controller.commands["mode_switch"] 192 | velocity_commands[0] = self.command_controller.commands["velocity_x"] 193 | velocity_commands[1] = self.command_controller.commands["velocity_y"] 194 | velocity_commands[2] = self.command_controller.commands["velocity_yaw"] 195 | 196 | self.next_state = self.command_controller.commands["mode_switch"] 197 | 198 | return self.lowlevel_states 199 | 200 | def update_joint_group(self, joint_id_l, joint_id_r): 201 | # adjust direction and offset of target values 202 | position_target_l = (self.joint_position_target[joint_id_l] + self.position_offsets[joint_id_l]) * self.joint_axis_directions[joint_id_l] 203 | position_target_r = (self.joint_position_target[joint_id_r] + self.position_offsets[joint_id_r]) * self.joint_axis_directions[joint_id_r] 204 | 205 | self.joints[joint_id_l][0].transmit_pdo_2(self.joints[joint_id_l][1], position_target=position_target_l, velocity_target=0.0) 206 | self.joints[joint_id_r][0].transmit_pdo_2(self.joints[joint_id_r][1], position_target=position_target_r, velocity_target=0.0) 207 | 208 | position_measured_l, velocity_measured_l = self.joints[joint_id_l][0].receive_pdo_2(self.joints[joint_id_l][1]) 209 | position_measured_r, velocity_measured_r = self.joints[joint_id_r][0].receive_pdo_2(self.joints[joint_id_r][1]) 210 | 211 | # adjust direction and offset of target values 212 | if position_measured_l is not None: 213 | self.joint_position_measured[joint_id_l] = (position_measured_l * self.joint_axis_directions[joint_id_l]) - self.position_offsets[joint_id_l] 214 | if velocity_measured_l is not None: 215 | self.joint_velocity_measured[joint_id_l] = velocity_measured_l * self.joint_axis_directions[joint_id_l] 216 | if position_measured_r is not None: 217 | self.joint_position_measured[joint_id_r] = (position_measured_r * self.joint_axis_directions[joint_id_r]) - self.position_offsets[joint_id_r] 218 | if velocity_measured_r is not None: 219 | self.joint_velocity_measured[joint_id_r] = velocity_measured_r * self.joint_axis_directions[joint_id_r] 220 | 221 | def update_joints(self): 222 | 223 | # communicate with actuators 224 | self.update_joint_group(0, 6) 225 | self.update_joint_group(1, 7) 226 | self.update_joint_group(2, 8) 227 | self.update_joint_group(3, 9) 228 | self.update_joint_group(4, 10) 229 | self.update_joint_group(5, 11) 230 | 231 | def reset(self): 232 | obs = self.get_observations() 233 | return obs 234 | 235 | def step(self, actions: np.ndarray): 236 | """ 237 | actions: np.ndarray of shape (n_joints, ) 238 | """ 239 | match (self.state): 240 | case State.IDLE: 241 | self.joint_position_target[:] = self.joint_position_measured[:] 242 | 243 | if self.next_state == State.RL_INIT: 244 | print("Switching to RL initialization mode") 245 | self.state = self.next_state 246 | 247 | for entry in self.joints: 248 | bus, device_id, _ = entry 249 | bus.feed(device_id) 250 | bus.set_mode(device_id, recoil.Mode.POSITION) 251 | 252 | self.starting_positions = self.joint_position_target[:] 253 | self.init_percentage = 0.0 254 | 255 | case State.RL_INIT: 256 | print(f"init: {self.init_percentage:.2f}") 257 | if self.init_percentage < 1.0: 258 | self.init_percentage += 1 / 100.0 259 | self.init_percentage = min(self.init_percentage, 1.0) 260 | 261 | self.joint_position_target = linear_interpolate(self.starting_positions, self.rl_init_positions, self.init_percentage) 262 | else: 263 | if self.next_state == State.RL_RUNNING: 264 | print("Switching to RL running mode") 265 | self.state = self.next_state 266 | 267 | if self.next_state == State.IDLE: 268 | print("Switching to idle mode") 269 | self.state = self.next_state 270 | 271 | for entry in self.joints: 272 | bus, device_id, _ = entry 273 | bus.set_mode(device_id, recoil.Mode.DAMPING) 274 | 275 | case State.RL_RUNNING: 276 | for i in range(len(self.joints)): 277 | self.joint_position_target[i] = actions[i] 278 | 279 | if self.next_state == State.IDLE: 280 | print("Switching to idle mode") 281 | self.state = self.next_state 282 | 283 | for entry in self.joints: 284 | bus, device_id, _ = entry 285 | bus.set_mode(device_id, recoil.Mode.DAMPING) 286 | 287 | self.update_joints() 288 | 289 | obs = self.get_observations() 290 | 291 | return obs 292 | 293 | def check_connection(self): 294 | for entry in self.joints: 295 | bus, device_id, joint_name = entry 296 | print(f"Pinging {joint_name} ... ", end="\t") 297 | result = bus.ping(device_id) 298 | if result: 299 | print("OK") 300 | else: 301 | print("ERROR") 302 | time.sleep(0.1) 303 | -------------------------------------------------------------------------------- /csrc/real_humanoid.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | #include 4 | 5 | #include "real_humanoid.h" 6 | #include "motor_controller_conf.h" 7 | 8 | 9 | static float linear_interpolate(float start, float end, float percentage) { 10 | float target; 11 | percentage = std::fmin(std::fmax(percentage, 0.0f), 1.0f); 12 | target = start * (1.f - percentage) + end * percentage; 13 | return target; 14 | } 15 | 16 | 17 | RealHumanoid::RealHumanoid() { 18 | imu = nullptr; 19 | state = STATE_IDLE; 20 | next_state = STATE_IDLE; 21 | 22 | for (size_t i=0; i(); 40 | } 41 | 42 | printf("loaded joint offsets: "); 43 | for (size_t i=0; i(); 52 | joint_kd[i] = policy_config["joint_kd"][i].as(); 53 | torque_limit[i] = policy_config["effort_limits"][i].as(); 54 | } 55 | config_control_dt_ = policy_config["control_dt"].as(); 56 | config_policy_dt_ = policy_config["policy_dt"].as(); 57 | } 58 | 59 | RealHumanoid::~RealHumanoid() { 60 | delete imu; 61 | } 62 | 63 | 64 | void RealHumanoid::control_loop() { 65 | switch (state) { 66 | case STATE_IDLE: 67 | /* In this state, the motor positions are held at the current measured positions */ 68 | /* When Y is pressed, the robot will switch to RL initialization mode */ 69 | 70 | for (int i = 0; i < N_JOINTS; i += 1) { 71 | position_target[i] = position_measured[i]; 72 | } 73 | if (next_state == STATE_RL_INIT) { 74 | printf("Switching to RL initialization mode\n"); 75 | state = next_state; 76 | 77 | for (int i = 0; i < N_JOINTS; i += 1) { 78 | usleep(5); 79 | joint_ptrs[i]->feed(); 80 | joint_ptrs[i]->set_mode(MODE_POSITION); 81 | starting_positions[i] = position_target[i]; 82 | } 83 | init_percentage = 0.0; 84 | } 85 | break; 86 | 87 | case STATE_RL_INIT: 88 | /* In this state, the robot will hold the getup position */ 89 | // printf("init: %.3f\n", init_percentage); 90 | 91 | if (init_percentage < 1.0) { 92 | init_percentage += 1 / 200.0; 93 | init_percentage = init_percentage < 1.0 ? init_percentage : 1.0; 94 | 95 | for (size_t i=0; i<12; i+=1) { 96 | position_target[i] = linear_interpolate(starting_positions[i], rl_init_positions[i], init_percentage); 97 | } 98 | } 99 | else { 100 | if (next_state == STATE_RL_RUNNING) { 101 | printf("Switching to RL running mode\n"); 102 | state = next_state; 103 | } 104 | if (next_state == STATE_IDLE) { 105 | printf("Switching to idle mode\n"); 106 | state = next_state; 107 | 108 | for (int i = 0; i < N_JOINTS; i += 1) { 109 | usleep(5); 110 | joint_ptrs[i]->set_mode(MODE_DAMPING); 111 | } 112 | } 113 | } 114 | break; 115 | 116 | case STATE_RL_RUNNING: 117 | /* In this state, the robot will follow the policy */ 118 | for (int i = 0; i < N_LOWLEVEL_COMMANDS; i += 1) { 119 | position_target[i] = lowlevel_commands[i]; 120 | } 121 | 122 | if (next_state == STATE_IDLE) { 123 | printf("Switching to idle mode\n"); 124 | state = next_state; 125 | 126 | for (int i = 0; i < N_JOINTS; i += 1) { 127 | usleep(5); 128 | joint_ptrs[i]->set_mode(MODE_DAMPING); 129 | } 130 | } 131 | 132 | break; 133 | } 134 | 135 | #if DEBUG_DISABLE_TRANSPORTS == 0 136 | update_joints(); 137 | #endif 138 | 139 | #if DEBUG_JOINT_DATA_LOGGING == 1 140 | printf("%d %.2f \t%.2f \t%.2f \t- %.2f \t- %.2f \t%.2f \t", 141 | control_loop_count, 142 | position_measured[0], position_measured[1], position_measured[2], 143 | position_measured[3], 144 | position_measured[4], position_measured[5]); 145 | // printf(" %.2f \t%.2f \t%.2f \t- %.2f \t- %.2f \t%.2f", 146 | // position_target[0], position_target[1], position_target[2], 147 | // position_target[3], 148 | // position_target[4], position_target[5]); 149 | printf("\n"); 150 | #endif 151 | 152 | /* base_quat */ 153 | lowlevel_states[0] = imu->quaternion[0]; 154 | lowlevel_states[1] = imu->quaternion[1]; 155 | lowlevel_states[2] = imu->quaternion[2]; 156 | lowlevel_states[3] = imu->quaternion[3]; 157 | 158 | /* base_ang_vel */ 159 | lowlevel_states[4] = imu->angular_velocity[0]; 160 | lowlevel_states[5] = imu->angular_velocity[1]; 161 | lowlevel_states[6] = imu->angular_velocity[2]; 162 | 163 | /* joint_positions */ 164 | for (int i = 0; i < N_JOINTS; i += 1) { 165 | lowlevel_states[7 + i] = position_measured[i]; 166 | } 167 | 168 | /* joint_velocities */ 169 | for (int i = 0; i < N_JOINTS; i += 1) { 170 | lowlevel_states[19 + i] = velocity_measured[i]; 171 | } 172 | 173 | /* mode */ 174 | lowlevel_states[31] = state; 175 | 176 | /* command velocity */ 177 | lowlevel_states[32] = stick_command_velocity_x_; 178 | lowlevel_states[33] = stick_command_velocity_y_; 179 | lowlevel_states[34] = stick_command_velocity_yaw_; 180 | 181 | // execute every 4 control loops 182 | if (control_loop_count >= (int)std::round(config_policy_dt_/ config_control_dt_)) { 183 | if (state == STATE_IDLE || state == STATE_RL_RUNNING) { 184 | size_t expected_bytes = sizeof(float) * N_LOWLEVEL_STATES; 185 | ssize_t actual_bytes = sendto(udp.sockfd, lowlevel_states, expected_bytes, 0, (const struct sockaddr *)&udp.send_addr, sizeof(udp.send_addr)); 186 | 187 | if (actual_bytes < 0 || actual_bytes != expected_bytes) { 188 | printf("[Error] Error sending: %s\n", strerror(errno)); 189 | } 190 | } 191 | size_t expected_bytes = sizeof(float) * N_LOWLEVEL_STATES; 192 | struct sockaddr_in visualize_addr = {0}; 193 | visualize_addr.sin_family = AF_INET; 194 | visualize_addr.sin_port = htons(VISUALIZE_PORT); 195 | visualize_addr.sin_addr.s_addr = inet_addr(HOST_IP_ADDR); 196 | ssize_t actual_bytes = sendto(udp.sockfd, lowlevel_states, expected_bytes, 0, (const struct sockaddr *)&visualize_addr, sizeof(visualize_addr)); 197 | control_loop_count = 0; 198 | } 199 | 200 | control_loop_count += 1; 201 | } 202 | 203 | void RealHumanoid::imu_loop() { 204 | imu->update_reading(); 205 | } 206 | 207 | void RealHumanoid::keyboard_loop() { 208 | termios term; 209 | tcgetattr(0, &term); 210 | 211 | termios term2 = term; 212 | 213 | term2.c_lflag &= ~ICANON; 214 | tcsetattr(0, TCSANOW, &term2); 215 | 216 | int byteswaiting; 217 | ioctl(0, FIONREAD, &byteswaiting); 218 | 219 | tcsetattr(0, TCSANOW, &term); 220 | 221 | if (byteswaiting > 0) { 222 | char c = fgetc(stdin); 223 | printf("key pressed: %c\n", c); 224 | 225 | switch (c) { 226 | case 'r': 227 | next_state = STATE_RL_INIT; 228 | break; 229 | case 't': 230 | next_state = STATE_RL_RUNNING; 231 | break; 232 | case 'q': 233 | next_state = STATE_IDLE; 234 | break; 235 | } 236 | 237 | 238 | } 239 | 240 | tcsetattr(0, TCSANOW, &term); 241 | } 242 | 243 | void RealHumanoid::joystick_loop() { 244 | size_t expected_bytes = 13; 245 | uint8_t udp_buffer[13]; 246 | ssize_t actual_bytes = recvfrom(udp_joystick.sockfd, udp_buffer, expected_bytes, MSG_WAITALL, NULL, NULL); 247 | 248 | if (actual_bytes < 0 || actual_bytes != expected_bytes) { 249 | printf("[Error] Error receiving: %s\n", strerror(errno)); 250 | 251 | return; 252 | } 253 | 254 | uint8_t command_mode = udp_buffer[0]; 255 | stick_command_velocity_x_ = *(float *)(udp_buffer + 1); 256 | stick_command_velocity_y_ = *(float *)(udp_buffer + 5); 257 | stick_command_velocity_yaw_ = *(float *)(udp_buffer + 9); 258 | 259 | if (command_mode != 0) { 260 | switch (command_mode) { 261 | case 1: 262 | next_state = STATE_IDLE; 263 | break; 264 | case 2: 265 | next_state = STATE_RL_INIT; 266 | break; 267 | case 3: 268 | next_state = STATE_RL_RUNNING; 269 | break; 270 | default: 271 | next_state = STATE_IDLE; 272 | } 273 | } 274 | } 275 | 276 | void RealHumanoid::process_actions() { 277 | 278 | } 279 | 280 | void RealHumanoid::process_observations() { 281 | 282 | } 283 | 284 | void RealHumanoid::policy_forward() { 285 | 286 | } 287 | 288 | void RealHumanoid::udp_recv() { 289 | size_t expected_bytes = sizeof(float) * N_LOWLEVEL_COMMANDS; 290 | ssize_t actual_bytes = recvfrom(udp.sockfd, lowlevel_commands, expected_bytes, MSG_WAITALL, NULL, NULL); 291 | 292 | if (actual_bytes < 0 || actual_bytes != expected_bytes) { 293 | printf("[Error] Error receiving: %s\n", strerror(errno)); 294 | } 295 | //TODO: detect if the action is delayed 296 | 297 | } 298 | 299 | void RealHumanoid::update_joints() { 300 | /* set target positions to joint controller */ 301 | for (size_t i = 0; i < N_JOINTS; i += 1) { 302 | joint_ptrs[i]->set_target_position((position_target[i] + position_offsets[i]) * joint_axis_directions[i]); 303 | } 304 | 305 | joint_ptrs[LEG_LEFT_HIP_ROLL_JOINT]->write_pdo_2(); 306 | joint_ptrs[LEG_RIGHT_HIP_ROLL_JOINT]->write_pdo_2(); 307 | joint_ptrs[LEG_LEFT_HIP_ROLL_JOINT]->read_pdo_2(); 308 | joint_ptrs[LEG_RIGHT_HIP_ROLL_JOINT]->read_pdo_2(); 309 | 310 | joint_ptrs[LEG_LEFT_HIP_YAW_JOINT]->write_pdo_2(); 311 | joint_ptrs[LEG_RIGHT_HIP_YAW_JOINT]->write_pdo_2(); 312 | joint_ptrs[LEG_LEFT_HIP_YAW_JOINT]->read_pdo_2(); 313 | joint_ptrs[LEG_RIGHT_HIP_YAW_JOINT]->read_pdo_2(); 314 | 315 | joint_ptrs[LEG_LEFT_HIP_PITCH_JOINT]->write_pdo_2(); 316 | joint_ptrs[LEG_RIGHT_HIP_PITCH_JOINT]->write_pdo_2(); 317 | joint_ptrs[LEG_LEFT_HIP_PITCH_JOINT]->read_pdo_2(); 318 | joint_ptrs[LEG_RIGHT_HIP_PITCH_JOINT]->read_pdo_2(); 319 | 320 | joint_ptrs[LEG_LEFT_KNEE_PITCH_JOINT]->write_pdo_2(); 321 | joint_ptrs[LEG_RIGHT_KNEE_PITCH_JOINT]->write_pdo_2(); 322 | joint_ptrs[LEG_LEFT_KNEE_PITCH_JOINT]->read_pdo_2(); 323 | joint_ptrs[LEG_RIGHT_KNEE_PITCH_JOINT]->read_pdo_2(); 324 | 325 | joint_ptrs[LEG_LEFT_ANKLE_PITCH_JOINT]->write_pdo_2(); 326 | joint_ptrs[LEG_RIGHT_ANKLE_PITCH_JOINT]->write_pdo_2(); 327 | joint_ptrs[LEG_LEFT_ANKLE_PITCH_JOINT]->read_pdo_2(); 328 | joint_ptrs[LEG_RIGHT_ANKLE_PITCH_JOINT]->read_pdo_2(); 329 | 330 | joint_ptrs[LEG_LEFT_ANKLE_ROLL_JOINT]->write_pdo_2(); 331 | joint_ptrs[LEG_RIGHT_ANKLE_ROLL_JOINT]->write_pdo_2(); 332 | joint_ptrs[LEG_LEFT_ANKLE_ROLL_JOINT]->read_pdo_2(); 333 | joint_ptrs[LEG_RIGHT_ANKLE_ROLL_JOINT]->read_pdo_2(); 334 | 335 | /* update measured positions from joint controller */ 336 | for (size_t i = 0; i < N_JOINTS; i += 1) { 337 | position_measured[i] = joint_ptrs[i]->get_measured_position() * joint_axis_directions[i] - position_offsets[i]; 338 | velocity_measured[i] = joint_ptrs[i]->get_measured_velocity() * joint_axis_directions[i]; 339 | } 340 | } 341 | 342 | 343 | void RealHumanoid::stop() { 344 | if (stopped == 0) { 345 | stopped = 1; 346 | 347 | loop_udp_recv->shutdown(); 348 | loop_control->shutdown(); 349 | loop_keyboard->shutdown(); 350 | loop_joystick->shutdown(); 351 | 352 | #if DEBUG_DISABLE_TRANSPORTS == 0 353 | for (int i = 0; i < N_JOINTS; i += 1) { 354 | joint_ptrs[i]->set_mode(MODE_DAMPING); 355 | } 356 | #endif 357 | 358 | printf("Entered damping mode. Press Ctrl+C again to exit.\n"); 359 | } 360 | else if (stopped == 1) { 361 | printf("exiting...\n"); 362 | 363 | #if DEBUG_DISABLE_TRANSPORTS == 0 364 | for (int i = 0; i < N_JOINTS; i += 1) { 365 | joint_ptrs[i]->set_mode(MODE_IDLE); 366 | } 367 | #endif 368 | 369 | stopped = 2; 370 | 371 | sleep(1); 372 | } 373 | 374 | 375 | printf("RealHumanoid stopped\n"); 376 | } 377 | 378 | void RealHumanoid::initialize() { 379 | ssize_t status; 380 | 381 | #if DEBUG_DISABLE_TRANSPORTS == 0 382 | // // left arm 383 | // left_arm_bus.open("can0"); 384 | // if (!left_arm_bus.isOpen()) { 385 | // printf("[ERROR]
: Error initializing left arm transport\n"); 386 | // exit(1); 387 | // } 388 | 389 | // // right arm 390 | // right_arm_bus.open("can1"); 391 | // if (!right_arm_bus.isOpen()) { 392 | // printf("[ERROR]
: Error initializing right arm transport\n"); 393 | // exit(1); 394 | // } 395 | 396 | // left leg 397 | left_leg_bus.open("can0"); 398 | if (!left_leg_bus.isOpen()) { 399 | printf("[ERROR]
: Error initializing left leg transport\n"); 400 | exit(1); 401 | } 402 | 403 | // right leg 404 | right_leg_bus.open("can1"); 405 | if (!right_leg_bus.isOpen()) { 406 | printf("[ERROR]
: Error initializing right arm transport\n"); 407 | exit(1); 408 | } 409 | #endif 410 | 411 | // // left arm 412 | // joint_ptrs[ARM_LEFT_SHOULDER_PITCH_JOINT] = std::make_shared(&left_arm_bus, 1); 413 | // joint_ptrs[ARM_LEFT_SHOULDER_ROLL_JOINT] = std::make_shared(&left_arm_bus, 3); 414 | // joint_ptrs[ARM_LEFT_SHOULDER_YAW_JOINT] = std::make_shared(&left_arm_bus, 5); 415 | // joint_ptrs[ARM_LEFT_ELBOW_PITCH_JOINT] = std::make_shared(&left_arm_bus, 7); 416 | // joint_ptrs[ARM_LEFT_ELBOW_ROLL_JOINT] = std::make_shared(&left_arm_bus, 9); 417 | 418 | // // right arm 419 | // joint_ptrs[ARM_RIGHT_SHOULDER_PITCH_JOINT] = std::make_shared(&right_arm_bus, 2); 420 | // joint_ptrs[ARM_RIGHT_SHOULDER_ROLL_JOINT] = std::make_shared(&right_arm_bus, 4); 421 | // joint_ptrs[ARM_RIGHT_SHOULDER_YAW_JOINT] = std::make_shared(&right_arm_bus, 6); 422 | // joint_ptrs[ARM_RIGHT_ELBOW_PITCH_JOINT] = std::make_shared(&right_arm_bus, 8); 423 | // joint_ptrs[ARM_RIGHT_ELBOW_ROLL_JOINT] = std::make_shared(&right_arm_bus, 10); 424 | 425 | // left leg 426 | joint_ptrs[LEG_LEFT_HIP_ROLL_JOINT] = std::make_shared(&left_leg_bus, 1); 427 | joint_ptrs[LEG_LEFT_HIP_YAW_JOINT] = std::make_shared(&left_leg_bus, 3); 428 | joint_ptrs[LEG_LEFT_HIP_PITCH_JOINT] = std::make_shared(&left_leg_bus, 5); 429 | joint_ptrs[LEG_LEFT_KNEE_PITCH_JOINT] = std::make_shared(&left_leg_bus, 7); 430 | joint_ptrs[LEG_LEFT_ANKLE_PITCH_JOINT] = std::make_shared(&left_leg_bus, 11); 431 | joint_ptrs[LEG_LEFT_ANKLE_ROLL_JOINT] = std::make_shared(&left_leg_bus, 13); 432 | 433 | // right leg 434 | joint_ptrs[LEG_RIGHT_HIP_ROLL_JOINT] = std::make_shared(&right_leg_bus, 2); 435 | joint_ptrs[LEG_RIGHT_HIP_YAW_JOINT] = std::make_shared(&right_leg_bus, 4); 436 | joint_ptrs[LEG_RIGHT_HIP_PITCH_JOINT] = std::make_shared(&right_leg_bus, 6); 437 | joint_ptrs[LEG_RIGHT_KNEE_PITCH_JOINT] = std::make_shared(&right_leg_bus, 8); 438 | joint_ptrs[LEG_RIGHT_ANKLE_PITCH_JOINT] = std::make_shared(&right_leg_bus, 12); 439 | joint_ptrs[LEG_RIGHT_ANKLE_ROLL_JOINT] = std::make_shared(&right_leg_bus, 14); 440 | 441 | 442 | #if DEBUG_DISABLE_TRANSPORTS == 0 443 | for (int i = 0; i < N_JOINTS; i += 1) { 444 | joint_ptrs[i]->set_mode(MODE_IDLE); 445 | } 446 | #endif 447 | 448 | imu = new IMU(IMU_PATH, IMU_BAUDRATE); 449 | 450 | status = imu->init(); 451 | if (status < 0) { 452 | printf("[ERROR]
: Error initializing IMU\n"); 453 | exit(1); 454 | } 455 | 456 | status = initialize_udp(&udp, "0.0.0.0", POLICY_ACS_PORT, ROBOT_IP_ADDR, POLICY_OBS_PORT); 457 | if (status < 0) { 458 | printf("[ERROR]
: Error initializing UDP\n"); 459 | exit(1); 460 | } 461 | 462 | // Initialize joystick UDP socket to listen on ROBOT_IP:10011 and send to ROBOT_IP:10011 463 | status = initialize_udp(&udp_joystick, "0.0.0.0", JOYSTICK_PORT, "127.0.0.1", JOYSTICK_PORT); 464 | if (status < 0) { 465 | printf("[ERROR]
: Error initializing UDP joystick\n"); 466 | exit(1); 467 | } 468 | 469 | // 500 Hz UDP receive 470 | loop_udp_recv = std::make_shared("loop_udp_recv", 0.002, [this] { udp_recv(); }, 1, true, 49); 471 | 472 | // 500 Hz IMU input 473 | loop_imu = std::make_shared("loop_imu", 0.002, [this] { imu_loop(); }, 1, true, 49); 474 | 475 | // 20 Hz keyboard input 476 | loop_keyboard = std::make_shared("loop_keyboard", 0.05, [this] { keyboard_loop(); }, 0, true, 19); 477 | 478 | // 20 Hz joystick input 479 | loop_joystick = std::make_shared("loop_joystick", 0.05, [this] { joystick_loop(); }, 1, true, 19); 480 | 481 | // we want to ensure all packages are transmitted before killing this thread 482 | // 100 Hz control loop 483 | loop_control = std::make_shared("loop_control", 0.004, [this] { control_loop(); }, 0, false, 50, true); 484 | } 485 | 486 | 487 | void RealHumanoid::run() { 488 | initialize(); 489 | 490 | printf("Enabling motors...\n"); 491 | 492 | printf("read config joint_kp:\n ["); 493 | for (int i = 0; i < N_JOINTS; i += 1) { 494 | printf("%.3f ", joint_kp[i]); 495 | } 496 | printf("]\n"); 497 | printf("read config joint_kd:\n ["); 498 | for (int i = 0; i < N_JOINTS; i += 1) { 499 | printf("%.3f ", joint_kd[i]); 500 | } 501 | printf("]\n"); 502 | printf("read config torque_limit:\n ["); 503 | for (int i = 0; i < N_JOINTS; i += 1) { 504 | printf("%.3f ", torque_limit[i]); 505 | } 506 | printf("]\n"); 507 | 508 | #if DEBUG_DISABLE_TRANSPORTS == 0 509 | for (int i = 0; i < N_JOINTS; i += 1) { 510 | usleep(10); 511 | joint_ptrs[i]->write_position_kp(joint_kp[i]); 512 | usleep(10); 513 | joint_ptrs[i]->write_position_kd(joint_kd[i]); 514 | usleep(10); 515 | joint_ptrs[i]->write_torque_limit(torque_limit[i]); 516 | usleep(100); 517 | joint_ptrs[i]->feed(); 518 | joint_ptrs[i]->set_mode(MODE_DAMPING); 519 | } 520 | #endif 521 | 522 | printf("Motors enabled\n"); 523 | 524 | loop_udp_recv->start(); 525 | loop_keyboard->start(); 526 | loop_joystick->start(); 527 | loop_imu->start(); 528 | loop_control->start(); 529 | 530 | while (stopped != 2) { 531 | sleep(1); 532 | } 533 | } 534 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/robot/imu.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, The Berkeley Humanoid Lite Project Developers. 2 | 3 | import time 4 | import struct 5 | import threading 6 | 7 | import numpy as np 8 | from loop_rate_limiters import RateLimiter 9 | import serial 10 | 11 | 12 | class ImuRegisters: 13 | """ 14 | Register address mapping for the IMU. 15 | """ 16 | SAVE = 0x00 # save/reboot/factory reset 17 | CALSW = 0x01 # calibration mode 18 | RSW = 0x02 # output content 19 | RRATE = 0x03 # output rate 20 | BAUD = 0x04 # serial port baudrate 21 | AXOFFSET = 0x05 # acceleration X bias 22 | AYOFFSET = 0x06 # acceleration Y bias 23 | AZOFFSET = 0x07 # acceleration Z bias 24 | GXOFFSET = 0x08 # angular velocity X bias 25 | GYOFFSET = 0x09 # angular velocity Y bias 26 | GZOFFSET = 0x0A # angular velocity Z bias 27 | HXOFFSET = 0x0B # magnetic field X bias 28 | HYOFFSET = 0x0C # magnetic field Y bias 29 | HZOFFSET = 0x0D # magnetic field Z bias 30 | D0MODE = 0x0E # digital pin 0 mode 31 | D1MODE = 0x0F # digital pin 1 mode 32 | D2MODE = 0x10 # digital pin 2 mode 33 | D3MODE = 0x11 # digital pin 3 mode 34 | IICADDRESS = 0x1A # I2C device address 35 | LEDOFF = 0x1B # turn off the LED light 36 | MAGRANGX = 0x1C # magnetic field X calibration range 37 | MAGRANGY = 0x1D # magnetic field Y calibration range 38 | MAGRANGZ = 0x1E # magnetic field Z calibration range 39 | BANDWIDTH = 0x1F # bandwidth 40 | GYRORANGE = 0x20 # gyroscope range 41 | ACCRANGE = 0x21 # acceleration range 42 | SLEEP = 0x22 # hibernate 43 | ORIENT = 0x23 # installation direction 44 | AXIS6 = 0x24 # algorithm selection 45 | FILTK = 0x25 # dynamic filtering 46 | GPSBAUD = 0x26 # GPS baudrate 47 | READADDR = 0x27 # read register 48 | ACCFILT = 0x2A # acceleration filter 49 | POWONSEND = 0x2D # command start 50 | VERSION = 0x2E # version number 51 | YYMM = 0x30 # year and month 52 | DDHH = 0x31 # day and hour 53 | MMSS = 0x32 # minute and second 54 | MS = 0x33 # millisecond 55 | AX = 0x34 # acceleration X 56 | AY = 0x35 # acceleration Y 57 | AZ = 0x36 # acceleration Z 58 | GX = 0x37 # angular velocity X 59 | GY = 0x38 # angular velocity Y 60 | GZ = 0x39 # angular velocity Z 61 | HX = 0x3A # magnetic field X 62 | HY = 0x3B # magnetic field Y 63 | HZ = 0x3C # magnetic field Z 64 | ROLL = 0x3D # roll angle 65 | PITCH = 0x3E # pitch angle 66 | YAW = 0x3F # yaw angle 67 | TEMP = 0x40 # temperature 68 | D0STATUS = 0x41 # digital pin 0 state 69 | D1STATUS = 0x42 # digital pin 1 state 70 | D2STATUS = 0x43 # digital pin 2 state 71 | D3STATUS = 0x44 # digital pin 3 state 72 | PRESSUREL = 0x45 # pressure lower 16 bits 73 | PRESSUREH = 0x46 # pressure upper 16 bits 74 | HEIGHTL = 0x47 # height lower 16 bits 75 | HEIGHTH = 0x48 # height upper 16 bits 76 | LONL = 0x49 # longitude lower 16 bits 77 | LONH = 0x4A # longitude upper 16 bits 78 | LATL = 0x4B # latitude lower 16 bits 79 | LATH = 0x4C # latitude upper 16 bits 80 | GPSHEIGHT = 0x4D # GPS altitude 81 | GPSYAW = 0x4E # GPS heading angle 82 | GPSVL = 0x4F # GPS ground speed lower 16 bits 83 | GPSVH = 0x50 # GPS ground speed upper 16 bits 84 | Q0 = 0x51 # quaternion 0 85 | Q1 = 0x52 # quaternion 1 86 | Q2 = 0x53 # quaternion 2 87 | Q3 = 0x54 # quaternion 3 88 | SVNUM = 0x55 # number of visible satellites 89 | PDOP = 0x56 # position accuracy 90 | HDOP = 0x57 # horizontal accuracy 91 | VDOP = 0x58 # vertical accuracy 92 | DELAYT = 0x59 # alarm signal delay 93 | XMIN = 0x5A # X-axis angle alarm minimum value 94 | XMAX = 0x5B # X-axis angle alarm maximum value 95 | BATVAL = 0x5C # battery supply voltage 96 | ALARMPIN = 0x5D # alarm pin mapping 97 | YMIN = 0x5E # Y-axis angle alarm minimum value 98 | YMAX = 0x5F # Y-axis angle alarm maximum value 99 | GYROCALITHR = 0x61 # gyroscope still threshold 100 | ALARMLEVEL = 0x62 # alarm level 101 | GYROCALTIME = 0x63 # gyroscope auto calibration time 102 | TRIGTIME = 0x68 # alarm continuous trigger time 103 | KEY = 0x69 # unlock key 104 | WERROR = 0x6A # gyroscope change value 105 | TIMEZONE = 0x6B # GPS time zone 106 | WZTIME = 0x6E # angular velocity continuous rest time 107 | WZSTATIC = 0x6F # angular velocity integral threshold 108 | MODDELAY = 0x74 # RS485 data response delay 109 | XREFROLL = 0x79 # roll angle zero reference value 110 | YREFPITCH = 0x7A # pitch angle zero reference value 111 | NUMBERID1 = 0x7F # number ID 1-2 112 | NUMBERID2 = 0x80 # number ID 3-4 113 | NUMBERID3 = 0x81 # number ID 5-6 114 | NUMBERID4 = 0x82 # number ID 7-8 115 | NUMBERID5 = 0x83 # number ID 9-10 116 | NUMBERID6 = 0x84 # number ID 11-12 117 | 118 | 119 | class FrameType: 120 | TIME = 0x50 121 | ACCELERATION = 0x51 122 | ANGULAR_VELOCITY = 0x52 123 | ANGLE = 0x53 124 | MAGNETIC_FIELD = 0x54 125 | PORT_STATUS = 0x55 126 | BAROMETER_ALTITUDE = 0x56 127 | LATITUDE_LONGITUDE = 0x57 128 | GROUND_SPEED = 0x58 129 | QUATERNION = 0x59 130 | GPS_POSITION_ACCURACY = 0x5A 131 | READY = 0x5F 132 | 133 | 134 | class SamplingRate: 135 | RATE_0_2_HZ = 0x01 136 | RATE_0_5_HZ = 0x02 137 | RATE_1_HZ = 0x03 138 | RATE_2_HZ = 0x04 139 | RATE_5_HZ = 0x05 140 | RATE_10_HZ = 0x06 141 | RATE_20_HZ = 0x07 142 | RATE_50_HZ = 0x08 143 | RATE_100_HZ = 0x09 144 | RATE_200_HZ = 0x0B 145 | RATE_SINGLE = 0x0C 146 | RATE_NO_RETURN = 0x0D 147 | 148 | 149 | class Baudrate: 150 | BAUD_4800 = 0x01 151 | BAUD_9600 = 0x02 152 | BAUD_19200 = 0x03 153 | BAUD_38400 = 0x04 154 | BAUD_57600 = 0x05 155 | BAUD_115200 = 0x06 156 | BAUD_230400 = 0x07 157 | BAUD_460800 = 0x08 158 | # BAUD_921600 = 0x09 159 | 160 | 161 | class SerialImu: 162 | """ 163 | Driver for the HiWonder IM10A 10-axis USB IMU. 164 | 165 | @see https://www.hiwonder.com/products/imu-module 166 | 167 | To change configurations, the following steps can be performed first: 168 | 1. send UNLOCK command 169 | 2. send commands to modify or read configuration data 170 | 3. send SAVE command to apply the settings 171 | 172 | The commands must be completed within 10 seconds, otherwise the IMU will 173 | automatically be locked. 174 | """ 175 | FRAME_LENGTH = 11 176 | 177 | @staticmethod 178 | def baud_to_int(baudrate: int) -> int: 179 | match baudrate: 180 | case Baudrate.BAUD_4800: 181 | return 4800 182 | case Baudrate.BAUD_9600: 183 | return 9600 184 | case Baudrate.BAUD_19200: 185 | return 19200 186 | case Baudrate.BAUD_38400: 187 | return 38400 188 | case Baudrate.BAUD_57600: 189 | return 57600 190 | case Baudrate.BAUD_115200: 191 | return 115200 192 | case Baudrate.BAUD_230400: 193 | return 230400 194 | case Baudrate.BAUD_460800: 195 | return 460800 196 | # case Baudrate.BAUD_921600: 197 | # return 921600 198 | return 0 199 | 200 | def __init__(self, port: str = "/dev/ttyUSB0", baudrate: int = Baudrate.BAUD_115200, read_timeout=4): 201 | self.port: str = port 202 | self.baud: int = baudrate 203 | self.read_timeout: int = read_timeout 204 | 205 | baudrate_int = self.baud_to_int(baudrate) 206 | self.ser: serial.Serial = serial.Serial(self.port, baudrate_int, timeout=self.read_timeout) 207 | 208 | print("Serial is Opened:", self.ser.is_open) 209 | 210 | self.is_stopped: threading.Event = threading.Event() 211 | self.is_stopped.clear() 212 | 213 | # === IMU readings === 214 | # seconds 215 | self.timestamp: float = 0.0 216 | # Celcius degree 217 | self.temperature: float = 0.0 218 | # (x, y, z) m/s^2 219 | self.acceleration: np.ndarray = np.zeros(3, dtype=np.float32) 220 | # (x, y, z) deg/s 221 | self.angular_velocity: np.ndarray = np.zeros(3, dtype=np.float32) 222 | # (yaw, pitch, roll) deg 223 | self.angle: np.ndarray = np.zeros(3, dtype=np.float32) 224 | # (x, y, z) μT 225 | self.magnetic_field: np.ndarray = np.zeros(3, dtype=np.float32) 226 | # (w, x, y, z) 227 | self.quaternion: np.ndarray = np.zeros(4, dtype=np.float32) 228 | 229 | # self.__debug_last_time: float = time.perf_counter_ns() 230 | 231 | def __read_frame(self) -> None: 232 | """ 233 | Parse a frame from the serial port. 234 | """ 235 | start = self.ser.read(1) 236 | start, = struct.unpack(" None: 279 | """ 280 | Start the IMU reading loop. 281 | """ 282 | self.start_time = time.time() 283 | while not self.is_stopped.is_set(): 284 | self.__read_frame() 285 | 286 | def run_forever(self) -> None: 287 | """ 288 | Start the IMU reading loop in a separate thread with high priority. 289 | """ 290 | # self.thread = threading.Thread(target=self.run, daemon=True) 291 | self.thread = threading.Thread(target=self.run) 292 | self.thread.start() 293 | 294 | # Set thread priority to high 295 | try: 296 | import os 297 | import psutil 298 | process = psutil.Process(os.getpid()) 299 | process.nice(0) # Set process priority to high (-20 is highest, 19 is lowest) 300 | except (ImportError, PermissionError): 301 | print("Warning: Could not set thread priority. Running with default priority.") 302 | 303 | def stop(self) -> None: 304 | """ 305 | Stop the IMU reading loop. 306 | """ 307 | self.is_stopped.set() 308 | 309 | def unlock(self) -> None: 310 | """ 311 | Unlock the IMU to allow configuration changes. 312 | """ 313 | register_addr = 0x69 314 | data = 0xB588 315 | frame = struct.pack(" None: 319 | """ 320 | Save the configurations permanently. 321 | """ 322 | register_addr = 0x00 323 | data = 0x0000 324 | frame = struct.pack(" None: 328 | frame = struct.pack(" None: 345 | """Set the output content configuration for the IMU. 346 | 347 | Args: 348 | time: Enable time output 349 | acceleration: Enable acceleration output 350 | angular_velocity: Enable angular velocity output 351 | angle: Enable angle output 352 | magnetic_field: Enable magnetic field output 353 | port_status: Enable port status output 354 | pressure: Enable pressure output 355 | gps: Enable GPS output 356 | velocity: Enable velocity output 357 | quaternion: Enable quaternion output 358 | gps_position_accuracy: Enable GPS position accuracy output 359 | 360 | Returns: 361 | int: The output content configuration value where each bit represents 362 | whether that particular data type should be included in the output. 363 | """ 364 | # Validate all inputs are boolean 365 | for param_name, param_value in locals().items(): 366 | if param_name != "self" and not isinstance(param_value, bool): 367 | raise TypeError(f"Parameter {param_name} must be a boolean") 368 | 369 | output_content = 0x00 370 | output_content |= (time << 0) 371 | output_content |= (acceleration << 1) 372 | output_content |= (angular_velocity << 2) 373 | output_content |= (angle << 3) 374 | output_content |= (magnetic_field << 4) 375 | output_content |= (port_status << 5) 376 | output_content |= (pressure << 6) 377 | output_content |= (gps << 7) 378 | output_content |= (velocity << 8) 379 | output_content |= (quaternion << 9) 380 | output_content |= (gps_position_accuracy << 10) 381 | 382 | self.write_frame(ImuRegisters.RSW, output_content) 383 | 384 | def set_sampling_rate(self, rate: int) -> None: 385 | self.write_frame(ImuRegisters.RRATE, rate) 386 | 387 | def set_baudrate(self, baudrate: int) -> None: 388 | self.baud = baudrate 389 | 390 | # wait for the unlock to be set 391 | time.sleep(0.1) 392 | self.write_frame(ImuRegisters.BAUD, baudrate) 393 | # wait for the baudrate to be set 394 | time.sleep(0.1) 395 | 396 | # reopen the serial port with the new baudrate 397 | del self.ser 398 | baudrate_int = self.baud_to_int(baudrate) 399 | self.ser = serial.Serial(self.port, baudrate_int, timeout=self.read_timeout) 400 | 401 | 402 | if __name__ == "__main__": 403 | imu = SerialImu(baudrate=Baudrate.BAUD_460800) 404 | 405 | # change baudrate: 406 | # imu.unlock() 407 | # time.sleep(0.1) 408 | # imu.set_baudrate(Baudrate.BAUD_115200) 409 | # time.sleep(0.1) 410 | # imu.save() 411 | 412 | # set sampling rate: 413 | # imu.unlock() 414 | # imu.set_sampling_rate(SamplingRate.RATE_200_HZ) 415 | # imu.save() 416 | 417 | # # set output content: 418 | # imu.unlock() 419 | # time.sleep(0.1) 420 | # imu.set_output_content( 421 | # acceleration=True, 422 | # angular_velocity=True, 423 | # quaternion=True, 424 | # ) 425 | # time.sleep(0.1) 426 | # imu.save() 427 | 428 | # time.sleep(0.1) 429 | # imu_reader.save() 430 | # # time.sleep(0.2) 431 | 432 | imu.run_forever() 433 | 434 | rate = RateLimiter(100) 435 | 436 | print("IMU reader started") 437 | 438 | try: 439 | while True: 440 | print(f"ax: {imu.acceleration[0]:.2f}\tay: {imu.acceleration[1]:.2f}\taz: {imu.acceleration[2]:.2f}", end="\t") 441 | print(f"gx: {imu.angular_velocity[0]:.2f}\tgy: {imu.angular_velocity[1]:.2f}\tgz: {imu.angular_velocity[2]:.2f}", end="\t") 442 | print(f"qw: {imu.quaternion[0]:.2f}\tqx: {imu.quaternion[1]:.2f}\tqy: {imu.quaternion[2]:.2f}\tqz: {imu.quaternion[3]:.2f}") 443 | rate.sleep() 444 | except KeyboardInterrupt: 445 | imu.stop() 446 | imu.ser.close() 447 | print("IMU reader stopped") 448 | -------------------------------------------------------------------------------- /berkeley_humanoid_lite_lowlevel/recoil/core.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025, -T.K.-. 2 | 3 | import math 4 | import struct 5 | import time 6 | 7 | import can 8 | 9 | 10 | class Function: 11 | NMT = 0b0000 12 | SYNC_EMCY = 0b0001 13 | TIME = 0b0010 14 | TRANSMIT_PDO_1 = 0b0011 15 | RECEIVE_PDO_1 = 0b0100 16 | TRANSMIT_PDO_2 = 0b0101 17 | RECEIVE_PDO_2 = 0b0110 18 | TRANSMIT_PDO_3 = 0b0111 19 | RECEIVE_PDO_3 = 0b1000 20 | TRANSMIT_PDO_4 = 0b1001 21 | RECEIVE_PDO_4 = 0b1010 22 | TRANSMIT_SDO = 0b1011 23 | RECEIVE_SDO = 0b1100 24 | FLASH = 0b1101 25 | HEARTBEAT = 0b1110 26 | 27 | 28 | class Mode: 29 | # these are three safe modes 30 | DISABLED = 0x00 31 | IDLE = 0x01 32 | 33 | # these are special modes 34 | DAMPING = 0x02 35 | CALIBRATION = 0x05 36 | 37 | # these are closed-loop modes 38 | CURRENT = 0x10 39 | TORQUE = 0x11 40 | VELOCITY = 0x12 41 | POSITION = 0x13 42 | 43 | # these are open-loop modes 44 | VABC_OVERRIDE = 0x20 45 | VALPHABETA_OVERRIDE = 0x21 46 | VQD_OVERRIDE = 0x22 47 | 48 | DEBUG = 0x80 49 | 50 | 51 | class ErrorCode: 52 | NO_ERROR = 0b0000000000000000 53 | GENERAL = 0b0000000000000001 54 | ESTOP = 0b0000000000000010 55 | INITIALIZATION_ERROR = 0b0000000000000100 56 | CALIBRATION_ERROR = 0b0000000000001000 57 | POWERSTAGE_ERROR = 0b0000000000010000 58 | INVALID_MODE = 0b0000000000100000 59 | WATCHDOG_TIMEOUT = 0b0000000001000000 60 | OVER_VOLTAGE = 0b0000000010000000 61 | OVER_CURRENT = 0b0000000100000000 62 | OVER_TEMPERATURE = 0b0000001000000000 63 | CAN_TX_FAULT = 0b0000010000000000 64 | I2C_FAULT = 0b0000100000000000 65 | 66 | 67 | # supported version: >= 1.1.1 68 | class Parameter: 69 | DEVICE_ID = 0x000 70 | FIRMWARE_VERSION = 0x004 71 | WATCHDOG_TIMEOUT = 0x008 72 | FAST_FRAME_FREQUENCY = 0x00C 73 | MODE = 0x010 74 | ERROR = 0x014 75 | POSITION_CONTROLLER_UPDATE_COUNTER = 0x018 76 | POSITION_CONTROLLER_GEAR_RATIO = 0x01C 77 | POSITION_CONTROLLER_POSITION_KP = 0x020 78 | POSITION_CONTROLLER_POSITION_KI = 0x024 79 | POSITION_CONTROLLER_VELOCITY_KP = 0x028 80 | POSITION_CONTROLLER_VELOCITY_KI = 0x02C 81 | POSITION_CONTROLLER_TORQUE_LIMIT = 0x030 82 | POSITION_CONTROLLER_VELOCITY_LIMIT = 0x034 83 | POSITION_CONTROLLER_POSITION_LIMIT_LOWER = 0x038 84 | POSITION_CONTROLLER_POSITION_LIMIT_UPPER = 0x03C 85 | POSITION_CONTROLLER_POSITION_OFFSET = 0x040 86 | POSITION_CONTROLLER_TORQUE_TARGET = 0x044 87 | POSITION_CONTROLLER_TORQUE_MEASURED = 0x048 88 | POSITION_CONTROLLER_TORQUE_SETPOINT = 0x04C 89 | POSITION_CONTROLLER_VELOCITY_TARGET = 0x050 90 | POSITION_CONTROLLER_VELOCITY_MEASURED = 0x054 91 | POSITION_CONTROLLER_VELOCITY_SETPOINT = 0x058 92 | POSITION_CONTROLLER_POSITION_TARGET = 0x05C 93 | POSITION_CONTROLLER_POSITION_MEASURED = 0x060 94 | POSITION_CONTROLLER_POSITION_SETPOINT = 0x064 95 | POSITION_CONTROLLER_POSITION_INTEGRATOR = 0x068 96 | POSITION_CONTROLLER_VELOCITY_INTEGRATOR = 0x06C 97 | POSITION_CONTROLLER_TORQUE_FILTER_ALPHA = 0x070 98 | CURRENT_CONTROLLER_I_LIMIT = 0x074 99 | CURRENT_CONTROLLER_I_KP = 0x078 100 | CURRENT_CONTROLLER_I_KI = 0x07C 101 | CURRENT_CONTROLLER_I_A_MEASURED = 0x080 102 | CURRENT_CONTROLLER_I_B_MEASURED = 0x084 103 | CURRENT_CONTROLLER_I_C_MEASURED = 0x088 104 | CURRENT_CONTROLLER_V_A_SETPOINT = 0x08C 105 | CURRENT_CONTROLLER_V_B_SETPOINT = 0x090 106 | CURRENT_CONTROLLER_V_C_SETPOINT = 0x094 107 | CURRENT_CONTROLLER_I_ALPHA_MEASURED = 0x098 108 | CURRENT_CONTROLLER_I_BETA_MEASURED = 0x09C 109 | CURRENT_CONTROLLER_V_ALPHA_SETPOINT = 0x0A0 110 | CURRENT_CONTROLLER_V_BETA_SETPOINT = 0x0A4 111 | CURRENT_CONTROLLER_V_Q_TARGET = 0x0A8 112 | CURRENT_CONTROLLER_V_D_TARGET = 0x0AC 113 | CURRENT_CONTROLLER_V_Q_SETPOINT = 0x0B0 114 | CURRENT_CONTROLLER_V_D_SETPOINT = 0x0B4 115 | CURRENT_CONTROLLER_I_Q_TARGET = 0x0B8 116 | CURRENT_CONTROLLER_I_D_TARGET = 0x0BC 117 | CURRENT_CONTROLLER_I_Q_MEASURED = 0x0C0 118 | CURRENT_CONTROLLER_I_D_MEASURED = 0x0C4 119 | CURRENT_CONTROLLER_I_Q_SETPOINT = 0x0C8 120 | CURRENT_CONTROLLER_I_D_SETPOINT = 0x0CC 121 | CURRENT_CONTROLLER_I_Q_INTEGRATOR = 0x0D0 122 | CURRENT_CONTROLLER_I_D_INTEGRATOR = 0x0D4 123 | POWERSTAGE_HTIM = 0x0D8 124 | POWERSTAGE_HADC1 = 0x0DC 125 | POWERSTAGE_HADC2 = 0x0E0 126 | POWERSTAGE_ADC_READING_RAW = 0x0E4 127 | POWERSTAGE_ADC_READING_OFFSET = 0x0EC 128 | POWERSTAGE_UNDERVOLTAGE_THRESHOLD = 0x0F4 129 | POWERSTAGE_OVERVOLTAGE_THRESHOLD = 0x0F8 130 | POWERSTAGE_BUS_VOLTAGE_FILTER_ALPHA = 0x0FC 131 | POWERSTAGE_BUS_VOLTAGE_MEASURED = 0x100 132 | MOTOR_POLE_PAIRS = 0x104 133 | MOTOR_TORQUE_CONSTANT = 0x108 134 | MOTOR_PHASE_ORDER = 0x10C 135 | MOTOR_MAX_CALIBRATION_CURRENT = 0x110 136 | ENCODER_HI2C = 0x114 137 | ENCODER_I2C_BUFFER = 0x118 138 | ENCODER_I2C_UPDATE_COUNTER = 0x11C 139 | ENCODER_CPR = 0x120 140 | ENCODER_POSITION_OFFSET = 0x124 141 | ENCODER_VELOCITY_FILTER_ALPHA = 0x128 142 | ENCODER_POSITION_RAW = 0x12C 143 | ENCODER_N_ROTATIONS = 0x130 144 | ENCODER_POSITION = 0x134 145 | ENCODER_VELOCITY = 0x138 146 | ENCODER_FLUX_OFFSET = 0x13C 147 | ENCODER_FLUX_OFFSET_TABLE = 0x140 148 | # end: 840 0x348 149 | 150 | 151 | class DataFrame: 152 | def __init__( 153 | self, 154 | device_id: int = 0, 155 | func_id: int | None = None, 156 | size: int = 0, 157 | data: bytes | bytearray = b"" 158 | ): 159 | self.device_id = device_id 160 | self.func_id = func_id 161 | self.size = size 162 | self.data = data 163 | 164 | assert self.size == len(self.data) 165 | 166 | 167 | class CANFrame(DataFrame): 168 | ID_STANDARD = 0 169 | ID_EXTENDED = 1 170 | 171 | DEVICE_ID_MSK = 0x7F 172 | FUNC_ID_POS = 7 173 | FUNC_ID_MSK = 0x0F << FUNC_ID_POS 174 | 175 | def __init__( 176 | self, 177 | device_id: int = 0, 178 | func_id: int | None = None, 179 | size: int = 0, 180 | data: bytes | bytearray = b"" 181 | ): 182 | super().__init__(device_id, func_id, size, data) 183 | assert self.size <= 8 184 | 185 | 186 | class Bus: 187 | @staticmethod 188 | def unpack(format_str, data) -> tuple: 189 | try: 190 | return struct.unpack(format_str, data) 191 | except struct.error as e: 192 | print("warning:", e, data) 193 | return () 194 | 195 | def __init__(self, channel: str, bitrate: int = 1000000): 196 | """ 197 | Args: 198 | channel (str): The port to use for communication, e.g., "can0" 199 | bitrate (int): The bitrate for the CAN bus, default is 1 Mbps 200 | """ 201 | self.channel = channel 202 | self.bitrate = bitrate 203 | 204 | self.__bus = can.interface.Bus(interface="socketcan", channel=self.channel, bitrate=self.bitrate) 205 | 206 | def __del__(self): 207 | self.stop() 208 | 209 | def stop(self): 210 | self.__bus.shutdown() 211 | 212 | """ 213 | Receive data. 214 | 215 | timeout == None: blocking forever 216 | timeout == 0: non-blocking (the actual delay is around 0.1s) 217 | timeout > 0: blocking for timeout seconds 218 | 219 | @param timeout: timeout in seconds 220 | """ 221 | def receive(self, 222 | filter_device_id: int | None = None, 223 | filter_function: int | None = None, 224 | timeout=None 225 | ) -> CANFrame | None: 226 | while True: 227 | try: 228 | msg = self.__bus.recv(timeout=timeout) 229 | except can.exceptions.CanOperationError as e: 230 | print(" error:", e) 231 | return None 232 | except TypeError as e: 233 | print(" error:", e) 234 | return None 235 | 236 | if not msg: 237 | return None 238 | 239 | if msg.is_error_frame: 240 | print(f"{time.time()} <{self.channel}> Error Frame: {msg.arbitration_id}, {msg.dlc}") 241 | continue 242 | 243 | frame = CANFrame( 244 | device_id=msg.arbitration_id & CANFrame.DEVICE_ID_MSK, 245 | func_id=msg.arbitration_id >> CANFrame.FUNC_ID_POS, 246 | size=msg.dlc, 247 | data=msg.data 248 | ) 249 | if filter_device_id: 250 | if frame.device_id != filter_device_id: 251 | continue 252 | if filter_function: 253 | if frame.func_id != filter_function: 254 | continue 255 | 256 | return frame 257 | 258 | def transmit(self, frame: CANFrame): 259 | assert frame.device_id <= CANFrame.DEVICE_ID_MSK, "device_id: {0} out of range".format(frame.device_id) 260 | assert frame.func_id is not None 261 | 262 | can_id = (frame.func_id << CANFrame.FUNC_ID_POS) | frame.device_id 263 | 264 | msg = can.Message( 265 | arbitration_id=can_id, 266 | is_extended_id=False, 267 | data=frame.data) 268 | 269 | self.__bus.send(msg) 270 | 271 | def ping(self, device_id: int, timeout=0.1) -> bool: 272 | self.transmit(CANFrame(device_id, Function.RECEIVE_PDO_1, size=1, data=b"\xCA")) 273 | rx_frame = self.receive(filter_device_id=device_id, filter_function=Function.TRANSMIT_PDO_1, timeout=timeout) 274 | if not rx_frame: 275 | return False 276 | rx_data = self.unpack(" None: 280 | self.transmit(CANFrame(device_id, Function.HEARTBEAT)) 281 | 282 | def set_mode(self, device_id: int, mode: Mode) -> None: 283 | self.transmit(CANFrame( 284 | device_id, 285 | Function.NMT, 286 | size=2, 287 | data=struct.pack(" None: 291 | self.transmit(CANFrame( 292 | device_id, 293 | Function.FLASH, 294 | size=1, 295 | data=struct.pack(" None: 299 | self.transmit(CANFrame( 300 | device_id, 301 | Function.FLASH, 302 | size=1, 303 | data=struct.pack(" CANFrame | None: 307 | self.transmit(CANFrame( 308 | device_id, 309 | Function.RECEIVE_SDO, 310 | size=3, 311 | data=struct.pack(" None: 317 | self.transmit(CANFrame( 318 | device_id, 319 | Function.RECEIVE_SDO, 320 | size=8, 321 | data=struct.pack(" bytes | bytearray | None: 325 | rx_frame = self._read_parameter(device_id, param_id, timeout) 326 | if not rx_frame: 327 | return None 328 | rx_data = rx_frame.data[0:4] 329 | return rx_data 330 | 331 | def _read_parameter_f32(self, device_id: int, param_id: int, timeout=None) -> float | None: 332 | rx_frame = self._read_parameter(device_id, param_id, timeout) 333 | if not rx_frame: 334 | return None 335 | rx_data = self.unpack(" int | None: 339 | rx_frame = self._read_parameter(device_id, param_id, timeout) 340 | if not rx_frame: 341 | return None 342 | rx_data = self.unpack(" int | None: 346 | rx_frame = self._read_parameter(device_id, param_id, timeout) 347 | if not rx_frame: 348 | return None 349 | rx_data = self.unpack("= 0, "value must be unsigned integer" 367 | tx_data = struct.pack(" int | None: 373 | return self._read_parameter_u32(device_id, Parameter.FAST_FRAME_FREQUENCY) 374 | 375 | def write_fast_frame_frequency(self, device_id: int, value: int) -> None: 376 | self._write_parameter_u32(device_id, Parameter.FAST_FRAME_FREQUENCY, value) 377 | 378 | def read_gear_ratio(self, device_id: int) -> float | None: 379 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_GEAR_RATIO) 380 | 381 | def write_gear_ratio(self, device_id: int, value: float): 382 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_GEAR_RATIO, value) 383 | 384 | def read_position_kp(self, device_id: int) -> float | None: 385 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_KP) 386 | 387 | def write_position_kp(self, device_id: int, value: float): 388 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_KP, value) 389 | 390 | def read_position_kd(self, device_id: int) -> float | None: 391 | return self.read_velocity_kp(device_id) 392 | 393 | def write_position_kd(self, device_id: int, value: float): 394 | self.write_velocity_kp(device_id, value) 395 | 396 | def read_position_ki(self, device_id: int) -> float | None: 397 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_KI) 398 | 399 | def write_position_ki(self, device_id: int, value: float): 400 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_KI, value) 401 | 402 | def read_velocity_kp(self, device_id: int) -> float | None: 403 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_KP) 404 | 405 | def write_velocity_kp(self, device_id: int, value: float): 406 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_KP, value) 407 | 408 | def read_velocity_ki(self, device_id: int) -> float | None: 409 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_KI) 410 | 411 | def write_velocity_ki(self, device_id: int, value: float): 412 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_KI, value) 413 | 414 | def read_torque_limit(self, device_id: int) -> float | None: 415 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_TORQUE_LIMIT) 416 | 417 | def write_torque_limit(self, device_id: int, value: float): 418 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_TORQUE_LIMIT, value) 419 | 420 | def read_velocity_limit(self, device_id: int) -> float | None: 421 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_LIMIT) 422 | 423 | def write_velocity_limit(self, device_id: int, value: float): 424 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_LIMIT, value) 425 | 426 | def read_position_limit_lower(self, device_id: int) -> float | None: 427 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_LIMIT_LOWER) 428 | 429 | def write_position_limit_lower(self, device_id: int, value: float): 430 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_LIMIT_LOWER, value) 431 | 432 | def read_position_limit_upper(self, device_id: int) -> float | None: 433 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_LIMIT_UPPER) 434 | 435 | def write_position_limit_upper(self, device_id: int, value: float): 436 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_LIMIT_UPPER, value) 437 | 438 | def read_position_offset(self, device_id: int) -> float | None: 439 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_OFFSET) 440 | 441 | def write_position_offset(self, device_id: int, value: float): 442 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_OFFSET, value) 443 | 444 | def read_torque_target(self, device_id: int) -> float | None: 445 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_TORQUE_TARGET) 446 | 447 | def write_torque_target(self, device_id: int, value: float): 448 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_TORQUE_TARGET, value) 449 | 450 | def read_torque_measured(self, device_id: int) -> float | None: 451 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_TORQUE_MEASURED) 452 | 453 | def read_velocity_target(self, device_id: int) -> float | None: 454 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_TARGET) 455 | 456 | def write_velocity_target(self, device_id: int, value: float): 457 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_TARGET, value) 458 | 459 | def read_velocity_measured(self, device_id: int) -> float | None: 460 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_VELOCITY_MEASURED) 461 | 462 | def read_position_target(self, device_id: int) -> float | None: 463 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_TARGET) 464 | 465 | def write_position_target(self, device_id: int, value: float): 466 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_TARGET, value) 467 | 468 | def read_position_measured(self, device_id: int) -> float | None: 469 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_POSITION_MEASURED) 470 | 471 | def read_torque_filter_alpha(self, device_id: int) -> float | None: 472 | return self._read_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_TORQUE_FILTER_ALPHA) 473 | 474 | def write_torque_filter_alpha(self, device_id: int, value: float): 475 | self._write_parameter_f32(device_id, Parameter.POSITION_CONTROLLER_TORQUE_FILTER_ALPHA, value) 476 | 477 | def read_current_limit(self, device_id: int) -> float | None: 478 | return self._read_parameter_f32(device_id, Parameter.CURRENT_CONTROLLER_I_LIMIT) 479 | 480 | def write_current_limit(self, device_id: int, value: float): 481 | self._write_parameter_f32(device_id, Parameter.CURRENT_CONTROLLER_I_LIMIT, value) 482 | 483 | def read_current_kp(self, device_id: int) -> float | None: 484 | return self._read_parameter_f32(device_id, Parameter.CURRENT_CONTROLLER_I_KP) 485 | 486 | def write_current_kp(self, device_id: int, value: float): 487 | self._write_parameter_f32(device_id, Parameter.CURRENT_CONTROLLER_I_KP, value) 488 | 489 | def read_current_ki(self, device_id: int) -> float | None: 490 | return self._read_parameter_f32(device_id, Parameter.CURRENT_CONTROLLER_I_KI) 491 | 492 | def write_current_ki(self, device_id: int, value: float): 493 | self._write_parameter_f32(device_id, Parameter.CURRENT_CONTROLLER_I_KI, value) 494 | 495 | def read_bus_voltage_filter_alpha(self, device_id: int) -> float | None: 496 | return self._read_parameter_f32(device_id, Parameter.POWERSTAGE_BUS_VOLTAGE_FILTER_ALPHA) 497 | 498 | def write_bus_voltage_filter_alpha(self, device_id: int, value: float): 499 | self._write_parameter_f32(device_id, Parameter.POWERSTAGE_BUS_VOLTAGE_FILTER_ALPHA, value) 500 | 501 | def read_motor_pole_pairs(self, device_id: int) -> int | None: 502 | return self._read_parameter_u32(device_id, Parameter.MOTOR_POLE_PAIRS) 503 | 504 | def write_motor_pole_pairs(self, device_id: int, value: int): 505 | return self._write_parameter_u32(device_id, Parameter.MOTOR_POLE_PAIRS, value) 506 | 507 | def read_motor_torque_constant(self, device_id: int) -> float | None: 508 | return self._read_parameter_f32(device_id, Parameter.MOTOR_TORQUE_CONSTANT) 509 | 510 | def write_motor_torque_constant(self, device_id: int, value: float): 511 | return self._write_parameter_f32(device_id, Parameter.MOTOR_TORQUE_CONSTANT, value) 512 | 513 | def read_motor_phase_order(self, device_id: int) -> int | None: 514 | return self._read_parameter_i32(device_id, Parameter.MOTOR_PHASE_ORDER) 515 | 516 | def write_motor_phase_order(self, device_id: int, value: int): 517 | return self._write_parameter_i32(device_id, Parameter.MOTOR_PHASE_ORDER, value) 518 | 519 | def read_motor_calibration_current(self, device_id: int) -> float | None: 520 | return self._read_parameter_f32(device_id, Parameter.MOTOR_MAX_CALIBRATION_CURRENT) 521 | 522 | def write_motor_calibration_current(self, device_id: int, value: float): 523 | return self._write_parameter_f32(device_id, Parameter.MOTOR_MAX_CALIBRATION_CURRENT, value) 524 | 525 | def read_encoder_cpr(self, device_id: int) -> int | None: 526 | return self._read_parameter_u32(device_id, Parameter.ENCODER_CPR) 527 | 528 | def write_encoder_cpr(self, device_id: int, value: int): 529 | return self._write_parameter_u32(device_id, Parameter.ENCODER_CPR, value) 530 | 531 | def read_encoder_position_offset(self, device_id: int) -> float | None: 532 | return self._read_parameter_f32(device_id, Parameter.ENCODER_POSITION_OFFSET) 533 | 534 | def write_encoder_position_offset(self, device_id: int, value: float): 535 | return self._write_parameter_f32(device_id, Parameter.ENCODER_POSITION_OFFSET, value) 536 | 537 | def read_encoder_velocity_filter_alpha(self, device_id: int) -> float | None: 538 | return self._read_parameter_f32(device_id, Parameter.ENCODER_VELOCITY_FILTER_ALPHA) 539 | 540 | def write_encoder_velocity_filter_alpha(self, device_id: int, value: float): 541 | return self._write_parameter_f32(device_id, Parameter.ENCODER_VELOCITY_FILTER_ALPHA, value) 542 | 543 | def read_encoder_flux_offset(self, device_id: int) -> float | None: 544 | return self._read_parameter_f32(device_id, Parameter.ENCODER_FLUX_OFFSET) 545 | 546 | def write_encoder_flux_offset(self, device_id: int, value: float): 547 | return self._write_parameter_f32(device_id, Parameter.ENCODER_FLUX_OFFSET, value) 548 | 549 | # Quick helper functions 550 | def set_current_bandwidth(self, device_id: int, bandwidth_hz: float, phase_resistance: float, phase_inductance: float): 551 | kp = bandwidth_hz * 2.0 * math.pi * phase_inductance 552 | ki = phase_resistance / phase_inductance 553 | print(f"Calculated current kp: {kp:.4f}, ki: {ki:.4f}") 554 | self.write_current_kp(device_id, kp) 555 | self.write_current_ki(device_id, ki) 556 | 557 | def set_torque_bandwidth(self, device_id: int, bandwidth_hz: float, position_loop_rate: float = 2000): 558 | alpha = min(max(1. - math.exp(-2. * math.pi * (bandwidth_hz / position_loop_rate)), 0.), 1.) 559 | print(f"Calculated torque filter alpha: {alpha:.4f}") 560 | self.write_torque_filter_alpha(device_id, alpha) 561 | 562 | def set_bus_voltage_bandwidth(self, device_id: int, bandwidth_hz: float, bus_voltage_update_rate: float = 10000): 563 | alpha = min(max(1. - math.exp(-2. * math.pi * (bandwidth_hz / bus_voltage_update_rate)), 0.), 1.) 564 | print(f"Calculated bus voltage filter alpha: {alpha:.4f}") 565 | self.write_bus_voltage_filter_alpha(device_id, alpha) 566 | 567 | def set_encoder_velocity_bandwidth(self, device_id: int, bandwidth_hz: float, encoder_update_rate: float = 10000): 568 | alpha = min(max(1. - math.exp(-2. * math.pi * (bandwidth_hz / encoder_update_rate)), 0.), 1.) 569 | print(f"Calculated encoder velocity filter alpha: {alpha:.4f}") 570 | self.write_encoder_velocity_filter_alpha(device_id, alpha) 571 | 572 | def write_read_pdo_2(self, device_id: int, position_target: float, velocity_target: float) -> tuple: 573 | self.transmit_pdo_2(device_id, position_target, velocity_target) 574 | return self.receive_pdo_2(device_id) 575 | 576 | def transmit_pdo_2(self, device_id: int, position_target: float, velocity_target: float): 577 | self.transmit(CANFrame( 578 | device_id, 579 | Function.RECEIVE_PDO_2, 580 | size=8, 581 | data=struct.pack(" tuple: 585 | rx_frame = self.receive(filter_device_id=device_id, timeout=0.001) 586 | 587 | if rx_frame: 588 | measured_position, measured_velocity = struct.unpack(" No response from device {device_id}, timeout") 592 | return None, None 593 | --------------------------------------------------------------------------------