diff --git a/.gitignore b/.gitignore index 22139e4..a04cbd6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ *.DS_Store *.pyc +*.swp src/__pycache__ \ No newline at end of file diff --git a/README.md b/README.md index 95e0643..0474ac9 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,12 @@ -# Pupper Robot: Python Simulation +# Pupper Robot: Control & Simulation + +This repo has been replaced by: https://github.com/stanfordroboticsclub/stanfordquadruped ## Overview 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. +Video of the robot following a QR code: https://www.youtube.com/watch?v=iyuJq_Pn7TM + ## Installing and Running Code on the Raspberry Pi ### Materials - Raspberry Pi 4 @@ -36,22 +40,33 @@ This repository contains Python code to run Pupper, a Raspberry Pi-based quadrup - Clone the Pupper repository https://github.com/Nate711/PupperPythonSim/ - Install requirements: ```shell - bash install_packages_robot.sh + sudo bash install_packages_robot.sh ``` - Get the Pupper controller code - Clone the controller repo: https://github.com/stanfordroboticsclub/PupperCommand - Follow the instructions in the README ## Running the Robot -- Start the joystick publisher. Instructions here: https://github.com/stanfordroboticsclub/PupperCommand/blob/master/README.md -- Start the PiGPIO daemon by executing in shell: +- SSH into the robot ```shell - sudo pigpiod + ssh pi@10.0.0.xx + ``` + where xx is the local address you chose for the Pi +- Once connected to the Pi, go into read-write mode + ```shell + rw ``` -- Run the robot code: +- Now go into this repo's PupperPythonSim directory and run the robot code! ```shell + cd PupperPythonSim + sudo pigpiod python3 run_robot.py - ``` - + ``` + If you already have the pigpio daemon running, you can ignore when ```sudo pigpiod``` says it can't initialize pigpiod. +- You can interrupt and stop the program by pressing Control-C. +- To turn off the servo motors, run + ```shell + sudo pkill pigpiod + ``` ## Installation for PyBullet Simulation 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. @@ -95,10 +110,11 @@ brew link --overwrite gcc 3. Install the python requirements: ```bash -bash install_packages_sim.sh +sudo bash install_packages_sim.sh ``` ## Run MuJoCo Simulation +(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!) 1. Run ```shell python3 simulate.py diff --git a/TestIMUTransformation.py b/TestIMUTransformation.py new file mode 100644 index 0000000..18082b8 --- /dev/null +++ b/TestIMUTransformation.py @@ -0,0 +1,17 @@ +from transforms3d.euler import euler2mat, quat2euler +from transforms3d.quaternions import qconjugate, quat2axangle +from transforms3d.axangles import axangle2mat +from src.IMU import read_orientation, create_imu_handle +from src.PupperConfig import IMUParams + +imu_params = IMUParams("/dev/cu.usbmodem63711001") +imu_handle = create_imu_handle(imu_params) +imu_handle.reset_input_buffer() + +while True: + quat_orientation = read_orientation(imu_handle) + if quat_orientation is not None: + # q_inv = qconjugate(quat_orientation) + # (yaw, pitch, roll) = quat2euler(q_inv) + (yaw, pitch, roll) = quat2euler(quat_orientation) + print(round(roll,3), round(pitch,3), round(yaw,3)) \ No newline at end of file diff --git a/install_packages_robot.sh b/install_packages_robot.sh index 7a2e02c..9056ab4 100644 --- a/install_packages_robot.sh +++ b/install_packages_robot.sh @@ -1,2 +1,2 @@ yes | sudo apt-get install libatlas-base-dev -yes | pip3 install -r robot_requirements.txt +yes | pip3 install numpy scipy transforms3d pigpio pyserial diff --git a/pupper.service b/pupper.service new file mode 100644 index 0000000..81070ea --- /dev/null +++ b/pupper.service @@ -0,0 +1,13 @@ +[Unit] +Description=Pupper control service +Requires=joystick.service +After=joystick.service + +[Service] +ExecStartPre=sudo pigpiod +ExecStart=/usr/bin/python3 /home/pi/PupperPythonSim/run_robot.py +KillSignal=2 +TimeoutStopSec=10 + +[Install] +WantedBy=multi-user.target diff --git a/robot_requirements.txt b/robot_requirements.txt deleted file mode 100644 index 0a40845..0000000 --- a/robot_requirements.txt +++ /dev/null @@ -1,4 +0,0 @@ -numpy -scipy -transforms3d -pigpio diff --git a/run_robot.py b/run_robot.py index 25a555e..9ba58ae 100644 --- a/run_robot.py +++ b/run_robot.py @@ -2,68 +2,103 @@ import numpy as np import UDPComms import time +import subprocess +from src.IMU import IMU from src.Controller import step_controller, Controller -from src.HardwareInterface import send_servo_commands, initialize_pwm +from src.HardwareInterface import send_servo_commands, initialize_pwm, deactivate_servos from src.PupperConfig import ( + PupperConfig, MovementReference, GaitParams, StanceParams, SwingParams, ServoParams, PWMParams, + UserInputParams, ) from src.UserInput import UserInputs, get_input, update_controller +def start_pigpiod(): + print("Starting pigpiod...") + subprocess.Popen(["sudo", "pigpiod"]) + time.sleep(2) + print("Done.") + + +def stop_pigpiod(): + print("Killing pigpiod...") + subprocess.Popen(["sudo", "pkill", "pigpiod"]) + time.sleep(2) + print("Done.") + + def main(): """Main program """ + + # Start pwm to servos + # start_pigpiod() pi_board = pigpio.pi() pwm_params = PWMParams() initialize_pwm(pi_board, pwm_params) + # Create config + robot_config = PupperConfig() servo_params = ServoParams() - controller = Controller() - controller.movement_reference = MovementReference() - controller.movement_reference.v_xy_ref = np.array([0.0, 0.0]) - controller.movement_reference.wz_ref = 0 - - controller.movement_reference.pitch = 15.0 * np.pi / 180.0 - controller.movement_reference.roll = 0 + # Create imu handle + imu = IMU(port="/dev/ttyACM0") + imu.flush_buffer() - controller.swing_params = SwingParams() - controller.swing_params.z_clearance = 0.05 - controller.stance_params = StanceParams() - controller.stance_params.delta_y = 0.10 - controller.gait_params = GaitParams() - - user_input = UserInputs() + # Create controller and user input handles + controller = Controller(robot_config) + input_params = UserInputParams() + 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) last_loop = time.time() - now = last_loop - start = time.time() - - for i in range(60000): - last_loop = time.time() - - # Parse the udp joystick commands and then update the robot controller's parameters - get_input(user_input) - update_controller(controller, user_input) - # Step the controller forward by dt - step_controller(controller) + print("Summary of gait parameters:") + print("overlap time: ", controller.gait_params.overlap_time) + print("swing time: ", controller.gait_params.swing_time) + print("z clearance: ", controller.swing_params.z_clearance) + print("x shift: ", controller.stance_params.x_shift) - # Update the pwm widths going to the servos - send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) + # Wait until the activate button has been pressed + while True: + print("Waiting for L1 to activate robot.") + while True: + get_input(user_input) + if user_input.activate == 1 and user_input.last_activate == 0: + user_input.last_activate = 1 + break + user_input.last_activate = user_input.activate + print("Robot activated.") - # Wait until it's time to execute again - while now - last_loop < controller.gait_params.dt: + while True: now = time.time() - # print("Time since last loop: ", now - last_loop) + if now - last_loop < controller.gait_params.dt: + continue + last_loop = time.time() + + # Parse the udp joystick commands and then update the robot controller's parameters + get_input(user_input) + if user_input.activate == 1 and user_input.last_activate == 0: + user_input.last_activate = 1 + break + else: + user_input.last_activate = user_input.activate - end = time.time() - print("seconds per loop: ", (end - start) / 1000.0) + update_controller(controller, user_input) + # Read imu data. Orientation will be None if no data was available + quat_orientation = imu.read_orientation() + # Step the controller forward by dt + step_controller(controller, robot_config, quat_orientation) + + # Update the pwm widths going to the servos + send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) + deactivate_servos(pi_board, pwm_params) main() + diff --git a/simulate_pybullet.py b/simulate_pybullet.py index 171f6b7..5a21e64 100644 --- a/simulate_pybullet.py +++ b/simulate_pybullet.py @@ -1,11 +1,23 @@ import pybullet as p import pybullet_data import time +import numpy as np from src import PupperXMLParser -from src.Controller import Controller, step_controller -from src.PupperConfig import PupperConfig, EnvironmentConfig, SolverConfig, SwingParams -import numpy as np +from src.Controller import step_controller, Controller +from src.HardwareInterface import send_servo_commands, initialize_pwm +from src.PupperConfig import ( + PupperConfig, + MovementReference, + GaitParams, + StanceParams, + SwingParams, + ServoParams, + PWMParams, + EnvironmentConfig, + SolverConfig +) +from src.UserInput import UserInputs, get_input, update_controller def parallel_to_serial_joint_angles(joint_matrix): @@ -27,11 +39,23 @@ def parallel_to_serial_joint_angles(joint_matrix): return temp +# Create environment objects +PUPPER_CONFIG = PupperConfig() +PUPPER_CONFIG.XML_IN = "pupper_pybullet.xml" +PUPPER_CONFIG.XML_OUT = "pupper_pybullet_out.xml" + +ENVIRONMENT_CONFIG = EnvironmentConfig() +SOLVER_CONFIG = SolverConfig() + +# Initailize MuJoCo +PupperXMLParser.Parse(PUPPER_CONFIG, ENVIRONMENT_CONFIG, SOLVER_CONFIG) + # Set up PyBullet Simulator physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally p.setGravity(0, 0, -9.81) -pupperId = p.loadMJCF("src/pupper_out.xml") +pupperId = p.loadMJCF("src/pupper_pybullet_out.xml") + print("") print("Pupper bodies IDs:", pupperId) numjoints = p.getNumJoints(pupperId[1]) @@ -39,79 +63,86 @@ def parallel_to_serial_joint_angles(joint_matrix): print("Joint Info: ") for i in range(numjoints): print(p.getJointInfo(pupperId[1], i)) -joint_indices = list(range(0, 24, 2)) -# Create environment objects -PUPPER_CONFIG = PupperConfig() -ENVIRONMENT_CONFIG = EnvironmentConfig() -SOLVER_CONFIG = SolverConfig() - -# Initailize MuJoCo -PupperXMLParser.Parse(PUPPER_CONFIG, ENVIRONMENT_CONFIG, SOLVER_CONFIG) +joint_indices = list(range(0, 24, 2)) -# Create pupper_controller -pupper_controller = Controller() -pupper_controller.movement_reference.v_xy_ref = np.array([0.2, 0.0]) -pupper_controller.movement_reference.wz_ref = 0.0 -pupper_controller.swing_params.z_clearance = 0.03 # Changing to be higher -pupper_controller.gait_params.dt = 0.01 # Simulated seconds per controller step -# Whole sim is set to run about 600ms per gate -pupper_controller.stance_params.delta_y = 0.1 +# Create controller +robot_config = PupperConfig() +servo_params = ServoParams() +controller = Controller(robot_config) +user_input = UserInputs() # Run the simulation -timesteps = 60000 +timesteps = 240*60*10 # simulate for a max of 10 minutes # Sim seconds per sim step -sim_steps_per_sim_second = 1000 +sim_steps_per_sim_second = 240 sim_seconds_per_sim_step = 1.0 / sim_steps_per_sim_second -p.setTimeStep(1.0 / sim_steps_per_sim_second) - start = time.time() last_control_update = 0 + +controller.gait_params.contact_phases = np.array( + [[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]] +) +controller.swing_params.z_clearance = 0.03 +controller.movement_reference.v_xy_ref = np.array([0.10, 0.0]) +# controller.movement_reference.wz_ref = 0.5 + +# To account for the fact that the CoM of the robot is a little behind the geometric center, +# put the robot feet a little behind the geometric center to try to match the actual CoM +# controller.stance_params.x_shift = -0.01 + +(hey, now) = (0, 0) + for steps in range(timesteps): - # Step the pupper controller forward current_time = time.time() # Simulated time can be computed as sim_seconds_per_sim_step * steps simluated_time_elapsed = sim_seconds_per_sim_step * steps - # Want a function that start at 100 then linearly ramps to 0 over 1 second then stays at 0 - p.setGravity(0, 0, -9.81 - max(0, (20 - simluated_time_elapsed * 10))) - - if simluated_time_elapsed - last_control_update > pupper_controller.gait_params.dt: + if simluated_time_elapsed - last_control_update > controller.gait_params.dt: + # This block usually takes < 1ms to run, but every 10 or so iterations it takes as many as 50ms to run + + hey = time.time() last_control_update = simluated_time_elapsed - # step_controller takes between 0.3ms and 1ms to complete! Definitely fast enough! - # This will move the joints far enough to last gait_params.dt seconds - # If we want the legs to move the correct distance in simulated time, we need to tell the - # Robot how many *simulated* seconds have ellapse - step_controller(pupper_controller) + (pos, q_scalar_last) = p.getBasePositionAndOrientation(1) + q_corrected = (q_scalar_last[3], q_scalar_last[0], q_scalar_last[1], q_scalar_last[2]) + # print("Orientation: ", q_corrected) + + # Calculate the next joint angle commands + step_controller(controller, robot_config, q_corrected) + # Convert the joint angles from the parallel linkage to the simulated serial linkage serial_joint_angles = parallel_to_serial_joint_angles( - pupper_controller.joint_angles + controller.joint_angles ) - # t2 = time.time() + + # Send the joint angles to the sim p.setJointMotorControlArray( bodyUniqueId=pupperId[1], jointIndices=joint_indices, controlMode=p.POSITION_CONTROL, targetPositions=list(serial_joint_angles.T.reshape(12)), - # positionGains=[1]*12, - # velocityGains=[1]*12, - forces=[2] * 12, + positionGains=[0.25]*12, + velocityGains=[0.5]*12, + forces=[10] * 12, ) - # print(t2-now, ",", time.time()-t2) + now = time.time() + + # Simulate physics for 1/240 seconds (1/240 is the default timestep) p.stepSimulation() - # time.sleep(ENVIRONMENT_CONFIG.DT) - # Perf testing + # time.sleep(0.01) + + # Performance testing elapsed = time.time() - start - if (steps % 100) == 0: + if (steps % 60) == 0: print( "Sim seconds elapsed: {}, Real seconds elapsed: {}".format( - simluated_time_elapsed, elapsed + round(simluated_time_elapsed,3), round(elapsed,3) ) ) - # print("Average steps per second: {0}, elapsed: {1}, i:{2}".format(i / elapsed, elapsed, i)) + # print("Average steps per second: {0}, elapsed: {1}, i:{2}".format(steps / elapsed, elapsed, i)) diff --git a/src/Controller.py b/src/Controller.py index 4992a9b..f7db634 100644 --- a/src/Controller.py +++ b/src/Controller.py @@ -1,24 +1,29 @@ from src.PupperConfig import SwingParams, StanceParams, GaitParams, MovementReference -from src.PupperConfig import PupperConfig from src.Gaits import contacts, subphase_time from src.Kinematics import four_legs_inverse_kinematics from src.StanceController import stance_foot_location from src.SwingLegController import swing_foot_location +from src.Utilities import clipped_first_order_filter +from src.PupperConfig import BehaviorState import numpy as np -from transforms3d.euler import euler2mat - +from transforms3d.euler import euler2mat, quat2euler +from transforms3d.quaternions import qconjugate, quat2axangle +from transforms3d.axangles import axangle2mat class Controller: """Controller and planner object """ - def __init__(self): + def __init__(self, robot_config): self.swing_params = SwingParams() self.stance_params = StanceParams() self.gait_params = GaitParams() self.movement_reference = MovementReference() - self.robot_config = PupperConfig() + self.smoothed_yaw = 0.0 # for REST mode only + + self.previous_state = BehaviorState.REST + self.state = BehaviorState.REST self.ticks = 0 @@ -27,8 +32,9 @@ def __init__(self): self.stance_params.default_stance + np.array([0, 0, self.movement_reference.z_ref])[:, np.newaxis] ) + self.contact_modes = np.zeros(4) self.joint_angles = four_legs_inverse_kinematics( - self.foot_locations, self.robot_config + self.foot_locations, robot_config ) @@ -36,11 +42,11 @@ def step( ticks, foot_locations, swing_params, stance_params, gait_params, movement_reference ): """Calculate the desired foot locations for the next timestep - + Parameters ---------- ticks : int - Number of clock ticks since the start. Time between ticks is given my the gait params dt variable. + Number of clock ticks since the start. Time between ticks is given by the gait params dt variable. foot_locations : Numpy array (3, 4) Locations of all four feet. swing_params : SwingParams @@ -51,7 +57,7 @@ def step( Gait parameters object. movement_reference : MovementReference Movement reference object. - + Returns ------- Numpy array (3, 4) @@ -80,36 +86,101 @@ def step( movement_reference, ) new_foot_locations[:, leg_index] = new_location - return new_foot_locations + return new_foot_locations, contact_modes -def step_controller(controller): +def step_controller(controller, robot_config, quat_orientation): """Steps the controller forward one timestep - + Parameters ---------- controller : Controller Robot controller object. """ - controller.foot_locations = step( - controller.ticks, - controller.foot_locations, - controller.swing_params, - controller.stance_params, - controller.gait_params, - controller.movement_reference, - ) + if controller.state == BehaviorState.TROT: + controller.foot_locations, controller.contact_modes = step( + controller.ticks, + controller.foot_locations, + controller.swing_params, + controller.stance_params, + controller.gait_params, + controller.movement_reference, + ) - # Apply the desired body rotation - # TODO: See https://github.com/Nate711/PupperPythonSim/issues/2 - rotated_foot_locations = ( - euler2mat( - controller.movement_reference.roll, controller.movement_reference.pitch, 0.0 + # Apply the desired body rotation + # foot_locations = ( + # euler2mat( + # controller.movement_reference.roll, controller.movement_reference.pitch, 0.0 + # ) + # @ controller.foot_locations + # ) + # Disable joystick-based pitch and roll for trotting with IMU feedback + foot_locations = controller.foot_locations + + # Construct foot rotation matrix to compensate for body tilt + (roll, pitch, yaw) = quat2euler(quat_orientation) + correction_factor = 0.8 + max_tilt = 0.4 + roll_compensation = correction_factor * np.clip(roll, -max_tilt, max_tilt) + pitch_compensation = correction_factor * np.clip(pitch, -max_tilt, max_tilt) + rmat = euler2mat(roll_compensation, pitch_compensation, 0) + + foot_locations = rmat.T @ foot_locations + + controller.joint_angles = four_legs_inverse_kinematics( + foot_locations, robot_config + ) + + elif controller.state == BehaviorState.HOP: + hop_foot_locations = ( + controller.stance_params.default_stance + + np.array([0, 0, -0.09])[:, np.newaxis] + ) + controller.joint_angles = four_legs_inverse_kinematics( + hop_foot_locations, robot_config ) - @ controller.foot_locations - ) + elif controller.state == BehaviorState.FINISHHOP: + hop_foot_locations = ( + controller.stance_params.default_stance + + np.array([0, 0, -.22])[:, np.newaxis] + ) + controller.joint_angles = four_legs_inverse_kinematics( + hop_foot_locations, robot_config + ) + + + elif controller.state == BehaviorState.REST: + if controller.previous_state != BehaviorState.REST: + controller.smoothed_yaw = 0 + + yaw_factor = -0.25 + 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) + # Set the foot locations to the default stance plus the standard height + controller.foot_locations = ( + controller.stance_params.default_stance + + np.array([0, 0, controller.movement_reference.z_ref])[:, np.newaxis] + ) + # Apply the desired body rotation + rotated_foot_locations = ( + euler2mat( + controller.movement_reference.roll, controller.movement_reference.pitch, controller.smoothed_yaw + ) + @ controller.foot_locations + ) + controller.joint_angles = four_legs_inverse_kinematics( + rotated_foot_locations, robot_config + ) + + controller.ticks += 1 + controller.previous_state = controller.state + + +def set_pose_to_default(controller, robot_config): + controller.foot_locations = ( + controller.stance_params.default_stance + + np.array([0, 0, controller.movement_reference.z_ref])[:, np.newaxis] + ) controller.joint_angles = four_legs_inverse_kinematics( - rotated_foot_locations, controller.robot_config + controller.foot_locations, robot_config ) - controller.ticks += 1 \ No newline at end of file diff --git a/src/Gaits.py b/src/Gaits.py index ed5a759..45570b1 100644 --- a/src/Gaits.py +++ b/src/Gaits.py @@ -1,4 +1,18 @@ def phase_index(ticks, gaitparams): + """Calculates which part of the gait cycle the robot should be in given the time in ticks. + + Parameters + ---------- + ticks : int + Number of timesteps since the program started + gaitparams : GaitParams + GaitParams object + + Returns + ------- + Int + The index of the gait phase that the robot should be in. + """ phase_time = ticks % gaitparams.phase_length phase_sum = 0 for i in range(gaitparams.num_phases): @@ -9,6 +23,20 @@ def phase_index(ticks, gaitparams): def subphase_time(ticks, gaitparams): + """Calculates the number of ticks (timesteps) since the start of the current phase. + + Parameters + ---------- + ticks : Int + Number of timesteps since the program started + gaitparams : GaitParams + GaitParams object + + Returns + ------- + Int + Number of ticks since the start of the current phase. + """ phase_time = ticks % gaitparams.phase_length phase_sum = 0 subphase_t = 0 @@ -21,4 +49,18 @@ def subphase_time(ticks, gaitparams): def contacts(ticks, gaitparams): + """Calculates which feet should be in contact at the given number of ticks + + Parameters + ---------- + ticks : Int + Number of timesteps since the program started. + gaitparams : GaitParams + GaitParams object + + Returns + ------- + numpy array (4,) + Numpy vector with 0 indicating flight and 1 indicating stance. + """ return gaitparams.contact_phases[:, phase_index(ticks, gaitparams)] diff --git a/src/HardwareInterface.py b/src/HardwareInterface.py index 3e95a7d..1897229 100644 --- a/src/HardwareInterface.py +++ b/src/HardwareInterface.py @@ -2,10 +2,42 @@ def pwm_to_duty_cycle(pulsewidth_micros, pwm_params): + """Converts a pwm signal (measured in microseconds) to a corresponding duty cycle on the gpio pwm pin + + Parameters + ---------- + pulsewidth_micros : float + Width of the pwm signal in microseconds + pwm_params : PWMParams + PWMParams object + + Returns + ------- + float + PWM duty cycle corresponding to the pulse width + """ return int(pulsewidth_micros / 1e6 * pwm_params.freq * pwm_params.range) def angle_to_pwm(angle, servo_params, axis_index, leg_index): + """Converts a desired servo angle into the corresponding PWM command + + Parameters + ---------- + angle : float + Desired servo angle, relative to the vertical (z) axis + servo_params : ServoParams + ServoParams object + axis_index : int + Specifies which joint of leg to control. 0 is abduction servo, 1 is inner hip servo, 2 is outer hip servo. + leg_index : int + Specifies which leg to control. 0 is front-right, 1 is front-left, 2 is back-right, 3 is back-left. + + Returns + ------- + float + PWM width in microseconds + """ angle_deviation = ( angle - servo_params.neutral_angles[axis_index, leg_index] ) * servo_params.servo_multipliers[axis_index, leg_index] @@ -47,3 +79,9 @@ def send_servo_commands(pi, pwm_params, servo_params, joint_angles): def send_servo_command(pi, pwm_params, servo_params, joint_angle, axis, leg): duty_cycle = angle_to_duty_cycle(joint_angle, pwm_params, servo_params, axis, leg) pi.set_PWM_dutycycle(pwm_params.pins[axis, leg], duty_cycle) + + +def deactivate_servos(pi, pwm_params): + for leg_index in range(4): + for axis_index in range(3): + pi.set_PWM_dutycycle(pwm_params.pins[axis_index, leg_index], 0) diff --git a/src/IMU.py b/src/IMU.py new file mode 100644 index 0000000..9af5cb4 --- /dev/null +++ b/src/IMU.py @@ -0,0 +1,45 @@ +import serial +import numpy as np +import time + + +class IMU: + def __init__(self, port, baudrate=500000): + self.serial_handle = serial.Serial( + port=port, + baudrate=baudrate, + parity=serial.PARITY_NONE, + stopbits=serial.STOPBITS_ONE, + bytesize=serial.EIGHTBITS, + timeout=0, + ) + self.last_quat = np.array([1, 0, 0, 0]) + self.start_time = time.time() + + def flush_buffer(self): + self.serial_handle.reset_input_buffer() + + def read_orientation(self): + """Reads quaternion measurements from the Teensy until none are left. Returns the last read quaternion. + + Parameters + ---------- + serial_handle : Serial object + Handle to the pyserial Serial object + + Returns + ------- + np array (4,) + If there was quaternion data to read on the serial port returns the quaternion as a numpy array, otherwise returns the last read quaternion. + """ + + while True: + x = self.serial_handle.readline().decode("utf").strip() + if x is "" or x is None: + return self.last_quat + else: + parsed = x.split(",") + if len(parsed) == 4: + self.last_quat = np.array(parsed, dtype=np.float64) + else: + print("Did not receive 4-vector from imu") diff --git a/src/Kinematics.py b/src/Kinematics.py index 5ae56fc..8ea3e09 100644 --- a/src/Kinematics.py +++ b/src/Kinematics.py @@ -2,40 +2,6 @@ from transforms3d.euler import euler2mat -def _assert_valid_leg(i): - """Assert that the given leg index is valid - - Parameters - ---------- - i : int - Leg index. - """ - assert i in [0, 1, 2, 3] - - -def leg_forward_kinematics(alpha, leg_index, config): - """Find the body-centric coordinates of a given foot given the joint angles. - - Parameters - ---------- - alpha : Numpy array (3) - Joint angles ordered as (abduction, hip, knee) - leg_index : int - Leg index. - config : Config object - Robot parameters object - - Returns - ------- - Numpy array (3) - Body-centric coordinates of the specified foot - """ - _assert_valid_leg(leg_index) - y = config.ABDUCTION_OFFSET[leg_index] - unrotated_leg = np.array([0, y, -config.LEG_L + alpha[2]]) - return euler2mat(alpha[0], alpha[1], "sxyz") * unrotated_leg - - def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config): """Find the joint angles corresponding to the given body-relative foot position for a given leg and configuration @@ -53,7 +19,6 @@ def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config): numpy array (3) Array of corresponding joint angles. """ - _assert_valid_leg(leg_index) (x, y, z) = r_body_foot # Distance from the leg origin to the foot, projected into the y-z plane @@ -67,7 +32,9 @@ def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config): # Interior angle of the right triangle formed in the y-z plane by the leg that is coincident to the ab/adduction axis # For feet 2 (front left) and 4 (back left), the abduction offset is positive, for the right feet, the abduction offset is negative. - phi = np.arccos(config.ABDUCTION_OFFSETS[leg_index] / R_body_foot_yz) + arccos_argument = config.ABDUCTION_OFFSETS[leg_index] / R_body_foot_yz + arccos_argument = np.clip(arccos_argument, -0.99, 0.99) + phi = np.arccos(arccos_argument) # Angle of the y-z projection of the hip-to-foot vector, relative to the positive y-axis hip_foot_angle = np.arctan2(z, y) @@ -82,19 +49,17 @@ def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config): R_hip_foot = (R_hip_foot_yz ** 2 + x ** 2) ** 0.5 # Angle between the line going from hip to foot and the link L1 - trident = np.arccos( - (config.LEG_L1 ** 2 + R_hip_foot ** 2 - config.LEG_L2 ** 2) - / (2 * config.LEG_L1 * R_hip_foot) - ) + arccos_argument = (config.LEG_L1 ** 2 + R_hip_foot ** 2 - config.LEG_L2 ** 2) / (2 * config.LEG_L1 * R_hip_foot) + arccos_argument = np.clip(arccos_argument, -0.99, 0.99) + trident = np.arccos(arccos_argument) # Angle of the first link relative to the tilted negative z axis hip_angle = theta + trident # Angle between the leg links L1 and L2 - beta = np.arccos( - (config.LEG_L1 ** 2 + config.LEG_L2 ** 2 - R_hip_foot ** 2) - / (2 * config.LEG_L1 * config.LEG_L2) - ) + arccos_argument = (config.LEG_L1 ** 2 + config.LEG_L2 ** 2 - R_hip_foot ** 2) / (2 * config.LEG_L1 * config.LEG_L2) + arccos_argument = np.clip(arccos_argument, -0.99, 0.99) + beta = np.arccos(arccos_argument) # Angle of the second link relative to the tilted negative z axis knee_angle = hip_angle - (np.pi - beta) diff --git a/src/PupperConfig.py b/src/PupperConfig.py index b99cb05..4b8b09f 100644 --- a/src/PupperConfig.py +++ b/src/PupperConfig.py @@ -1,7 +1,14 @@ import numpy as np from scipy.linalg import solve from src.RobotConfig import MICROS_PER_RAD, NEUTRAL_ANGLE_DEGREES +from enum import Enum +class UserInputParams: + def __init__(self): + self.max_x_velocity = 0.5 + self.max_y_velocity = 0.24 + self.max_yaw_rate = 0.2 + self.max_pitch = 30.0 * np.pi / 180.0 class PWMParams: def __init__(self): @@ -27,6 +34,13 @@ def neutral_angles(self): return self.neutral_angle_degrees * np.pi / 180.0 # Convert to radians +class BehaviorState(Enum): + REST = 0 + TROT = 1 + HOP = 2 + FINISHHOP = 3 + + class MovementCommand: """Stores movement command """ @@ -55,17 +69,25 @@ class StanceParams: def __init__(self): self.z_time_constant = 0.02 - self.z_speed = 0.02 # maximum speed [m/s] - self.pitch_time_constant = 0.5 - self.roll_speed = 0.12 # maximum roll rate [rad/s] + self.z_speed = 0.03 # maximum speed [m/s] + self.pitch_deadband = 0.02 + self.pitch_time_constant = 0.25 + self.max_pitch_rate = 0.15 + self.roll_speed = 0.16 # maximum roll rate [rad/s] self.delta_x = 0.1 - self.delta_y = 0.07 + self.delta_y = 0.10 + self.x_shift = -0.01 @property def default_stance(self): return np.array( [ - [self.delta_x, self.delta_x, -self.delta_x, -self.delta_x], + [ + self.delta_x + self.x_shift, + self.delta_x + self.x_shift, + -self.delta_x + self.x_shift, + -self.delta_x + self.x_shift, + ], [-self.delta_y, self.delta_y, -self.delta_y, self.delta_y], [0, 0, 0, 0], ] @@ -78,13 +100,13 @@ class SwingParams: def __init__(self): self.z_coeffs = None - self.z_clearance = 0.01 + self.z_clearance = 0.05 self.alpha = ( - 0.5 - ) # Ratio between touchdown distance and total horizontal stance movement + 0.5 # Ratio between touchdown distance and total horizontal stance movement + ) self.beta = ( - 0.5 - ) # Ratio between touchdown distance and total horizontal stance movement + 0.5 # Ratio between touchdown distance and total horizontal stance movement + ) @property def z_clearance(self): @@ -114,14 +136,17 @@ def __init__(self): self.dt = 0.01 self.num_phases = 4 self.contact_phases = np.array( - [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]] + [[1, 1, 1, 0], + [1, 0, 1, 1], + [1, 0, 1, 1], + [1, 1, 1, 0]] ) self.overlap_time = ( - 0.35 - ) # duration of the phase where all four feet are on the ground + 0.10 # duration of the phase where all four feet are on the ground + ) self.swing_time = ( - 0.1 - ) # duration of the phase when only two feet are on the ground + 0.20 # duration of the phase when only two feet are on the ground + ) @property def overlap_ticks(self): @@ -157,11 +182,10 @@ def __init__(self): # Robot geometry self.LEG_FB = 0.10 # front-back distance from center line to leg axis - self.LEG_LR = 0.04 #0.0419 # left-right distance from center line to leg plane - self.LEG_L = 0.125 - self.LEG_L2 = 0.125 + self.LEG_LR = 0.04 # left-right distance from center line to leg plane + self.LEG_L2 = 0.115 self.LEG_L1 = 0.1235 - self.ABDUCTION_OFFSET = 0.03 #0.027 # distance from abduction axis to leg + self.ABDUCTION_OFFSET = 0.03 # distance from abduction axis to leg self.FOOT_RADIUS = 0.01 self.HIP_L = 0.0394 @@ -208,7 +232,7 @@ def __init__(self): leg_z = 1e-6 leg_mass = 0.010 - leg_x = 1 / 12 * self.LEG_L ** 2 * leg_mass + leg_x = 1 / 12 * self.LEG_L1 ** 2 * leg_mass leg_y = leg_x self.LEG_INERTIA = (leg_x, leg_y, leg_z) diff --git a/src/RobotConfig.py b/src/RobotConfig.py index a4260e9..44a11a5 100644 --- a/src/RobotConfig.py +++ b/src/RobotConfig.py @@ -5,4 +5,7 @@ MICROS_PER_RAD = 10.0 * 180.0 / np.pi # Must be calibrated -NEUTRAL_ANGLE_DEGREES = np.array([[5, 3.5, -3, 12], [65.5, 47, 34, 53.5], [-29, -20, -35, -43]]) \ No newline at end of file +NEUTRAL_ANGLE_DEGREES = np.array( + [[5, 3.5, -3, 12], [65.5, 47, 34, 53.5], [-29, -20, -35, -43]] +) + diff --git a/src/StanceController.py b/src/StanceController.py index 83284c6..f17e62f 100644 --- a/src/StanceController.py +++ b/src/StanceController.py @@ -62,7 +62,4 @@ def stance_foot_location( ) incremented_location = delta_R @ stance_foot_location + delta_p - # rotated_locations = euler2mat(movement_reference.roll, movement_reference.pitch, 0.0) @ incremented_location - # print(incremented_location, rotated_locations) - return incremented_location diff --git a/src/Tests.py b/src/Tests.py deleted file mode 100644 index 83a5afb..0000000 --- a/src/Tests.py +++ /dev/null @@ -1,311 +0,0 @@ -# using LinearAlgebra -# using Profile -# using StaticArrays -# using Plots -# using BenchmarkTools - -# include("Kinematics.jl") -# include("PupperConfig.jl") -# include("Gait.jl") -# include("StanceController.jl") -# include("SwingLegController.jl") -# include("Types.jl") -# include("Controller.jl") - -import numpy as np -import matplotlib.pyplot as plt - -from Kinematics import leg_explicit_inverse_kinematics -from PupperConfig import * -from Gaits import * -from StanceController import position_delta, stance_foot_location -from SwingLegController import * -from Types import MovementReference, GaitParams, StanceParams, SwingParams -from Controller import * - -# function round_(a, dec) -# return map(x -> round(x, digits=dec), a) -# end - -# function testInverseKinematicsExplicit!() -# println("\n-------------- Testing Inverse Kinematics -----------") -# config = PupperConfig() -# println("\nTesting Inverse Kinematics") -# function testHelper(r, alpha_true, i; do_assert=true) -# eps = 1e-6 -# @time α = leg_explicitinversekinematics_prismatic(r, i, config) -# println("Leg ", i, ": r: ", r, " -> α: ", α) -# if do_assert -# @assert norm(α - alpha_true) < eps -# end -# end - -# c = config.LEG_L/sqrt(2) -# offset = config.ABDUCTION_OFFSET -# testHelper(SVector(0, offset, -0.125), SVector(0, 0, 0), 2) -# testHelper(SVector(c, offset, -c), SVector(0, -pi/4, 0), 2) -# testHelper(SVector(-c, offset, -c), SVector(0, pi/4, 0), 2) -# testHelper(SVector(0, c, -c), missing, 2, do_assert=false) - -# testHelper(SVector(-c, -offset, -c), [0, pi/4, 0], 1) -# testHelper(SVector(config.LEG_L * sqrt(3)/2, offset, -config.LEG_L / 2), SVector(0, -pi/3, 0), 2) -# end - - -def test_inverse_kinematics_linkage(): - print("\n-------------- Testing Five-bar Linkage Inverse Kinematics -----------") - config = PupperConfig() - print("\nTesting Inverse Kinematics") - - def testHelper(r, alpha_true, i, do_assert=True): - eps = 1e-6 - alpha = leg_explicit_inverse_kinematics(r, i, config) - print("Leg ", i, ": r: ", r, " -> α: ", alpha) - if do_assert: - assert np.linalg.norm(alpha - alpha_true) < eps - - c = config.LEG_L / (2 ** 0.5) - offset = config.ABDUCTION_OFFSET - testHelper(np.array([0, offset, -0.125]), None, 1, do_assert=False) - testHelper(np.array([c, offset, -c]), None, 1, do_assert=False) - testHelper(np.array([-c, offset, -c]), None, 1, do_assert=False) - testHelper(np.array([0, c, -c]), None, 1, do_assert=False) - - testHelper(np.array([-c, -offset, -c]), None, 0, do_assert=False) - testHelper( - np.array([config.LEG_L * (3 ** 0.5) / 2, offset, -config.LEG_L / 2]), - None, - 1, - do_assert=False, - ) - - -# function testForwardKinematics!() -# println("\n-------------- Testing Forward Kinematics -----------") -# config = PupperConfig() -# println("\nTesting Forward Kinematics") -# function testHelper(alpha, r_true, i; do_assert=true) -# eps = 1e-6 -# r = zeros(3) -# println("Vectors") -# a = [alpha.data...] -# @time legForwardKinematics!(r, a, i, config) -# println("SVectors") -# @time r = legForwardKinematics(alpha, i, config) -# println("Leg ", i, ": α: ", alpha, " -> r: ", r) -# if do_assert -# @assert norm(r_true - r) < eps -# end -# end - -# l = config.LEG_L -# offset = config.ABDUCTION_OFFSET -# testHelper(SVector{3}([0.0, 0.0, 0.0]), SVector{3}([0, offset, -l]), 2) -# testHelper(SVector{3}([0.0, pi/4, 0.0]), missing, 2, do_assert=false) -# # testHelper([0.0, 0.0, 0.0], [0, offset, -l], 2) -# # testHelper([0.0, pi/4, 0.0], missing, 2, do_assert=false) -# end - -# function testForwardInverseAgreeance() -# println("\n-------------- Testing Forward/Inverse Consistency -----------") -# config = PupperConfig() -# println("\nTest forward/inverse consistency") -# eps = 1e-6 -# for i in 1:10 -# alpha = SVector(rand()-0.5, rand()-0.5, (rand()-0.5)*0.05) -# leg = rand(1:4) -# @time r = legForwardKinematics(alpha, leg, config) -# # @code_warntype legForwardKinematics!(r, alpha, leg, config) -# @time alpha_prime = leg_explicitinversekinematics_prismatic(r, leg, config) -# # @code_warntype inverseKinematicsExplicit!(alpha_prime, r, leg, config) -# println("Leg ", leg, ": α: ", round_(alpha, 3), " -> r_body_foot: ", round_(r, 3), " -> α': ", round_(alpha_prime, 3)) -# @assert norm(alpha_prime - alpha) < eps -# end -# end - -# function testAllInverseKinematics() -# println("\n-------------- Testing Four Leg Inverse Kinematics -----------") -# function helper(r_body, alpha_true; do_assert=true) -# println("Timing for fourlegs_inversekinematics") -# config = PupperConfig() -# @time alpha = fourlegs_inversekinematics(SMatrix(r_body), config) -# @code_warntype fourlegs_inversekinematics(SMatrix(r_body), config) -# println("r: ", r_body, " -> α: ", alpha) - -# if do_assert -# @assert norm(alpha - alpha_true) < 1e-10 -# end -# end -# config = PupperConfig() -# f = config.LEG_FB -# l = config.LEG_LR -# s = -0.125 -# o = config.ABDUCTION_OFFSET -# r_body = MMatrix{3,4}(zeros(3,4)) -# r_body[:,1] = [f, -l-o, s] -# r_body[:,2] = [f, l+o, s] -# r_body[:,3] = [-f, -l-o, s] -# r_body[:,4] = [-f, l+o, s] - -# helper(r_body, zeros(3,4)) -# helper(SMatrix{3,4}(zeros(3,4)), missing, do_assert=false) -# end - -# function testKinematics() -# testInverseKinematicsExplicit!() -# testForwardKinematics!() -# testForwardInverseAgreeance() -# testAllInverseKinematics() -# end - -# function testGait() -# println("\n-------------- Testing Gait -----------") -# p = GaitParams() -# # println("Gait params=",p) -# t = 680 -# println("Timing for phaseindex") -# @time ph = phaseindex(t, p) -# # @code_warntype phaseindex(t, p) -# println("t=",t," phase=",ph) -# @assert ph == 4 -# @assert phaseindex(0, p) == 1 - -# println("Timing for contacts") -# @time c = contacts(t, p) -# # @code_warntype contacts(t, p) -# @assert typeof(c) == SArray{Tuple{4},Int64,1,4} -# println("t=", t, " contacts=", c) -# end - - -def test_stance_controller(): - print("\n-------------- Testing Stance Controller -----------") - stanceparams = StanceParams() - gaitparams = GaitParams() - - zmeas = -0.20 - mvref = MovementReference() - dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) - assert np.linalg.norm(dR - np.eye(3)) < 1e-10 - assert np.linalg.norm(dp - np.array([0, 0, gaitparams.dt * 0.04])) < 1e-10 - - zmeas = -0.18 - mvref = MovementReference() - mvref.v_xy_ref = np.array([1.0, 0.0]) - mvref.z_ref = -0.18 - dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) - - zmeas = -0.20 - mvref = MovementReference() - mvref.wz_ref = 1.0 - mvref.z_ref = -0.20 - dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) - assert np.linalg.norm(dp - np.array([0, 0, 0])) < 1e-10 - assert np.linalg.norm(dR[0, 1] - (gaitparams.dt)) < 1e-6 - - stancefootloc = np.zeros(3) - sloc = stance_foot_location(stancefootloc, stanceparams, gaitparams, mvref) - - -# function typeswinglegcontroller() -# println("\n--------------- Code warn type for raibert_tdlocation[s] ----------") -# swp = SwingParams() -# stp = StanceParams() -# gp = GaitParams() -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# raibert_tdlocations(swp, stp, gp, mvref) - -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# raibert_tdlocation(1, swp, stp, gp, mvref) -# end - -# function TestSwingLegController() -# println("\n-------------- Testing Swing Leg Controller -----------") -# swp = SwingParams() -# stp = StanceParams() -# gp = GaitParams() -# p = ControllerParams() -# println("Timing for swingheight:") -# @time z = swingheight(0.5, swp) -# println("z clearance at t=1/2swingtime =>",z) -# @assert abs(z - swp.zclearance) < 1e-10 - -# println("Timing for swingheight:") -# @time z = swingheight(0, swp) -# println("Z clearance at t=0 =>",z) -# @assert abs(z) < 1e-10 - -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# println("Timing for raibert tdlocation*s*:") -# @time l = raibert_tdlocations(swp, stp, gp, mvref) -# target = stp.defaultstance .+ [gp.stanceticks*gp.dt*0.5*1, 0, 0] -# println("Touchdown locations =>", l, " ", target) -# @assert norm(l - target) <= 1e-10 - -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# println("Timing for raibert tdlocation:") -# @time l = raibert_tdlocation(1, swp, stp, gp, mvref) - -# fcurrent = SMatrix{3, 4, Float64}(stp.defaultstance) -# mvref = MovementReference() -# tswing = 0.125 -# println("Timing for swingfootlocation*s* increment") -# @time l = swingfootlocations(tswing, fcurrent, swp, stp, gp, mvref) -# println(l) - -# fcurrent = SVector{3, Float64}(0.0, 0.0, 0.0) -# println("Timing for swingfootlocation") -# @time swingfootlocation(tswing, fcurrent, 1, swp, stp, gp, mvref) - -# typeswinglegcontroller() -# return nothing -# end - - -def test_run(): - print("Run timing") - foot_loc_history, joint_angle_history = run() - plt.subplot(211) - x = plt.plot(foot_loc_history[0, :, :].T, label="x") - y = plt.plot(foot_loc_history[1, :, :].T, label="y") - z = plt.plot(foot_loc_history[2, :, :].T, label="z") - - plt.subplot(212) - alpha = plt.plot(joint_angle_history[0, :, :].T, label="alpha") - beta = plt.plot(joint_angle_history[1, :, :].T, label="beta") - gamma = plt.plot(joint_angle_history[2, :, :].T, label="gamma") - plt.show() - - # plot(x, β, y, α, z, γ, layout=(3,2), legend=false)) - - -# function teststep() -# swingparams = SwingParams() -# stanceparams = StanceParams() -# gaitparams = GaitParams() -# mvref = MovementReference(vxyref=SVector{2}(0.2, 0.0), wzref=0.0) -# conparams = ControllerParams() -# robotconfig = PupperConfig() - - -# footlocations::SMatrix{3, 4, Float64, 12} = stanceparams.defaultstance .+ SVector{3, Float64}(0, 0, mvref.zref) - -# ticks = 1 -# println("Timing for step!") -# @btime step($ticks, $footlocations, $swingparams, $stanceparams, $gaitparams, $mvref, $conparams) -# @code_warntype step(ticks, footlocations, swingparams, stanceparams, gaitparams, mvref, conparams) -# end - -# # testGait() -# # testKinematics() -# # TestStanceController() -# # testStaticArrays() -# # TestSwingLegController() -# test_inversekinematics_linkage() - -# # teststep() -# # testrun() - -test_inverse_kinematics_linkage() -test_stance_controller() -test_run() diff --git a/src/UserInput.py b/src/UserInput.py index 90d1980..6a210c5 100644 --- a/src/UserInput.py +++ b/src/UserInput.py @@ -1,41 +1,80 @@ import UDPComms import numpy as np - +import time +from src.PupperConfig import BehaviorState +from src.Utilities import deadband, clipped_first_order_filter class UserInputs: - def __init__(self, udp_port=8830): + def __init__(self, max_x_velocity, max_y_velocity, max_yaw_rate, max_pitch, udp_port=8830): + self.max_x_velocity = max_x_velocity + self.max_y_velocity = max_y_velocity + self.max_yaw_rate = max_yaw_rate + self.max_pitch = max_pitch + self.x_vel = 0.0 self.y_vel = 0.0 self.yaw_rate = 0.0 self.pitch = 0.0 + self.stance_movement = 0 self.roll_movement = 0 + self.gait_toggle = 0 - self.gait_mode = 0 self.previous_gait_toggle = 0 + self.gait_mode = 0 + + self.previous_state = BehaviorState.REST + self.current_state = BehaviorState.REST + + self.previous_hop_toggle = 0 + self.hop_toggle = 0 + self.hop_begin_time = 0 + + self.activate = 0 + self.last_activate = 0 + self.message_rate = 50 self.udp_handle = UDPComms.Subscriber(udp_port, timeout=0.3) -def get_input(user_input_obj): +def get_input(user_input_obj, do_print=False): try: msg = user_input_obj.udp_handle.get() - user_input_obj.x_vel = msg["y"] * 0.14 - user_input_obj.y_vel = msg["x"] * -0.14 - user_input_obj.yaw_rate = msg["twist"] * -0.8 - user_input_obj.pitch = msg["pitch"] * 30 * np.pi / 180.0 - user_input_obj.gait_toggle = msg["gait_toggle"] - user_input_obj.stance_movement = msg["stance_movement"] - user_input_obj.roll_movement = msg["roll_movement"] + user_input_obj.x_vel = msg["ly"] * 0.5 + user_input_obj.y_vel = msg["lx"] * -0.24 + user_input_obj.yaw_rate = msg["rx"] * -2.0 + user_input_obj.pitch = msg["ry"] * 40 * np.pi / 180.0 + user_input_obj.gait_toggle = msg["R1"] + user_input_obj.activate = msg["L1"] + user_input_obj.stance_movement = msg["dpady"] + user_input_obj.roll_movement = msg["dpadx"] user_input_obj.message_rate = msg["message_rate"] + user_input_obj.hop_toggle = msg["x"] + + # Check if requesting a state transition to trotting, or from trotting to resting + if user_input_obj.gait_toggle == 1 and user_input_obj.previous_gait_toggle == 0: + if user_input_obj.previous_state == BehaviorState.TROT: + user_input_obj.current_state = BehaviorState.REST + elif user_input_obj.previous_state == BehaviorState.REST: + user_input_obj.current_state = BehaviorState.TROT + + # Check if requesting a state transition to hopping, from trotting or resting + if user_input_obj.hop_toggle == 1 and user_input_obj.previous_hop_toggle == 0: + if user_input_obj.current_state == BehaviorState.HOP: + user_input_obj.current_state = BehaviorState.FINISHHOP + elif user_input_obj.current_state == BehaviorState.REST: + user_input_obj.current_state = BehaviorState.HOP + elif user_input_obj.current_state == BehaviorState.FINISHHOP: + user_input_obj.current_state = BehaviorState.REST - # Update gait mode - if user_input_obj.previous_gait_toggle == 0 and user_input_obj.gait_toggle == 1: - user_input_obj.gait_mode = not user_input_obj.gait_mode + # Update previous values for toggles and state + user_input_obj.previous_state = user_input_obj.current_state user_input_obj.previous_gait_toggle = user_input_obj.gait_toggle + user_input_obj.previous_hop_toggle = user_input_obj.hop_toggle except UDPComms.timeout: - print("UDP Timed out") + if do_print: + print("UDP Timed out") def update_controller(controller, user_input_obj): @@ -44,19 +83,19 @@ def update_controller(controller, user_input_obj): ) controller.movement_reference.wz_ref = user_input_obj.yaw_rate - message_dt = 1.0 / user_input_obj.message_rate - alpha = message_dt / controller.stance_params.pitch_time_constant - controller.movement_reference.pitch = controller.movement_reference.pitch * (1 - alpha) + user_input_obj.pitch * alpha + message_dt = 1.0 / user_input_obj.message_rate - if user_input_obj.gait_mode == 0: - controller.gait_params.contact_phases = np.array( - [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]] - ) - else: - controller.gait_params.contact_phases = np.array( - [[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]] - ) + # TODO: Put this filter code somewhere else + deadbanded_pitch = deadband(user_input_obj.pitch, controller.stance_params.pitch_deadband) + pitch_rate = clipped_first_order_filter(controller.movement_reference.pitch, deadbanded_pitch, controller.stance_params.max_pitch_rate, controller.stance_params.pitch_time_constant) + controller.movement_reference.pitch += message_dt * pitch_rate + + controller.state = user_input_obj.current_state # Note this is negative since it is the feet relative to the body - controller.movement_reference.z_ref -= controller.stance_params.z_speed * message_dt * user_input_obj.stance_movement - controller.movement_reference.roll += controller.stance_params.roll_speed * message_dt * user_input_obj.roll_movement + controller.movement_reference.z_ref -= ( + controller.stance_params.z_speed * message_dt * user_input_obj.stance_movement + ) + controller.movement_reference.roll += ( + controller.stance_params.roll_speed * message_dt * user_input_obj.roll_movement + ) diff --git a/src/Utilities.py b/src/Utilities.py new file mode 100644 index 0000000..a51149f --- /dev/null +++ b/src/Utilities.py @@ -0,0 +1,8 @@ +import numpy as np + +def deadband(value, band_radius): + return max(value - band_radius, 0) + min(value + band_radius, 0) + +def clipped_first_order_filter(input, target, max_rate, tau): + rate = (target - input) / tau + return np.clip(rate, -max_rate, max_rate) \ No newline at end of file diff --git a/src/pupper_out.xml b/src/pupper_out.xml index 346cea5..9cc7767 100644 --- a/src/pupper_out.xml +++ b/src/pupper_out.xml @@ -21,8 +21,8 @@ _ext indicates linear extension of the leg. Positive values = leg goes up - - + + @@ -38,8 +38,8 @@ _ext indicates linear extension of the leg. Positive values = leg goes up -