├── .gitignore ├── .vscode └── settings.json ├── README.md ├── TestIMUTransformation.py ├── calibrate_servos.py ├── install_packages_robot.sh ├── install_packages_sim.sh ├── pupper.service ├── pupper_pybullet.xml ├── run_robot.py ├── sim_requirements.txt ├── simulate.py ├── simulate_pybullet.py ├── src ├── Controller.py ├── Gaits.py ├── HardwareInterface.py ├── IMU.py ├── Kinematics.py ├── PupperConfig.py ├── PupperXMLParser.py ├── RobotConfig.py ├── StanceController.py ├── SwingLegController.py ├── UserInput.py ├── Utilities.py ├── __init__.py ├── puppe_prismatic.xml ├── pupper.xml ├── pupper_out.xml ├── pupper_pybullet.xml └── pupper_pybullet_out.xml └── tests └── IMULatencyTest.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.DS_Store 2 | *.pyc 3 | *.swp 4 | src/__pycache__ -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.linting.pylintEnabled": false, 3 | "python.linting.flake8Enabled": true, 4 | "python.linting.enabled": true 5 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pupper Robot: Control & Simulation 2 | 3 | This repo has been replaced by: https://github.com/stanfordroboticsclub/stanfordquadruped 4 | 5 | ## Overview 6 | This repository contains Python code to run Pupper, a Raspberry Pi-based quadruped robot. In addition to the robot code, this repository also contains a wrapper to simulate the robot in MuJoCo or PyBullet using the same code that runs on the robot. 7 | 8 | Video of the robot following a QR code: https://www.youtube.com/watch?v=iyuJq_Pn7TM 9 | 10 | ## Installing and Running Code on the Raspberry Pi 11 | ### Materials 12 | - Raspberry Pi 4 13 | - SD Card (32GB recommended) 14 | - Raspberry Pi 4 power supply (USB-C, 5V, >=3A) 15 | - Ethernet cable 16 | 17 | ### Steps 18 | - Install Raspbian Buster Lite onto the Pi 19 | - Download https://www.raspberrypi.org/downloads/raspbian/ 20 | - Use BalenaEtcher to flash the OS to the SD card 21 | - Set up the Raspberry Pi 22 | - Before even ejecting the SD Card, follow the instructions on this repo to put the self-installing setup script on the Pi: https://github.com/stanfordroboticsclub/RPI-Setup 23 | - Complete the “Actually Doing It”, “Getting Internet Access”, and “Getting Started With the Pi” sections 24 | - Test that the Pi works and connects to the internet 25 | - (Optional) Install the PREEMPT-RT kernel onto the Pi 26 | - Download the kernel patch https://github.com/lemariva/RT-Tools-RPi/tree/master/preempt-rt/kernel_4_19_59-rt23-v7l%2B 27 | - Follow these instructions starting from “Transfer the Kernel” https://lemariva.com/blog/2019/09/raspberry-pi-4b-preempt-rt-kernel-419y-performance-test 28 | - Test by running in the shell: 29 | ```shell 30 | uname -r 31 | ``` 32 | - We haven't yet characterized the benefits of using the preempt-rt kernel on the Pi so skipping this step might still give fine performance. 33 | - Install pigpio 34 | - Run 35 | ```shell 36 | sudo apt-get install python3-distutils 37 | ``` 38 | - Follow the instructions available at http://abyz.me.uk/rpi/pigpio/download.html to install a GPIO library for the Pi that adds software PWM capability. 39 | - Get the Pupper code 40 | - Clone the Pupper repository https://github.com/Nate711/PupperPythonSim/ 41 | - Install requirements: 42 | ```shell 43 | sudo bash install_packages_robot.sh 44 | ``` 45 | - Get the Pupper controller code 46 | - Clone the controller repo: https://github.com/stanfordroboticsclub/PupperCommand 47 | - Follow the instructions in the README 48 | ## Running the Robot 49 | - SSH into the robot 50 | ```shell 51 | ssh pi@10.0.0.xx 52 | ``` 53 | where xx is the local address you chose for the Pi 54 | - Once connected to the Pi, go into read-write mode 55 | ```shell 56 | rw 57 | ``` 58 | - Now go into this repo's PupperPythonSim directory and run the robot code! 59 | ```shell 60 | cd PupperPythonSim 61 | sudo pigpiod 62 | python3 run_robot.py 63 | ``` 64 | If you already have the pigpio daemon running, you can ignore when ```sudo pigpiod``` says it can't initialize pigpiod. 65 | - You can interrupt and stop the program by pressing Control-C. 66 | - To turn off the servo motors, run 67 | ```shell 68 | sudo pkill pigpiod 69 | ``` 70 | 71 | ## Installation for PyBullet Simulation 72 | The PyBullet simulator is free for academic use and requires no license whatsoever, but in my experience PyBullet is much slower than MuJoCo and is less clear about how to tune the contact parameters. 73 | 74 | 0. Clone this repository 75 | ```shell 76 | git clone https://github.com/Nate711/PupperSimulation.git 77 | ``` 78 | 1. Install pybullet (in a python 3.7 environment) 79 | ```shell 80 | pip install pybullet 81 | ``` 82 | ## Run PyBullet Simulation 83 | 1. Run simulation: 84 | ```shell 85 | python3 simulate_pybullet.py 86 | ``` 87 | 88 | ## Installation for MuJoCo Simulation 89 | MuJoCo has been faster than PyBullet in my experience, but requires that you request and activate a license. The process to acquire a license can take several days. 90 | 91 | 0. Clone this repository 92 | ```shell 93 | git clone https://github.com/Nate711/PupperSimulation.git 94 | ``` 95 | 1. Acquire a license for MuJoCo at http://mujoco.org/. You can get a free trial of the professional license for a month, or with a student account, a free year. 96 | 97 | 2. Follow the instructions at https://github.com/openai/mujoco-py to correctly install MuJoCo. 98 | 99 | If you have trouble installing MuJoCo on macOS because gcc can't find certain header files, like "limits.h" or "stdio.h", then try completing the installation wizard that pops up when you run: 100 | ```shell 101 | sudo open /Library/Developer/CommandLineTools/Packages/macOS_SDK_headers_for_macOS_10.14.pkg 102 | ``` 103 | And also do: 104 | ```shell 105 | brew update 106 | brew install gcc@8 107 | brew link --overwrite gcc 108 | ``` 109 | 110 | 111 | 3. Install the python requirements: 112 | ```bash 113 | sudo bash install_packages_sim.sh 114 | ``` 115 | 116 | ## Run MuJoCo Simulation 117 | (Sorry, the MuJoCo sim has not been updated in a while so does not currently work. You're welcome to update it to work with our other code though!) 118 | 1. Run 119 | ```shell 120 | python3 simulate.py 121 | ``` 122 | 2. The MuJoCo simulator should then pop up in a new window with various interactive options. Press space to stop or start the simulation. 123 | 124 | -------------------------------------------------------------------------------- /TestIMUTransformation.py: -------------------------------------------------------------------------------- 1 | from transforms3d.euler import euler2mat, quat2euler 2 | from transforms3d.quaternions import qconjugate, quat2axangle 3 | from transforms3d.axangles import axangle2mat 4 | from src.IMU import read_orientation, create_imu_handle 5 | from src.PupperConfig import IMUParams 6 | 7 | imu_params = IMUParams("/dev/cu.usbmodem63711001") 8 | imu_handle = create_imu_handle(imu_params) 9 | imu_handle.reset_input_buffer() 10 | 11 | while True: 12 | quat_orientation = read_orientation(imu_handle) 13 | if quat_orientation is not None: 14 | # q_inv = qconjugate(quat_orientation) 15 | # (yaw, pitch, roll) = quat2euler(q_inv) 16 | (yaw, pitch, roll) = quat2euler(quat_orientation) 17 | print(round(roll,3), round(pitch,3), round(yaw,3)) -------------------------------------------------------------------------------- /calibrate_servos.py: -------------------------------------------------------------------------------- 1 | import pigpio 2 | from src.HardwareInterface import initialize_pwm, send_servo_command 3 | from src.PupperConfig import PWMParams, ServoParams 4 | import numpy as np 5 | 6 | 7 | def get_motor_name(i, j): 8 | motor_type = {0: "Abduction", 1: "Inner", 2: "Outer"} # Top # Bottom 9 | leg_pos = {0: "Front Right", 1: "Front Left", 2: "Back Right", 3: "Back Left"} 10 | final_name = motor_type[i] + " " + leg_pos[j] 11 | return final_name 12 | 13 | 14 | def get_motor_setpoint(i, j): 15 | data = [[0, 0, 0, 0], [45, 45, 45, 45], [45, 45, 45, 45]] 16 | return data[i][j] 17 | 18 | 19 | def degrees_to_radians(input_array): 20 | """Converts degrees to radians. 21 | 22 | Parameters 23 | ---------- 24 | input_array : Numpy array or float 25 | Degrees 26 | 27 | Returns 28 | ------- 29 | Numpy array or float 30 | Radians 31 | """ 32 | return np.pi / 180.0 * input_array 33 | 34 | 35 | def step_until(servo_params, pi_board, pwm_params, kValue, i_index, j_index, set_point): 36 | """Returns the angle offset needed to correct a given link by asking the user for input. 37 | 38 | Returns 39 | ------- 40 | Float 41 | Angle offset needed to correct the link. 42 | """ 43 | found_position = False 44 | set_names = ["horizontal", "horizontal", "vertical"] 45 | offset = 0 46 | while not found_position: 47 | above_or_below = str( 48 | input( 49 | "Desired position: " 50 | + set_names[i_index] 51 | + ". Enter 'a' or 'b' to raise or lower the link. Enter 'd' when done. Input: " 52 | ) 53 | ) 54 | if above_or_below == "above" or above_or_below == "a": 55 | offset += 1.0 56 | send_servo_command( 57 | pi_board, 58 | pwm_params, 59 | servo_params, 60 | degrees_to_radians(set_point + offset), 61 | i_index, 62 | j_index, 63 | ) 64 | elif above_or_below == "below" or above_or_below == "b": 65 | offset -= 1.0 66 | send_servo_command( 67 | pi_board, 68 | pwm_params, 69 | servo_params, 70 | degrees_to_radians(set_point + offset), 71 | i_index, 72 | j_index, 73 | ) 74 | elif above_or_below == "done" or above_or_below == "d": 75 | found_position = True 76 | print("offset: ", offset, " original: ", set_point) 77 | 78 | return offset 79 | 80 | 81 | def calibrate_b(servo_params, pi_board, pwm_params): 82 | """Calibrate the angle offset for the twelve motors on the robot. Note that servo_params is modified in-place. 83 | Parameters 84 | ---------- 85 | servo_params : ServoParams 86 | Servo parameters. This variable is updated in-place. 87 | pi_board : Pi 88 | RaspberryPi object. 89 | pwm_params : PWMParams 90 | PWMParams object. 91 | """ 92 | 93 | # Found K value of (11.4) 94 | kValue = float( 95 | input("Please provide a K value (microseconds per degree) for your servos: ") 96 | ) 97 | servo_params.micros_per_rad = kValue * 180 / np.pi 98 | 99 | servo_params.neutral_angle_degrees = np.zeros((3, 4)) 100 | 101 | for j in range(4): 102 | for i in range(3): 103 | # Loop until we're satisfied with the calibration 104 | completed = False 105 | while not completed: 106 | motor_name = get_motor_name(i, j) 107 | print("Currently calibrating " + motor_name + "...") 108 | set_point = get_motor_setpoint(i, j) 109 | 110 | # Move servo to set_point angle 111 | send_servo_command( 112 | pi_board, 113 | pwm_params, 114 | servo_params, 115 | degrees_to_radians(set_point), 116 | i, 117 | j, 118 | ) 119 | 120 | # Adjust the angle using keyboard input until it matches the reference angle 121 | offset = step_until( 122 | servo_params, pi_board, pwm_params, kValue, i, j, set_point 123 | ) 124 | print("Final offset: ", offset) 125 | 126 | # The upper leg link has a different equation because we're calibrating to make it horizontal, not vertical 127 | if i == 1: 128 | servo_params.neutral_angle_degrees[i, j] = set_point - offset 129 | else: 130 | servo_params.neutral_angle_degrees[i, j] = -(set_point + offset) 131 | print("New beta angle: ", servo_params.neutral_angle_degrees[i, j]) 132 | 133 | # Send the servo command using the new beta value and check that it's ok 134 | send_servo_command( 135 | pi_board, 136 | pwm_params, 137 | servo_params, 138 | degrees_to_radians([0, 45, -45][i]), 139 | i, 140 | j, 141 | ) 142 | okay = "" 143 | while okay not in ["yes", "no"]: 144 | okay = str( 145 | input("Check angle. Are you satisfied? Enter 'yes' or 'no': ") 146 | ) 147 | completed = okay == "yes" 148 | 149 | 150 | def main(): 151 | """Main program 152 | """ 153 | pi_board = pigpio.pi() 154 | pwm_params = PWMParams() 155 | servo_params = ServoParams() 156 | initialize_pwm(pi_board, pwm_params) 157 | 158 | calibrate_b(servo_params, pi_board, pwm_params) 159 | print("Calibrated neutral angles:") 160 | print(servo_params.neutral_angle_degrees) 161 | 162 | 163 | main() 164 | -------------------------------------------------------------------------------- /install_packages_robot.sh: -------------------------------------------------------------------------------- 1 | yes | sudo apt-get install libatlas-base-dev 2 | yes | pip3 install numpy scipy transforms3d pigpio pyserial 3 | -------------------------------------------------------------------------------- /install_packages_sim.sh: -------------------------------------------------------------------------------- 1 | echo '**** Ensure you have installed MuJoCo before running this script! Instructions for installing can be found here https://github.com/openai/mujoco-py ****' 2 | pip install -r sim_requirements.txt -------------------------------------------------------------------------------- /pupper.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Pupper control service 3 | Requires=joystick.service 4 | After=joystick.service 5 | 6 | [Service] 7 | ExecStartPre=sudo pigpiod 8 | ExecStart=/usr/bin/python3 /home/pi/PupperPythonSim/run_robot.py 9 | KillSignal=2 10 | TimeoutStopSec=10 11 | 12 | [Install] 13 | WantedBy=multi-user.target 14 | -------------------------------------------------------------------------------- /pupper_pybullet.xml: -------------------------------------------------------------------------------- 1 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 145 | -------------------------------------------------------------------------------- /run_robot.py: -------------------------------------------------------------------------------- 1 | import pigpio 2 | import numpy as np 3 | import UDPComms 4 | import time 5 | import subprocess 6 | from src.IMU import IMU 7 | from src.Controller import step_controller, Controller 8 | from src.HardwareInterface import send_servo_commands, initialize_pwm, deactivate_servos 9 | from src.PupperConfig import ( 10 | PupperConfig, 11 | MovementReference, 12 | GaitParams, 13 | StanceParams, 14 | SwingParams, 15 | ServoParams, 16 | PWMParams, 17 | UserInputParams, 18 | ) 19 | from src.UserInput import UserInputs, get_input, update_controller 20 | 21 | 22 | def start_pigpiod(): 23 | print("Starting pigpiod...") 24 | subprocess.Popen(["sudo", "pigpiod"]) 25 | time.sleep(2) 26 | print("Done.") 27 | 28 | 29 | def stop_pigpiod(): 30 | print("Killing pigpiod...") 31 | subprocess.Popen(["sudo", "pkill", "pigpiod"]) 32 | time.sleep(2) 33 | print("Done.") 34 | 35 | 36 | def main(): 37 | """Main program 38 | """ 39 | 40 | # Start pwm to servos 41 | # start_pigpiod() 42 | pi_board = pigpio.pi() 43 | pwm_params = PWMParams() 44 | initialize_pwm(pi_board, pwm_params) 45 | 46 | # Create config 47 | robot_config = PupperConfig() 48 | servo_params = ServoParams() 49 | 50 | # Create imu handle 51 | imu = IMU(port="/dev/ttyACM0") 52 | imu.flush_buffer() 53 | 54 | # Create controller and user input handles 55 | controller = Controller(robot_config) 56 | input_params = UserInputParams() 57 | user_input = UserInputs(max_x_velocity=input_params.max_x_velocity, max_y_velocity=input_params.max_y_velocity, max_yaw_rate=input_params.max_yaw_rate, max_pitch=input_params.max_pitch) 58 | 59 | last_loop = time.time() 60 | 61 | print("Summary of gait parameters:") 62 | print("overlap time: ", controller.gait_params.overlap_time) 63 | print("swing time: ", controller.gait_params.swing_time) 64 | print("z clearance: ", controller.swing_params.z_clearance) 65 | print("x shift: ", controller.stance_params.x_shift) 66 | 67 | # Wait until the activate button has been pressed 68 | while True: 69 | print("Waiting for L1 to activate robot.") 70 | while True: 71 | get_input(user_input) 72 | if user_input.activate == 1 and user_input.last_activate == 0: 73 | user_input.last_activate = 1 74 | break 75 | user_input.last_activate = user_input.activate 76 | print("Robot activated.") 77 | 78 | while True: 79 | now = time.time() 80 | if now - last_loop < controller.gait_params.dt: 81 | continue 82 | last_loop = time.time() 83 | 84 | # Parse the udp joystick commands and then update the robot controller's parameters 85 | get_input(user_input) 86 | if user_input.activate == 1 and user_input.last_activate == 0: 87 | user_input.last_activate = 1 88 | break 89 | else: 90 | user_input.last_activate = user_input.activate 91 | 92 | update_controller(controller, user_input) 93 | 94 | # Read imu data. Orientation will be None if no data was available 95 | quat_orientation = imu.read_orientation() 96 | 97 | # Step the controller forward by dt 98 | step_controller(controller, robot_config, quat_orientation) 99 | 100 | # Update the pwm widths going to the servos 101 | send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) 102 | deactivate_servos(pi_board, pwm_params) 103 | main() 104 | 105 | -------------------------------------------------------------------------------- /sim_requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | transforms3d 3 | matplotlib 4 | mujoco-py 5 | -------------------------------------------------------------------------------- /simulate.py: -------------------------------------------------------------------------------- 1 | from src import PupperXMLParser 2 | from src.Controller import Controller, step_controller 3 | from src.PupperConfig import ( 4 | PupperConfig, 5 | EnvironmentConfig, 6 | SolverConfig, 7 | SwingParams, 8 | ServoParams, 9 | PWMParams, 10 | ) 11 | import time 12 | import numpy as np 13 | from mujoco_py import load_model_from_path, MjSim, MjViewer, functions 14 | from src.HardwareInterface import angle_to_pwm 15 | 16 | 17 | def parallel_to_serial_joint_angles(joint_matrix): 18 | """Convert from joint angles meant for the parallel linkage in 19 | Pupper to the joint angles in the serial linkage approximation implemented in the simulation 20 | 21 | Parameters 22 | ---------- 23 | joint_matrix : Numpy array (3, 4) 24 | Joint angles for parallel linkage 25 | 26 | Returns 27 | ------- 28 | Numpy array (3, 4) 29 | Joint angles for equivalent serial linkage 30 | """ 31 | temp = joint_matrix 32 | temp[2, :] -= joint_matrix[1, :] 33 | return temp 34 | 35 | 36 | # Create environment objects 37 | PUPPER_CONFIG = PupperConfig() 38 | ENVIRONMENT_CONFIG = EnvironmentConfig() 39 | SOLVER_CONFIG = SolverConfig() 40 | 41 | # Initailize MuJoCo 42 | PupperXMLParser.Parse(PUPPER_CONFIG, ENVIRONMENT_CONFIG, SOLVER_CONFIG) 43 | model = load_model_from_path("src/pupper_out.xml") 44 | sim = MjSim(model) 45 | viewer = MjViewer(sim) 46 | 47 | # Create pupper_controller 48 | pupper_controller = Controller() 49 | pupper_controller.movement_reference.v_xy_ref = np.array([0.2, 0.0]) 50 | pupper_controller.movement_reference.wz_ref = 0.0 51 | pupper_controller.swing_params.z_clearance = 0.03 52 | pupper_controller.gait_params.dt = 0.005 53 | pupper_controller.stance_params.delta_y = 0.12 54 | 55 | # Initialize joint angles 56 | sim.data.qpos[7:] = parallel_to_serial_joint_angles( 57 | pupper_controller.joint_angles 58 | ).T.reshape(12) 59 | # Set the robot to be above the floor to begin with 60 | sim.data.qpos[2] = 0.5 61 | 62 | # Initialize pwm and servo params 63 | pwm_params = PWMParams() 64 | servo_params = ServoParams() 65 | 66 | # Run the simulation 67 | timesteps = 60000 68 | 69 | # Calculate how many simulation steps to make for every controller update 70 | pupper_update_rate = int( 71 | 1.0 / pupper_controller.gait_params.dt 72 | ) # control rate, updates per second 73 | sim_rate = int(1 / ENVIRONMENT_CONFIG.DT) # simulation updates per second 74 | sim_steps_per_control_step = int(sim_rate / pupper_update_rate) 75 | 76 | for i in range(timesteps): 77 | # sim.data.qpos[2] = 0.5 78 | 79 | # Step the pupper controller forward 80 | if i % sim_steps_per_control_step == 0: 81 | # step_controller takes between 0.3ms and 1ms to complete! Definitely fast enough! 82 | step_controller(pupper_controller) 83 | 84 | # calculate theoretical pwm values 85 | pwm_commands = np.zeros((3, 4)) 86 | for axis in range(3): 87 | for leg in range(4): 88 | pwm_commands[axis, leg] = angle_to_pwm( 89 | pupper_controller.joint_angles[axis, leg], servo_params, axis, leg 90 | ) 91 | print("Joint Angles: ") 92 | print(pupper_controller.joint_angles) 93 | print("Servo Pulse Width (uS): ") 94 | print(pwm_commands) 95 | print("") 96 | 97 | # Convert from joint angles meant for a parallel linkage to the serial linkage implemented in Mujoco 98 | sim.data.ctrl[:] = parallel_to_serial_joint_angles( 99 | pupper_controller.joint_angles 100 | ).T.reshape(12) 101 | 102 | sim.step() 103 | viewer.render() 104 | -------------------------------------------------------------------------------- /simulate_pybullet.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import pybullet_data 3 | import time 4 | import numpy as np 5 | 6 | from src import PupperXMLParser 7 | from src.Controller import step_controller, Controller 8 | from src.HardwareInterface import send_servo_commands, initialize_pwm 9 | from src.PupperConfig import ( 10 | PupperConfig, 11 | MovementReference, 12 | GaitParams, 13 | StanceParams, 14 | SwingParams, 15 | ServoParams, 16 | PWMParams, 17 | EnvironmentConfig, 18 | SolverConfig 19 | ) 20 | from src.UserInput import UserInputs, get_input, update_controller 21 | 22 | 23 | def parallel_to_serial_joint_angles(joint_matrix): 24 | """Convert from joint angles meant for the parallel linkage in 25 | Pupper to the joint angles in the serial linkage approximation implemented in the simulation 26 | 27 | Parameters 28 | ---------- 29 | joint_matrix : Numpy array (3, 4) 30 | Joint angles for parallel linkage 31 | 32 | Returns 33 | ------- 34 | Numpy array (3, 4) 35 | Joint angles for equivalent serial linkage 36 | """ 37 | temp = joint_matrix 38 | temp[2, :] -= joint_matrix[1, :] 39 | return temp 40 | 41 | 42 | # Create environment objects 43 | PUPPER_CONFIG = PupperConfig() 44 | PUPPER_CONFIG.XML_IN = "pupper_pybullet.xml" 45 | PUPPER_CONFIG.XML_OUT = "pupper_pybullet_out.xml" 46 | 47 | ENVIRONMENT_CONFIG = EnvironmentConfig() 48 | SOLVER_CONFIG = SolverConfig() 49 | 50 | # Initailize MuJoCo 51 | PupperXMLParser.Parse(PUPPER_CONFIG, ENVIRONMENT_CONFIG, SOLVER_CONFIG) 52 | 53 | # Set up PyBullet Simulator 54 | physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version 55 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally 56 | p.setGravity(0, 0, -9.81) 57 | pupperId = p.loadMJCF("src/pupper_pybullet_out.xml") 58 | 59 | print("") 60 | print("Pupper bodies IDs:", pupperId) 61 | numjoints = p.getNumJoints(pupperId[1]) 62 | print("Number of joints in converted MJCF: ", numjoints) 63 | print("Joint Info: ") 64 | for i in range(numjoints): 65 | print(p.getJointInfo(pupperId[1], i)) 66 | 67 | joint_indices = list(range(0, 24, 2)) 68 | 69 | # Create controller 70 | robot_config = PupperConfig() 71 | servo_params = ServoParams() 72 | controller = Controller(robot_config) 73 | user_input = UserInputs() 74 | 75 | # Run the simulation 76 | timesteps = 240*60*10 # simulate for a max of 10 minutes 77 | 78 | # Sim seconds per sim step 79 | sim_steps_per_sim_second = 240 80 | sim_seconds_per_sim_step = 1.0 / sim_steps_per_sim_second 81 | 82 | start = time.time() 83 | last_control_update = 0 84 | 85 | controller.gait_params.contact_phases = np.array( 86 | [[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]] 87 | ) 88 | controller.swing_params.z_clearance = 0.03 89 | controller.movement_reference.v_xy_ref = np.array([0.10, 0.0]) 90 | # controller.movement_reference.wz_ref = 0.5 91 | 92 | # To account for the fact that the CoM of the robot is a little behind the geometric center, 93 | # put the robot feet a little behind the geometric center to try to match the actual CoM 94 | # controller.stance_params.x_shift = -0.01 95 | 96 | (hey, now) = (0, 0) 97 | 98 | for steps in range(timesteps): 99 | current_time = time.time() 100 | 101 | # Simulated time can be computed as sim_seconds_per_sim_step * steps 102 | simluated_time_elapsed = sim_seconds_per_sim_step * steps 103 | 104 | if simluated_time_elapsed - last_control_update > controller.gait_params.dt: 105 | # This block usually takes < 1ms to run, but every 10 or so iterations it takes as many as 50ms to run 106 | 107 | hey = time.time() 108 | last_control_update = simluated_time_elapsed 109 | 110 | (pos, q_scalar_last) = p.getBasePositionAndOrientation(1) 111 | q_corrected = (q_scalar_last[3], q_scalar_last[0], q_scalar_last[1], q_scalar_last[2]) 112 | # print("Orientation: ", q_corrected) 113 | 114 | # Calculate the next joint angle commands 115 | step_controller(controller, robot_config, q_corrected) 116 | 117 | # Convert the joint angles from the parallel linkage to the simulated serial linkage 118 | serial_joint_angles = parallel_to_serial_joint_angles( 119 | controller.joint_angles 120 | ) 121 | 122 | # Send the joint angles to the sim 123 | p.setJointMotorControlArray( 124 | bodyUniqueId=pupperId[1], 125 | jointIndices=joint_indices, 126 | controlMode=p.POSITION_CONTROL, 127 | targetPositions=list(serial_joint_angles.T.reshape(12)), 128 | positionGains=[0.25]*12, 129 | velocityGains=[0.5]*12, 130 | forces=[10] * 12, 131 | ) 132 | 133 | now = time.time() 134 | 135 | # Simulate physics for 1/240 seconds (1/240 is the default timestep) 136 | p.stepSimulation() 137 | 138 | # time.sleep(0.01) 139 | 140 | # Performance testing 141 | elapsed = time.time() - start 142 | if (steps % 60) == 0: 143 | print( 144 | "Sim seconds elapsed: {}, Real seconds elapsed: {}".format( 145 | round(simluated_time_elapsed,3), round(elapsed,3) 146 | ) 147 | ) 148 | # print("Average steps per second: {0}, elapsed: {1}, i:{2}".format(steps / elapsed, elapsed, i)) 149 | -------------------------------------------------------------------------------- /src/Controller.py: -------------------------------------------------------------------------------- 1 | from src.PupperConfig import SwingParams, StanceParams, GaitParams, MovementReference 2 | from src.Gaits import contacts, subphase_time 3 | from src.Kinematics import four_legs_inverse_kinematics 4 | from src.StanceController import stance_foot_location 5 | from src.SwingLegController import swing_foot_location 6 | from src.Utilities import clipped_first_order_filter 7 | from src.PupperConfig import BehaviorState 8 | 9 | import numpy as np 10 | from transforms3d.euler import euler2mat, quat2euler 11 | from transforms3d.quaternions import qconjugate, quat2axangle 12 | from transforms3d.axangles import axangle2mat 13 | 14 | class Controller: 15 | """Controller and planner object 16 | """ 17 | 18 | def __init__(self, robot_config): 19 | self.swing_params = SwingParams() 20 | self.stance_params = StanceParams() 21 | self.gait_params = GaitParams() 22 | self.movement_reference = MovementReference() 23 | self.smoothed_yaw = 0.0 # for REST mode only 24 | 25 | self.previous_state = BehaviorState.REST 26 | self.state = BehaviorState.REST 27 | 28 | self.ticks = 0 29 | 30 | # Set default for foot locations and joint angles 31 | self.foot_locations = ( 32 | self.stance_params.default_stance 33 | + np.array([0, 0, self.movement_reference.z_ref])[:, np.newaxis] 34 | ) 35 | self.contact_modes = np.zeros(4) 36 | self.joint_angles = four_legs_inverse_kinematics( 37 | self.foot_locations, robot_config 38 | ) 39 | 40 | 41 | def step( 42 | ticks, foot_locations, swing_params, stance_params, gait_params, movement_reference 43 | ): 44 | """Calculate the desired foot locations for the next timestep 45 | 46 | Parameters 47 | ---------- 48 | ticks : int 49 | Number of clock ticks since the start. Time between ticks is given by the gait params dt variable. 50 | foot_locations : Numpy array (3, 4) 51 | Locations of all four feet. 52 | swing_params : SwingParams 53 | Swing parameters object. 54 | stance_params : StanceParams 55 | Stance parameters object. 56 | gait_params : GaitParams 57 | Gait parameters object. 58 | movement_reference : MovementReference 59 | Movement reference object. 60 | 61 | Returns 62 | ------- 63 | Numpy array (3, 4) 64 | Matrix of new foot locations. 65 | """ 66 | contact_modes = contacts(ticks, gait_params) 67 | new_foot_locations = np.zeros((3, 4)) 68 | for leg_index in range(4): 69 | contact_mode = contact_modes[leg_index] 70 | foot_location = foot_locations[:, leg_index] 71 | if contact_mode == 1: 72 | new_location = stance_foot_location( 73 | foot_location, stance_params, gait_params, movement_reference 74 | ) 75 | else: 76 | swing_proportion = ( 77 | subphase_time(ticks, gait_params) / gait_params.swing_ticks 78 | ) 79 | new_location = swing_foot_location( 80 | swing_proportion, 81 | foot_location, 82 | leg_index, 83 | swing_params, 84 | stance_params, 85 | gait_params, 86 | movement_reference, 87 | ) 88 | new_foot_locations[:, leg_index] = new_location 89 | return new_foot_locations, contact_modes 90 | 91 | 92 | def step_controller(controller, robot_config, quat_orientation): 93 | """Steps the controller forward one timestep 94 | 95 | Parameters 96 | ---------- 97 | controller : Controller 98 | Robot controller object. 99 | """ 100 | if controller.state == BehaviorState.TROT: 101 | controller.foot_locations, controller.contact_modes = step( 102 | controller.ticks, 103 | controller.foot_locations, 104 | controller.swing_params, 105 | controller.stance_params, 106 | controller.gait_params, 107 | controller.movement_reference, 108 | ) 109 | 110 | # Apply the desired body rotation 111 | # foot_locations = ( 112 | # euler2mat( 113 | # controller.movement_reference.roll, controller.movement_reference.pitch, 0.0 114 | # ) 115 | # @ controller.foot_locations 116 | # ) 117 | # Disable joystick-based pitch and roll for trotting with IMU feedback 118 | foot_locations = controller.foot_locations 119 | 120 | # Construct foot rotation matrix to compensate for body tilt 121 | (roll, pitch, yaw) = quat2euler(quat_orientation) 122 | correction_factor = 0.8 123 | max_tilt = 0.4 124 | roll_compensation = correction_factor * np.clip(roll, -max_tilt, max_tilt) 125 | pitch_compensation = correction_factor * np.clip(pitch, -max_tilt, max_tilt) 126 | rmat = euler2mat(roll_compensation, pitch_compensation, 0) 127 | 128 | foot_locations = rmat.T @ foot_locations 129 | 130 | controller.joint_angles = four_legs_inverse_kinematics( 131 | foot_locations, robot_config 132 | ) 133 | 134 | elif controller.state == BehaviorState.HOP: 135 | hop_foot_locations = ( 136 | controller.stance_params.default_stance 137 | + np.array([0, 0, -0.09])[:, np.newaxis] 138 | ) 139 | controller.joint_angles = four_legs_inverse_kinematics( 140 | hop_foot_locations, robot_config 141 | ) 142 | 143 | elif controller.state == BehaviorState.FINISHHOP: 144 | hop_foot_locations = ( 145 | controller.stance_params.default_stance 146 | + np.array([0, 0, -.22])[:, np.newaxis] 147 | ) 148 | controller.joint_angles = four_legs_inverse_kinematics( 149 | hop_foot_locations, robot_config 150 | ) 151 | 152 | 153 | elif controller.state == BehaviorState.REST: 154 | if controller.previous_state != BehaviorState.REST: 155 | controller.smoothed_yaw = 0 156 | 157 | yaw_factor = -0.25 158 | controller.smoothed_yaw += controller.gait_params.dt * clipped_first_order_filter(controller.smoothed_yaw, controller.movement_reference.wz_ref * yaw_factor, 1.5, 0.25) 159 | # Set the foot locations to the default stance plus the standard height 160 | controller.foot_locations = ( 161 | controller.stance_params.default_stance 162 | + np.array([0, 0, controller.movement_reference.z_ref])[:, np.newaxis] 163 | ) 164 | # Apply the desired body rotation 165 | rotated_foot_locations = ( 166 | euler2mat( 167 | controller.movement_reference.roll, controller.movement_reference.pitch, controller.smoothed_yaw 168 | ) 169 | @ controller.foot_locations 170 | ) 171 | controller.joint_angles = four_legs_inverse_kinematics( 172 | rotated_foot_locations, robot_config 173 | ) 174 | 175 | controller.ticks += 1 176 | controller.previous_state = controller.state 177 | 178 | 179 | def set_pose_to_default(controller, robot_config): 180 | controller.foot_locations = ( 181 | controller.stance_params.default_stance 182 | + np.array([0, 0, controller.movement_reference.z_ref])[:, np.newaxis] 183 | ) 184 | controller.joint_angles = four_legs_inverse_kinematics( 185 | controller.foot_locations, robot_config 186 | ) 187 | -------------------------------------------------------------------------------- /src/Gaits.py: -------------------------------------------------------------------------------- 1 | def phase_index(ticks, gaitparams): 2 | """Calculates which part of the gait cycle the robot should be in given the time in ticks. 3 | 4 | Parameters 5 | ---------- 6 | ticks : int 7 | Number of timesteps since the program started 8 | gaitparams : GaitParams 9 | GaitParams object 10 | 11 | Returns 12 | ------- 13 | Int 14 | The index of the gait phase that the robot should be in. 15 | """ 16 | phase_time = ticks % gaitparams.phase_length 17 | phase_sum = 0 18 | for i in range(gaitparams.num_phases): 19 | phase_sum += gaitparams.phase_times[i] 20 | if phase_time < phase_sum: 21 | return i 22 | assert False 23 | 24 | 25 | def subphase_time(ticks, gaitparams): 26 | """Calculates the number of ticks (timesteps) since the start of the current phase. 27 | 28 | Parameters 29 | ---------- 30 | ticks : Int 31 | Number of timesteps since the program started 32 | gaitparams : GaitParams 33 | GaitParams object 34 | 35 | Returns 36 | ------- 37 | Int 38 | Number of ticks since the start of the current phase. 39 | """ 40 | phase_time = ticks % gaitparams.phase_length 41 | phase_sum = 0 42 | subphase_t = 0 43 | for i in range(gaitparams.num_phases): 44 | phase_sum += gaitparams.phase_times[i] 45 | if phase_time < phase_sum: 46 | subphase_t = phase_time - phase_sum + gaitparams.phase_times[i] 47 | return subphase_t 48 | assert False 49 | 50 | 51 | def contacts(ticks, gaitparams): 52 | """Calculates which feet should be in contact at the given number of ticks 53 | 54 | Parameters 55 | ---------- 56 | ticks : Int 57 | Number of timesteps since the program started. 58 | gaitparams : GaitParams 59 | GaitParams object 60 | 61 | Returns 62 | ------- 63 | numpy array (4,) 64 | Numpy vector with 0 indicating flight and 1 indicating stance. 65 | """ 66 | return gaitparams.contact_phases[:, phase_index(ticks, gaitparams)] 67 | -------------------------------------------------------------------------------- /src/HardwareInterface.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def pwm_to_duty_cycle(pulsewidth_micros, pwm_params): 5 | """Converts a pwm signal (measured in microseconds) to a corresponding duty cycle on the gpio pwm pin 6 | 7 | Parameters 8 | ---------- 9 | pulsewidth_micros : float 10 | Width of the pwm signal in microseconds 11 | pwm_params : PWMParams 12 | PWMParams object 13 | 14 | Returns 15 | ------- 16 | float 17 | PWM duty cycle corresponding to the pulse width 18 | """ 19 | return int(pulsewidth_micros / 1e6 * pwm_params.freq * pwm_params.range) 20 | 21 | 22 | def angle_to_pwm(angle, servo_params, axis_index, leg_index): 23 | """Converts a desired servo angle into the corresponding PWM command 24 | 25 | Parameters 26 | ---------- 27 | angle : float 28 | Desired servo angle, relative to the vertical (z) axis 29 | servo_params : ServoParams 30 | ServoParams object 31 | axis_index : int 32 | Specifies which joint of leg to control. 0 is abduction servo, 1 is inner hip servo, 2 is outer hip servo. 33 | leg_index : int 34 | Specifies which leg to control. 0 is front-right, 1 is front-left, 2 is back-right, 3 is back-left. 35 | 36 | Returns 37 | ------- 38 | float 39 | PWM width in microseconds 40 | """ 41 | angle_deviation = ( 42 | angle - servo_params.neutral_angles[axis_index, leg_index] 43 | ) * servo_params.servo_multipliers[axis_index, leg_index] 44 | pulse_width_micros = ( 45 | servo_params.neutral_position_pwm 46 | + servo_params.micros_per_rad * angle_deviation 47 | ) 48 | return pulse_width_micros 49 | 50 | 51 | def angle_to_duty_cycle(angle, pwm_params, servo_params, axis_index, leg_index): 52 | return pwm_to_duty_cycle( 53 | angle_to_pwm(angle, servo_params, axis_index, leg_index), pwm_params 54 | ) 55 | 56 | 57 | def initialize_pwm(pi, pwm_params): 58 | for leg_index in range(4): 59 | for axis_index in range(3): 60 | pi.set_PWM_frequency( 61 | pwm_params.pins[axis_index, leg_index], pwm_params.freq 62 | ) 63 | pi.set_PWM_range(pwm_params.pins[axis_index, leg_index], pwm_params.range) 64 | 65 | 66 | def send_servo_commands(pi, pwm_params, servo_params, joint_angles): 67 | for leg_index in range(4): 68 | for axis_index in range(3): 69 | duty_cycle = angle_to_duty_cycle( 70 | joint_angles[axis_index, leg_index], 71 | pwm_params, 72 | servo_params, 73 | axis_index, 74 | leg_index, 75 | ) 76 | pi.set_PWM_dutycycle(pwm_params.pins[axis_index, leg_index], duty_cycle) 77 | 78 | 79 | def send_servo_command(pi, pwm_params, servo_params, joint_angle, axis, leg): 80 | duty_cycle = angle_to_duty_cycle(joint_angle, pwm_params, servo_params, axis, leg) 81 | pi.set_PWM_dutycycle(pwm_params.pins[axis, leg], duty_cycle) 82 | 83 | 84 | def deactivate_servos(pi, pwm_params): 85 | for leg_index in range(4): 86 | for axis_index in range(3): 87 | pi.set_PWM_dutycycle(pwm_params.pins[axis_index, leg_index], 0) 88 | -------------------------------------------------------------------------------- /src/IMU.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import numpy as np 3 | import time 4 | 5 | 6 | class IMU: 7 | def __init__(self, port, baudrate=500000): 8 | self.serial_handle = serial.Serial( 9 | port=port, 10 | baudrate=baudrate, 11 | parity=serial.PARITY_NONE, 12 | stopbits=serial.STOPBITS_ONE, 13 | bytesize=serial.EIGHTBITS, 14 | timeout=0, 15 | ) 16 | self.last_quat = np.array([1, 0, 0, 0]) 17 | self.start_time = time.time() 18 | 19 | def flush_buffer(self): 20 | self.serial_handle.reset_input_buffer() 21 | 22 | def read_orientation(self): 23 | """Reads quaternion measurements from the Teensy until none are left. Returns the last read quaternion. 24 | 25 | Parameters 26 | ---------- 27 | serial_handle : Serial object 28 | Handle to the pyserial Serial object 29 | 30 | Returns 31 | ------- 32 | np array (4,) 33 | If there was quaternion data to read on the serial port returns the quaternion as a numpy array, otherwise returns the last read quaternion. 34 | """ 35 | 36 | while True: 37 | x = self.serial_handle.readline().decode("utf").strip() 38 | if x is "" or x is None: 39 | return self.last_quat 40 | else: 41 | parsed = x.split(",") 42 | if len(parsed) == 4: 43 | self.last_quat = np.array(parsed, dtype=np.float64) 44 | else: 45 | print("Did not receive 4-vector from imu") 46 | -------------------------------------------------------------------------------- /src/Kinematics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from transforms3d.euler import euler2mat 3 | 4 | 5 | def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config): 6 | """Find the joint angles corresponding to the given body-relative foot position for a given leg and configuration 7 | 8 | Parameters 9 | ---------- 10 | r_body_foot : [type] 11 | [description] 12 | leg_index : [type] 13 | [description] 14 | config : [type] 15 | [description] 16 | 17 | Returns 18 | ------- 19 | numpy array (3) 20 | Array of corresponding joint angles. 21 | """ 22 | (x, y, z) = r_body_foot 23 | 24 | # Distance from the leg origin to the foot, projected into the y-z plane 25 | R_body_foot_yz = (y ** 2 + z ** 2) ** 0.5 26 | 27 | # Distance from the leg's forward/back point of rotation to the foot 28 | R_hip_foot_yz = (R_body_foot_yz ** 2 - config.ABDUCTION_OFFSET ** 2) ** 0.5 29 | 30 | # Ensure that the target point is reachable 31 | assert R_body_foot_yz >= abs(config.ABDUCTION_OFFSET) 32 | 33 | # Interior angle of the right triangle formed in the y-z plane by the leg that is coincident to the ab/adduction axis 34 | # For feet 2 (front left) and 4 (back left), the abduction offset is positive, for the right feet, the abduction offset is negative. 35 | arccos_argument = config.ABDUCTION_OFFSETS[leg_index] / R_body_foot_yz 36 | arccos_argument = np.clip(arccos_argument, -0.99, 0.99) 37 | phi = np.arccos(arccos_argument) 38 | 39 | # Angle of the y-z projection of the hip-to-foot vector, relative to the positive y-axis 40 | hip_foot_angle = np.arctan2(z, y) 41 | 42 | # Ab/adduction angle, relative to the positive y-axis 43 | abduction_angle = phi + hip_foot_angle 44 | 45 | # theta: Angle between the tilted negative z-axis and the hip-to-foot vector 46 | theta = np.arctan2(-x, R_hip_foot_yz) 47 | 48 | # Distance between the hip and foot 49 | R_hip_foot = (R_hip_foot_yz ** 2 + x ** 2) ** 0.5 50 | 51 | # Angle between the line going from hip to foot and the link L1 52 | arccos_argument = (config.LEG_L1 ** 2 + R_hip_foot ** 2 - config.LEG_L2 ** 2) / (2 * config.LEG_L1 * R_hip_foot) 53 | arccos_argument = np.clip(arccos_argument, -0.99, 0.99) 54 | trident = np.arccos(arccos_argument) 55 | 56 | # Angle of the first link relative to the tilted negative z axis 57 | hip_angle = theta + trident 58 | 59 | # Angle between the leg links L1 and L2 60 | arccos_argument = (config.LEG_L1 ** 2 + config.LEG_L2 ** 2 - R_hip_foot ** 2) / (2 * config.LEG_L1 * config.LEG_L2) 61 | arccos_argument = np.clip(arccos_argument, -0.99, 0.99) 62 | beta = np.arccos(arccos_argument) 63 | 64 | # Angle of the second link relative to the tilted negative z axis 65 | knee_angle = hip_angle - (np.pi - beta) 66 | 67 | return np.array([abduction_angle, hip_angle, knee_angle]) 68 | 69 | 70 | def four_legs_inverse_kinematics(r_body_foot, config): 71 | """Find the joint angles for all twelve DOF correspoinding to the given matrix of body-relative foot positions. 72 | 73 | Parameters 74 | ---------- 75 | r_body_foot : numpy array (3,4) 76 | Matrix of the body-frame foot positions. Each column corresponds to a separate foot. 77 | config : Config object 78 | Object of robot configuration parameters. 79 | 80 | Returns 81 | ------- 82 | numpy array (3,4) 83 | Matrix of corresponding joint angles. 84 | """ 85 | alpha = np.zeros((3, 4)) 86 | for i in range(4): 87 | body_offset = config.LEG_ORIGINS[:, i] 88 | alpha[:, i] = leg_explicit_inverse_kinematics( 89 | r_body_foot[:, i] - body_offset, i, config 90 | ) 91 | return alpha 92 | -------------------------------------------------------------------------------- /src/PupperConfig.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.linalg import solve 3 | from src.RobotConfig import MICROS_PER_RAD, NEUTRAL_ANGLE_DEGREES 4 | from enum import Enum 5 | 6 | class UserInputParams: 7 | def __init__(self): 8 | self.max_x_velocity = 0.5 9 | self.max_y_velocity = 0.24 10 | self.max_yaw_rate = 0.2 11 | self.max_pitch = 30.0 * np.pi / 180.0 12 | 13 | class PWMParams: 14 | def __init__(self): 15 | self.pins = np.array([[2, 14, 18, 23], [3, 15, 27, 24], [4, 17, 22, 25]]) 16 | self.range = 4000 17 | self.freq = 250 18 | 19 | 20 | class ServoParams: 21 | def __init__(self): 22 | self.neutral_position_pwm = 1500 # Middle position 23 | self.micros_per_rad = MICROS_PER_RAD # Must be calibrated 24 | 25 | # The neutral angle of the joint relative to the modeled zero-angle in degrees, for each joint 26 | self.neutral_angle_degrees = NEUTRAL_ANGLE_DEGREES 27 | 28 | self.servo_multipliers = np.array( 29 | [[1, 1, 1, 1], [-1, 1, -1, 1], [1, -1, 1, -1]] 30 | ) 31 | 32 | @property 33 | def neutral_angles(self): 34 | return self.neutral_angle_degrees * np.pi / 180.0 # Convert to radians 35 | 36 | 37 | class BehaviorState(Enum): 38 | REST = 0 39 | TROT = 1 40 | HOP = 2 41 | FINISHHOP = 3 42 | 43 | 44 | class MovementCommand: 45 | """Stores movement command 46 | """ 47 | 48 | def __init__(self): 49 | self.v_xy_ref = np.array([0, 0]) 50 | self.wz_ref = 0.0 51 | self.z_ref = -0.16 52 | 53 | 54 | class MovementReference: 55 | """Stores movement reference 56 | """ 57 | 58 | def __init__(self): 59 | self.v_xy_ref = np.array([0, 0]) 60 | self.wz_ref = 0.0 61 | self.z_ref = -0.16 62 | self.pitch = 0.0 63 | self.roll = 0.0 64 | 65 | 66 | class StanceParams: 67 | """Stance parameters 68 | """ 69 | 70 | def __init__(self): 71 | self.z_time_constant = 0.02 72 | self.z_speed = 0.03 # maximum speed [m/s] 73 | self.pitch_deadband = 0.02 74 | self.pitch_time_constant = 0.25 75 | self.max_pitch_rate = 0.15 76 | self.roll_speed = 0.16 # maximum roll rate [rad/s] 77 | self.delta_x = 0.1 78 | self.delta_y = 0.10 79 | self.x_shift = -0.01 80 | 81 | @property 82 | def default_stance(self): 83 | return np.array( 84 | [ 85 | [ 86 | self.delta_x + self.x_shift, 87 | self.delta_x + self.x_shift, 88 | -self.delta_x + self.x_shift, 89 | -self.delta_x + self.x_shift, 90 | ], 91 | [-self.delta_y, self.delta_y, -self.delta_y, self.delta_y], 92 | [0, 0, 0, 0], 93 | ] 94 | ) 95 | 96 | 97 | class SwingParams: 98 | """Swing Parameters 99 | """ 100 | 101 | def __init__(self): 102 | self.z_coeffs = None 103 | self.z_clearance = 0.05 104 | self.alpha = ( 105 | 0.5 # Ratio between touchdown distance and total horizontal stance movement 106 | ) 107 | self.beta = ( 108 | 0.5 # Ratio between touchdown distance and total horizontal stance movement 109 | ) 110 | 111 | @property 112 | def z_clearance(self): 113 | return self.__z_clearance 114 | 115 | @z_clearance.setter 116 | def z_clearance(self, z): 117 | self.__z_clearance = z 118 | b_z = np.array([0, 0, 0, 0, self.__z_clearance]) 119 | A_z = np.array( 120 | [ 121 | [0, 0, 0, 0, 1], 122 | [1, 1, 1, 1, 1], 123 | [0, 0, 0, 1, 0], 124 | [4, 3, 2, 1, 0], 125 | [0.5 ** 4, 0.5 ** 3, 0.5 ** 2, 0.5 ** 1, 0.5 ** 0], 126 | ] 127 | ) 128 | self.z_coeffs = solve(A_z, b_z) 129 | 130 | 131 | class GaitParams: 132 | """Gait Parameters 133 | """ 134 | 135 | def __init__(self): 136 | self.dt = 0.01 137 | self.num_phases = 4 138 | self.contact_phases = np.array( 139 | [[1, 1, 1, 0], 140 | [1, 0, 1, 1], 141 | [1, 0, 1, 1], 142 | [1, 1, 1, 0]] 143 | ) 144 | self.overlap_time = ( 145 | 0.10 # duration of the phase where all four feet are on the ground 146 | ) 147 | self.swing_time = ( 148 | 0.20 # duration of the phase when only two feet are on the ground 149 | ) 150 | 151 | @property 152 | def overlap_ticks(self): 153 | return int(self.overlap_time / self.dt) 154 | 155 | @property 156 | def swing_ticks(self): 157 | return int(self.swing_time / self.dt) 158 | 159 | @property 160 | def stance_ticks(self): 161 | return 2 * self.overlap_ticks + self.swing_ticks 162 | 163 | @property 164 | def phase_times(self): 165 | return np.array( 166 | [self.overlap_ticks, self.swing_ticks, self.overlap_ticks, self.swing_ticks] 167 | ) 168 | 169 | @property 170 | def phase_length(self): 171 | return 2 * self.overlap_ticks + 2 * self.swing_ticks 172 | 173 | 174 | class PupperConfig: 175 | """Pupper hardware parameters 176 | """ 177 | 178 | def __init__(self): 179 | # XML files 180 | self.XML_IN = "pupper.xml" 181 | self.XML_OUT = "pupper_out.xml" 182 | 183 | # Robot geometry 184 | self.LEG_FB = 0.10 # front-back distance from center line to leg axis 185 | self.LEG_LR = 0.04 # left-right distance from center line to leg plane 186 | self.LEG_L2 = 0.115 187 | self.LEG_L1 = 0.1235 188 | self.ABDUCTION_OFFSET = 0.03 # distance from abduction axis to leg 189 | self.FOOT_RADIUS = 0.01 190 | 191 | self.HIP_L = 0.0394 192 | self.HIP_W = 0.0744 193 | self.HIP_T = 0.0214 194 | self.HIP_OFFSET = 0.0132 195 | 196 | self.L = 0.276 197 | self.W = 0.100 198 | self.T = 0.050 199 | 200 | self.LEG_ORIGINS = np.array( 201 | [ 202 | [self.LEG_FB, self.LEG_FB, -self.LEG_FB, -self.LEG_FB], 203 | [-self.LEG_LR, self.LEG_LR, -self.LEG_LR, self.LEG_LR], 204 | [0, 0, 0, 0], 205 | ] 206 | ) 207 | 208 | self.ABDUCTION_OFFSETS = np.array( 209 | [ 210 | -self.ABDUCTION_OFFSET, 211 | self.ABDUCTION_OFFSET, 212 | -self.ABDUCTION_OFFSET, 213 | self.ABDUCTION_OFFSET, 214 | ] 215 | ) 216 | 217 | self.START_HEIGHT = 0.3 218 | 219 | # Robot inertia params 220 | self.FRAME_MASS = 0.560 # kg 221 | self.MODULE_MASS = 0.080 # kg 222 | self.LEG_MASS = 0.030 # kg 223 | self.MASS = self.FRAME_MASS + (self.MODULE_MASS + self.LEG_MASS) * 4 224 | 225 | # Compensation factor of 3 because the inertia measurement was just 226 | # of the carbon fiber and plastic parts of the frame and did not 227 | # include the hip servos and electronics 228 | self.FRAME_INERTIA = tuple( 229 | map(lambda x: 3.0 * x, (1.844e-4, 1.254e-3, 1.337e-3)) 230 | ) 231 | self.MODULE_INERTIA = (3.698e-5, 7.127e-6, 4.075e-5) 232 | 233 | leg_z = 1e-6 234 | leg_mass = 0.010 235 | leg_x = 1 / 12 * self.LEG_L1 ** 2 * leg_mass 236 | leg_y = leg_x 237 | self.LEG_INERTIA = (leg_x, leg_y, leg_z) 238 | 239 | # Joint params 240 | G = 220 # Servo gear ratio 241 | m_rotor = 0.016 # Servo rotor mass 242 | r_rotor = 0.005 # Rotor radius 243 | self.ARMATURE = G ** 2 * m_rotor * r_rotor ** 2 # Inertia of rotational joints 244 | # print("Servo armature", self.ARMATURE) 245 | 246 | NATURAL_DAMPING = 1.0 # Damping resulting from friction 247 | ELECTRICAL_DAMPING = 0.049 # Damping resulting from back-EMF 248 | 249 | self.REV_DAMPING = ( 250 | NATURAL_DAMPING + ELECTRICAL_DAMPING 251 | ) # Damping torque on the revolute joints 252 | 253 | # Servo params 254 | self.SERVO_REV_KP = 300 # Position gain [Nm/rad] 255 | 256 | # Force limits 257 | self.MAX_JOINT_TORQUE = 3.0 258 | self.REVOLUTE_RANGE = 1.57 259 | 260 | 261 | class EnvironmentConfig: 262 | """Environmental parameters 263 | """ 264 | 265 | def __init__(self): 266 | self.MU = 1.5 # coeff friction 267 | self.DT = 0.001 # seconds between simulation steps 268 | 269 | 270 | class SolverConfig: 271 | """MuJoCo solver parameters 272 | """ 273 | 274 | def __init__(self): 275 | self.JOINT_SOLREF = "0.001 1" # time constant and damping ratio for joints 276 | self.JOINT_SOLIMP = "0.9 0.95 0.001" # joint constraint parameters 277 | self.GEOM_SOLREF = "0.01 1" # time constant and damping ratio for geom contacts 278 | self.GEOM_SOLIMP = "0.9 0.95 0.001" # geometry contact parameters 279 | -------------------------------------------------------------------------------- /src/PupperXMLParser.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | 4 | def Parse(PUPPER_CONFIG, ENVIRONMENT_CONFIG, SOLVER_CONFIG): 5 | """Replace the variable names in the placeholder XML robot file with actual values given from the configuration object parameters. 6 | 7 | Parameters 8 | ---------- 9 | PUPPER_CONFIG : PupperConfig 10 | Pupper configuration object. 11 | ENVIRONMENT_CONFIG : EnvironmentConfig 12 | MuJoCo environment configuration object. 13 | SOLVER_CONFIG : SolverConfig 14 | MuJoCo solver configuration object. 15 | """ 16 | # FILE PATHS 17 | dir_path = os.path.dirname(os.path.realpath(__file__)) 18 | IN_FILE = os.path.join(dir_path, PUPPER_CONFIG.XML_IN) 19 | OUT_FILE = os.path.join(dir_path, PUPPER_CONFIG.XML_OUT) 20 | 21 | # ROBOT PARAMETERS 22 | 23 | # Solver params 24 | pupper_timestep = ENVIRONMENT_CONFIG.DT 25 | pupper_joint_solref = SOLVER_CONFIG.JOINT_SOLREF 26 | pupper_joint_solimp = SOLVER_CONFIG.JOINT_SOLIMP 27 | pupper_geom_solref = SOLVER_CONFIG.GEOM_SOLREF 28 | pupper_geom_solimp = SOLVER_CONFIG.GEOM_SOLIMP 29 | 30 | # Geometry params 31 | pupper_leg_radius = PUPPER_CONFIG.FOOT_RADIUS # radius of leg capsule 32 | pupper_friction = ENVIRONMENT_CONFIG.MU # friction between legs and ground 33 | pupper_half_size = "%s %s %s" % ( 34 | PUPPER_CONFIG.L / 2, 35 | PUPPER_CONFIG.W / 2, 36 | PUPPER_CONFIG.T / 2, 37 | ) # half-size of body box 38 | # to-from leg geometry 39 | pupper_leg_geom = "0 0 0 0 0 %s" % (-PUPPER_CONFIG.LEG_L) 40 | pupper_l1_geom = "0 0 0 0 0 %s" % (-PUPPER_CONFIG.LEG_L1) 41 | pupper_l2_geom = "0 0 0 0 0 %s" % (-PUPPER_CONFIG.LEG_L2) 42 | 43 | pupper_start_position = "0 0 %s" % ( 44 | PUPPER_CONFIG.START_HEIGHT 45 | ) # Initial position of the robot torso 46 | pupper_hip_box = "%s %s %s" % ( 47 | PUPPER_CONFIG.HIP_L / 2, 48 | PUPPER_CONFIG.HIP_W / 2, 49 | PUPPER_CONFIG.HIP_T / 2, 50 | ) # Size of the box representing the hip 51 | 52 | pupper_force_geom = "0 0 -0.34" 53 | 54 | # Mass/Inertia Params 55 | pupper_armature = PUPPER_CONFIG.ARMATURE # armature for joints [kgm2] 56 | pupper_frame_inertia = "%s %s %s" % PUPPER_CONFIG.FRAME_INERTIA 57 | pupper_module_inertia = "%s %s %s" % PUPPER_CONFIG.MODULE_INERTIA 58 | pupper_leg_inertia = "%s %s %s" % PUPPER_CONFIG.LEG_INERTIA 59 | 60 | # Joint & servo params 61 | pupper_rev_kp = PUPPER_CONFIG.SERVO_REV_KP 62 | 63 | pupper_joint_range = "%s %s" % ( 64 | -PUPPER_CONFIG.REVOLUTE_RANGE, 65 | PUPPER_CONFIG.REVOLUTE_RANGE, 66 | ) # joint range in rads for angular joints 67 | pupper_l2_joint_range = "%s %s" % ( 68 | -PUPPER_CONFIG.REVOLUTE_RANGE - PUPPER_CONFIG.REVOLUTE_RANGE, 69 | PUPPER_CONFIG.REVOLUTE_RANGE - PUPPER_CONFIG.REVOLUTE_RANGE, 70 | ) # joint range for l2 knee joint 71 | pupper_rev_torque_range = "%s %s" % ( 72 | -PUPPER_CONFIG.MAX_JOINT_TORQUE, 73 | PUPPER_CONFIG.MAX_JOINT_TORQUE, 74 | ) # force range for ab/ad and forward/back angular joints 75 | pupper_rev_damping = ( 76 | PUPPER_CONFIG.REV_DAMPING 77 | ) # damping on angular joints [Nm/rad/s] 78 | 79 | # Sensor Noise Parameters # 80 | pupper_accel_noise = 0.01 81 | pupper_encoder_noise = 0.001 82 | pupper_gyro_noise = 0.02 83 | pupper_encoder_vel_noise = 0.01 84 | pupper_force_noise = 0 85 | 86 | # Parse the xml 87 | print("Parsing MuJoCo XML file:") 88 | print("Input xml: %s" % IN_FILE) 89 | print("Output xml: %s" % OUT_FILE) 90 | 91 | with open(IN_FILE, "r") as file: 92 | filedata = file.read() 93 | 94 | # Replace variable names with values 95 | 96 | # Solver specs 97 | filedata = filedata.replace("pupper_timestep", str(pupper_timestep)) 98 | filedata = filedata.replace("pupper_joint_solref", str(pupper_joint_solref)) 99 | filedata = filedata.replace("pupper_geom_solref", str(pupper_geom_solref)) 100 | filedata = filedata.replace("pupper_friction", str(pupper_friction)) 101 | filedata = filedata.replace("pupper_armature", str(pupper_armature)) 102 | filedata = filedata.replace("pupper_joint_solimp", str(pupper_joint_solimp)) 103 | filedata = filedata.replace("pupper_geom_solimp", str(pupper_geom_solimp)) 104 | 105 | # Joint specs 106 | filedata = filedata.replace("pupper_joint_range", str(pupper_joint_range)) 107 | filedata = filedata.replace("pupper_l2_joint_range", str(pupper_l2_joint_range)) 108 | filedata = filedata.replace("pupper_rev_torque_range", str(pupper_rev_torque_range)) 109 | filedata = filedata.replace("pupper_rev_damping", str(pupper_rev_damping)) 110 | 111 | # Servo specs 112 | filedata = filedata.replace("pupper_rev_kp", str(pupper_rev_kp)) 113 | 114 | # Geometry specs 115 | filedata = filedata.replace("pupper_frame_mass", str(PUPPER_CONFIG.FRAME_MASS)) 116 | filedata = filedata.replace("pupper_module_mass", str(PUPPER_CONFIG.MODULE_MASS)) 117 | filedata = filedata.replace("pupper_leg_mass", str(PUPPER_CONFIG.LEG_MASS)) 118 | filedata = filedata.replace("pupper_frame_inertia", str(pupper_frame_inertia)) 119 | filedata = filedata.replace("pupper_module_inertia", str(pupper_module_inertia)) 120 | filedata = filedata.replace("pupper_leg_inertia", str(pupper_leg_inertia)) 121 | filedata = filedata.replace("pupper_leg_radius", str(pupper_leg_radius)) 122 | filedata = filedata.replace("pupper_half_size", str(pupper_half_size)) 123 | filedata = filedata.replace("pupper_leg_fb", str(PUPPER_CONFIG.LEG_FB)) 124 | filedata = filedata.replace("pupper_leg_lr", str(PUPPER_CONFIG.LEG_LR)) 125 | filedata = filedata.replace("pupper_leg_geom", str(pupper_leg_geom)) 126 | filedata = filedata.replace("pupper_start_position", str(pupper_start_position)) 127 | filedata = filedata.replace("pupper_force_geom", str(pupper_force_geom)) 128 | filedata = filedata.replace("pupper_hip_box", str(pupper_hip_box)) 129 | filedata = filedata.replace("pupper_hip_offset", str(PUPPER_CONFIG.HIP_OFFSET)) 130 | filedata = filedata.replace( 131 | "pupper_abduction_offset", str(PUPPER_CONFIG.ABDUCTION_OFFSET) 132 | ) 133 | filedata = filedata.replace("pupper_l1_length", str(PUPPER_CONFIG.LEG_L1)) 134 | filedata = filedata.replace("pupper_l2_length", str(PUPPER_CONFIG.LEG_L2)) 135 | filedata = filedata.replace("pupper_l1_geom", str(pupper_leg_geom)) 136 | filedata = filedata.replace("pupper_l2_geom", str(pupper_leg_geom)) 137 | 138 | # Sensor noise 139 | filedata = filedata.replace("pupper_accel_noise", str(pupper_accel_noise)) 140 | filedata = filedata.replace("pupper_gyro_noise", str(pupper_gyro_noise)) 141 | filedata = filedata.replace("pupper_encoder_noise", str(pupper_encoder_noise)) 142 | filedata = filedata.replace( 143 | "pupper_encoder_vel_noise", str(pupper_encoder_vel_noise) 144 | ) 145 | filedata = filedata.replace("pupper_force_noise", str(pupper_force_noise)) 146 | 147 | # Write the xml file 148 | with open(OUT_FILE, "w") as file: 149 | file.write(filedata) 150 | -------------------------------------------------------------------------------- /src/RobotConfig.py: -------------------------------------------------------------------------------- 1 | """ 2 | Per-robot configuration file that is particular to each individual robot, not just the type of robot. 3 | """ 4 | import numpy as np 5 | 6 | 7 | MICROS_PER_RAD = 10.0 * 180.0 / np.pi # Must be calibrated 8 | NEUTRAL_ANGLE_DEGREES = np.array( 9 | [[5, 3.5, -3, 12], [65.5, 47, 34, 53.5], [-29, -20, -35, -43]] 10 | ) 11 | 12 | -------------------------------------------------------------------------------- /src/StanceController.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from transforms3d.euler import euler2mat 3 | 4 | 5 | def position_delta(z_measured, stance_params, movement_reference, gait_params): 6 | """Calculate the difference between the next desired body location and the current body location 7 | 8 | Parameters 9 | ---------- 10 | z_measured : float 11 | Z coordinate of the feet relative to the body. 12 | stance_params : StanceParams 13 | Stance parameters object. 14 | movement_reference : MovementReference 15 | Movement reference object. 16 | gait_params : GaitParams 17 | Gait parameters object. 18 | 19 | Returns 20 | ------- 21 | (Numpy array (3), Numpy array (3, 3)) 22 | (Position increment, rotation matrix increment) 23 | """ 24 | v_xy = np.array( 25 | [ 26 | -movement_reference.v_xy_ref[0], 27 | -movement_reference.v_xy_ref[1], 28 | 1.0 29 | / stance_params.z_time_constant 30 | * (movement_reference.z_ref - z_measured), 31 | ] 32 | ) 33 | delta_p = v_xy * gait_params.dt 34 | delta_R = euler2mat(0, 0, -movement_reference.wz_ref * gait_params.dt) 35 | return (delta_p, delta_R) 36 | 37 | 38 | def stance_foot_location( 39 | stance_foot_location, stance_params, gait_params, movement_reference 40 | ): 41 | """Find the next desired location for a foot in stance. 42 | 43 | Parameters 44 | ---------- 45 | stance_foot_location : Numpy array (3, 4) 46 | Location of the foot 47 | stance_params : StanceParams 48 | Stance parameters object. 49 | gait_params : GaitParams 50 | Gait parameters object. 51 | movement_reference : MovementReference 52 | Movement reference object. 53 | 54 | Returns 55 | ------- 56 | Numpy array (3) 57 | Next desired location for the foot 58 | """ 59 | z_measured = stance_foot_location[2] 60 | (delta_p, delta_R) = position_delta( 61 | z_measured, stance_params, movement_reference, gait_params 62 | ) 63 | incremented_location = delta_R @ stance_foot_location + delta_p 64 | 65 | return incremented_location 66 | -------------------------------------------------------------------------------- /src/SwingLegController.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from transforms3d.euler import euler2mat 3 | 4 | 5 | def raibert_touchdown_locations( 6 | swing_params, stance_params, gait_params, movement_reference 7 | ): 8 | """[summary] 9 | 10 | Parameters 11 | ---------- 12 | swing_params : [type] 13 | [description] 14 | stance_params : [type] 15 | [description] 16 | gait_params : [type] 17 | [description] 18 | movement_reference : [type] 19 | [description] 20 | """ 21 | p_temp = ( 22 | swing_params.alpha 23 | * gait_params.stance_ticks 24 | * gait_params.dt 25 | * movement_reference.v_xy_ref 26 | ) 27 | p = np.array([p_temp[0], p_temp[1], 0.0]) 28 | theta = ( 29 | swing_params.beta 30 | * gait_params.stance_ticks 31 | * gait_params.dt 32 | * movement_reference.wz_ref 33 | ) 34 | R = euler2mat(0, 0, theta) 35 | return R * stance_params.default_stance + p 36 | 37 | 38 | def raibert_touchdown_location( 39 | leg_index, swing_params, stance_params, gait_params, movement_reference 40 | ): 41 | """[summary] 42 | 43 | Parameters 44 | ---------- 45 | leg_index : [type] 46 | [description] 47 | swing_params : [type] 48 | [description] 49 | stance_params : [type] 50 | [description] 51 | gait_params : [type] 52 | [description] 53 | movement_reference : [type] 54 | [description] 55 | 56 | Returns 57 | ------- 58 | [type] 59 | [description] 60 | """ 61 | p_temp = ( 62 | swing_params.alpha 63 | * gait_params.stance_ticks 64 | * gait_params.dt 65 | * movement_reference.v_xy_ref 66 | ) 67 | p = np.array([p_temp[0], p_temp[1], 0.0]) 68 | theta = ( 69 | swing_params.beta 70 | * gait_params.stance_ticks 71 | * gait_params.dt 72 | * movement_reference.wz_ref 73 | ) 74 | R = euler2mat(0, 0, theta) 75 | return R @ stance_params.default_stance[:, leg_index] + p 76 | 77 | 78 | def swing_height(swing_phase, swing_params, triangular=True): 79 | """[summary] 80 | 81 | Parameters 82 | ---------- 83 | swing_phase : [type] 84 | [description] 85 | swing_params : [type] 86 | [description] 87 | triangular : bool, optional 88 | [description], by default True 89 | """ 90 | if triangular: 91 | if swing_phase < 0.5: 92 | swing_height_ = swing_phase / 0.5 * swing_params.z_clearance 93 | else: 94 | swing_height_ = swing_params.z_clearance * (1 - (swing_phase - 0.5) / 0.5) 95 | else: 96 | time_vec = np.array( 97 | [swing_phase ** 4, swing_phase ** 3, swing_phase ** 2, swing_phase, 1] 98 | ) 99 | swing_height_ = np.dot(time_vec, swing_params.z_coeffs) 100 | return swing_height_ 101 | 102 | 103 | def swing_foot_location( 104 | swing_prop, 105 | foot_location, 106 | leg_index, 107 | swing_params, 108 | stance_params, 109 | gait_params, 110 | movement_reference, 111 | ): 112 | """[summary] 113 | 114 | Parameters 115 | ---------- 116 | swing_prop : [type] 117 | [description] 118 | foot_location : [type] 119 | [description] 120 | leg_index : [type] 121 | [description] 122 | swing_params : [type] 123 | [description] 124 | stance_params : [type] 125 | [description] 126 | gait_params : [type] 127 | [description] 128 | movement_reference : [type] 129 | [description] 130 | 131 | Returns 132 | ------- 133 | [type] 134 | [description] 135 | """ 136 | assert swing_prop >= 0 and swing_prop <= 1 137 | 138 | swing_height_ = swing_height(swing_prop, swing_params) 139 | touchdown_location = raibert_touchdown_location( 140 | leg_index, swing_params, stance_params, gait_params, movement_reference 141 | ) 142 | time_left = gait_params.dt * gait_params.swing_ticks * (1.0 - swing_prop) 143 | v = (touchdown_location - foot_location) / time_left * np.array([1, 1, 0]) 144 | delta_foot_location = v * gait_params.dt 145 | z_vector = np.array([0, 0, swing_height_ + movement_reference.z_ref]) 146 | return foot_location * np.array([1, 1, 0]) + z_vector + delta_foot_location 147 | -------------------------------------------------------------------------------- /src/UserInput.py: -------------------------------------------------------------------------------- 1 | import UDPComms 2 | import numpy as np 3 | import time 4 | from src.PupperConfig import BehaviorState 5 | from src.Utilities import deadband, clipped_first_order_filter 6 | 7 | class UserInputs: 8 | def __init__(self, max_x_velocity, max_y_velocity, max_yaw_rate, max_pitch, udp_port=8830): 9 | self.max_x_velocity = max_x_velocity 10 | self.max_y_velocity = max_y_velocity 11 | self.max_yaw_rate = max_yaw_rate 12 | self.max_pitch = max_pitch 13 | 14 | self.x_vel = 0.0 15 | self.y_vel = 0.0 16 | self.yaw_rate = 0.0 17 | self.pitch = 0.0 18 | 19 | self.stance_movement = 0 20 | self.roll_movement = 0 21 | 22 | self.gait_toggle = 0 23 | self.previous_gait_toggle = 0 24 | self.gait_mode = 0 25 | 26 | self.previous_state = BehaviorState.REST 27 | self.current_state = BehaviorState.REST 28 | 29 | self.previous_hop_toggle = 0 30 | self.hop_toggle = 0 31 | self.hop_begin_time = 0 32 | 33 | self.activate = 0 34 | self.last_activate = 0 35 | 36 | self.message_rate = 50 37 | self.udp_handle = UDPComms.Subscriber(udp_port, timeout=0.3) 38 | 39 | 40 | def get_input(user_input_obj, do_print=False): 41 | try: 42 | msg = user_input_obj.udp_handle.get() 43 | user_input_obj.x_vel = msg["ly"] * 0.5 44 | user_input_obj.y_vel = msg["lx"] * -0.24 45 | user_input_obj.yaw_rate = msg["rx"] * -2.0 46 | user_input_obj.pitch = msg["ry"] * 40 * np.pi / 180.0 47 | user_input_obj.gait_toggle = msg["R1"] 48 | user_input_obj.activate = msg["L1"] 49 | user_input_obj.stance_movement = msg["dpady"] 50 | user_input_obj.roll_movement = msg["dpadx"] 51 | user_input_obj.message_rate = msg["message_rate"] 52 | user_input_obj.hop_toggle = msg["x"] 53 | 54 | # Check if requesting a state transition to trotting, or from trotting to resting 55 | if user_input_obj.gait_toggle == 1 and user_input_obj.previous_gait_toggle == 0: 56 | if user_input_obj.previous_state == BehaviorState.TROT: 57 | user_input_obj.current_state = BehaviorState.REST 58 | elif user_input_obj.previous_state == BehaviorState.REST: 59 | user_input_obj.current_state = BehaviorState.TROT 60 | 61 | # Check if requesting a state transition to hopping, from trotting or resting 62 | if user_input_obj.hop_toggle == 1 and user_input_obj.previous_hop_toggle == 0: 63 | if user_input_obj.current_state == BehaviorState.HOP: 64 | user_input_obj.current_state = BehaviorState.FINISHHOP 65 | elif user_input_obj.current_state == BehaviorState.REST: 66 | user_input_obj.current_state = BehaviorState.HOP 67 | elif user_input_obj.current_state == BehaviorState.FINISHHOP: 68 | user_input_obj.current_state = BehaviorState.REST 69 | 70 | # Update previous values for toggles and state 71 | user_input_obj.previous_state = user_input_obj.current_state 72 | user_input_obj.previous_gait_toggle = user_input_obj.gait_toggle 73 | user_input_obj.previous_hop_toggle = user_input_obj.hop_toggle 74 | 75 | except UDPComms.timeout: 76 | if do_print: 77 | print("UDP Timed out") 78 | 79 | 80 | def update_controller(controller, user_input_obj): 81 | controller.movement_reference.v_xy_ref = np.array( 82 | [user_input_obj.x_vel, user_input_obj.y_vel] 83 | ) 84 | controller.movement_reference.wz_ref = user_input_obj.yaw_rate 85 | 86 | message_dt = 1.0 / user_input_obj.message_rate 87 | 88 | # TODO: Put this filter code somewhere else 89 | deadbanded_pitch = deadband(user_input_obj.pitch, controller.stance_params.pitch_deadband) 90 | pitch_rate = clipped_first_order_filter(controller.movement_reference.pitch, deadbanded_pitch, controller.stance_params.max_pitch_rate, controller.stance_params.pitch_time_constant) 91 | controller.movement_reference.pitch += message_dt * pitch_rate 92 | 93 | controller.state = user_input_obj.current_state 94 | 95 | # Note this is negative since it is the feet relative to the body 96 | controller.movement_reference.z_ref -= ( 97 | controller.stance_params.z_speed * message_dt * user_input_obj.stance_movement 98 | ) 99 | controller.movement_reference.roll += ( 100 | controller.stance_params.roll_speed * message_dt * user_input_obj.roll_movement 101 | ) 102 | -------------------------------------------------------------------------------- /src/Utilities.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def deadband(value, band_radius): 4 | return max(value - band_radius, 0) + min(value + band_radius, 0) 5 | 6 | def clipped_first_order_filter(input, target, max_rate, tau): 7 | rate = (target - input) / tau 8 | return np.clip(rate, -max_rate, max_rate) -------------------------------------------------------------------------------- /src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Nate711/PupperPythonSim/a12b7362574ccab9b8b01f64a6ebeaffbbd1baaf/src/__init__.py -------------------------------------------------------------------------------- /src/puppe_prismatic.xml: -------------------------------------------------------------------------------- 1 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 133 | -------------------------------------------------------------------------------- /src/pupper.xml: -------------------------------------------------------------------------------- 1 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 145 | -------------------------------------------------------------------------------- /src/pupper_out.xml: -------------------------------------------------------------------------------- 1 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 145 | -------------------------------------------------------------------------------- /src/pupper_pybullet.xml: -------------------------------------------------------------------------------- 1 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /src/pupper_pybullet_out.xml: -------------------------------------------------------------------------------- 1 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /tests/IMULatencyTest.py: -------------------------------------------------------------------------------- 1 | import time 2 | import serial 3 | import threading 4 | import queue 5 | 6 | # ser = serial.Serial( 7 | # port="/dev/cu.usbmodem63711001", 8 | # baudrate=500000, 9 | # parity=serial.PARITY_NONE, 10 | # stopbits=serial.STOPBITS_ONE, 11 | # bytesize=serial.EIGHTBITS, 12 | # timeout=1, 13 | # ) 14 | 15 | # # while 1: 16 | # # x = ser.readline().decode("utf8").strip() 17 | # # print(x) 18 | # # parsed = x.split(",") 19 | # # print(parsed) 20 | 21 | 22 | def get_data_thread(data_queue): 23 | serial_handle = serial_obj(port="/dev/cu.usbmodem63711001", timeout=1.0) 24 | while True: 25 | x = serial_handle.readline().decode('utf8').strip() 26 | if x: 27 | parsed = x.split(',') 28 | data_queue.put(parsed) 29 | 30 | 31 | def serial_obj(port, timeout): 32 | return serial.Serial( 33 | port=port, 34 | baudrate=500000, 35 | parity=serial.PARITY_NONE, 36 | stopbits=serial.STOPBITS_ONE, 37 | bytesize=serial.EIGHTBITS, 38 | timeout=timeout 39 | ) 40 | 41 | 42 | def test_multithread(): 43 | data_queue = queue.Queue() 44 | imu_thread = threading.Thread(target=get_data_thread, args=(data_queue,)) 45 | imu_thread.start() 46 | 47 | while True: 48 | while not data_queue.empty(): 49 | orientation = data_queue.get() 50 | print(orientation) 51 | 52 | 53 | def test_inline(): 54 | serial_handle = serial_obj(port="/dev/cu.usbmodem63711001", timeout=0.0001) 55 | messages = 1 56 | start = time.time() 57 | while True: 58 | x = serial_handle.readline().decode('utf8').strip() 59 | if x: 60 | parsed = x.split(',') 61 | # print(parsed) 62 | print((time.time()-start)/messages) 63 | messages += 1 64 | else: 65 | pass 66 | # print("no message") 67 | test_multithread() 68 | # test_inline() --------------------------------------------------------------------------------