From 94f71214a06246cdcceaf39f37e5198e83161806 Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Sun, 1 Dec 2019 14:42:08 -0800 Subject: [PATCH 01/45] Update README.md --- README.md | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 95e0643..351aa61 100644 --- a/README.md +++ b/README.md @@ -42,7 +42,19 @@ This repository contains Python code to run Pupper, a Raspberry Pi-based quadrup - 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 +- SSH into the robot + ```shell + ssh pi@10.0.0.xx + ``` where xx is the local address you chose for the Pi +- Go into RW mode + ```shell + rw + ``` +- In a separate shell, start the joystick publisher. Instructions here: https://github.com/stanfordroboticsclub/PupperCommand/blob/master/README.md +- Go into this repo's PupperPythonSim directory + ```shell + cd PupperPythonSim + ``` - Start the PiGPIO daemon by executing in shell: ```shell sudo pigpiod From 6eebc6918b4ba7c99a7919064aeed66b22f3cf3c Mon Sep 17 00:00:00 2001 From: stuartbowers <35119713+stuartbowers@users.noreply.github.com> Date: Sun, 1 Dec 2019 14:47:13 -0800 Subject: [PATCH 02/45] Update README.md --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 351aa61..79c04fe 100644 --- a/README.md +++ b/README.md @@ -45,7 +45,8 @@ This repository contains Python code to run Pupper, a Raspberry Pi-based quadrup - SSH into the robot ```shell ssh pi@10.0.0.xx - ``` where xx is the local address you chose for the Pi + ``` + where xx is the local address you chose for the Pi - Go into RW mode ```shell rw From bacee07c96198ba1d586ec5eeb793fd70f7a1662 Mon Sep 17 00:00:00 2001 From: Stuart Bowers Date: Tue, 10 Dec 2019 03:16:00 +0000 Subject: [PATCH 03/45] Add ability to change stance height --- run_robot.py | 30 ++++++++++++++++++++++++++---- src/PupperConfig.py | 2 +- 2 files changed, 27 insertions(+), 5 deletions(-) diff --git a/run_robot.py b/run_robot.py index 05dfb50..c03fd36 100644 --- a/run_robot.py +++ b/run_robot.py @@ -42,6 +42,14 @@ def main(): last_loop = time.time() now = last_loop start = time.time() + + gait_mode = 0 # 0 for non-walking, 1 for walking + prev_gait_toggle = 0 + + non_walking_gait= np.array([[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]]) + walking_gait = np.array([[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]]) + + for i in range(60000): last_loop = time.time() step_controller(controller) @@ -49,20 +57,34 @@ def main(): try: msg = values.get() - print(msg) except UDPComms.timeout: - print("timout") - msg = {"x": 0, "y": 0, "twist": 0, "pitch": 0} + print("timeout") + msg = {"x": 0, "y": 0, "twist": 0, "pitch": 0, "gait_toggle": 0, "stance_movement": 0} x_vel = msg["y"] / 7.0 y_vel = -msg["x"] / 7.0 yaw_rate = -msg["twist"] * 0.8 + gait_toggle = msg["gait_toggle"] + stance_movement = msg["stance_movement"] + + # Check for gait toggle + if prev_gait_toggle == 0 and gait_toggle == 1: + gait_mode = not gait_mode + prev_gait_toggle = gait_toggle pitch = msg["pitch"] * 30.0 * np.pi / 180.0 - print(pitch) controller.movement_reference.v_xy_ref = np.array([x_vel, y_vel]) controller.movement_reference.wz_ref = yaw_rate controller.movement_reference.pitch = pitch + + if gait_mode == 0: + controller.gait_params.contact_phases = non_walking_gait + else: + controller.gait_params.contact_phases = walking_gait + + # Note this is negative since it is the feet relative to the body + controller.movement_reference.z_ref -= 0.001 * stance_movement + while now - last_loop < controller.gait_params.dt: now = time.time() # print("Time since last loop: ", now - last_loop) diff --git a/src/PupperConfig.py b/src/PupperConfig.py index 60532d2..4900126 100644 --- a/src/PupperConfig.py +++ b/src/PupperConfig.py @@ -17,7 +17,7 @@ def __init__(self): # The neutral angle of the joint relative to the modeled zero-angle in degrees, for each joint self.neutral_angle_degrees = np.array( - [[-12, -17, 6, -2], [49, 46, 47, 51], [-42, -31, -39, -33]] + [[9, 5, 13, -9], [45, 47, 44, 52], [-14, -36, -15, -35]] ) self.servo_multipliers = np.array( From c98574b41c73e65f20a136a5c2fb9a4aa1799f11 Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Tue, 10 Dec 2019 01:17:23 -0800 Subject: [PATCH 04/45] blacking --- run_robot.py | 18 +++++++++++++----- src/PupperConfig.py | 1 - 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/run_robot.py b/run_robot.py index c03fd36..2b48ce4 100644 --- a/run_robot.py +++ b/run_robot.py @@ -46,9 +46,10 @@ def main(): gait_mode = 0 # 0 for non-walking, 1 for walking prev_gait_toggle = 0 - non_walking_gait= np.array([[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]]) + non_walking_gait = np.array( + [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]] + ) walking_gait = np.array([[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]]) - for i in range(60000): last_loop = time.time() @@ -59,7 +60,14 @@ def main(): msg = values.get() except UDPComms.timeout: print("timeout") - msg = {"x": 0, "y": 0, "twist": 0, "pitch": 0, "gait_toggle": 0, "stance_movement": 0} + msg = { + "x": 0, + "y": 0, + "twist": 0, + "pitch": 0, + "gait_toggle": 0, + "stance_movement": 0, + } x_vel = msg["y"] / 7.0 y_vel = -msg["x"] / 7.0 yaw_rate = -msg["twist"] * 0.8 @@ -77,9 +85,9 @@ def main(): controller.movement_reference.wz_ref = yaw_rate controller.movement_reference.pitch = pitch - if gait_mode == 0: + if gait_mode == 0: controller.gait_params.contact_phases = non_walking_gait - else: + else: controller.gait_params.contact_phases = walking_gait # Note this is negative since it is the feet relative to the body diff --git a/src/PupperConfig.py b/src/PupperConfig.py index 4900126..22d48a9 100644 --- a/src/PupperConfig.py +++ b/src/PupperConfig.py @@ -113,7 +113,6 @@ def __init__(self): self.dt = 0.01 self.num_phases = 4 self.contact_phases = np.array( - # [[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]] [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]] ) self.overlap_time = ( From 11435dcc814b6128c45c6f738f87fb62d43e0454 Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Tue, 10 Dec 2019 01:18:02 -0800 Subject: [PATCH 05/45] remove adafruit pwm hat test --- run_robot_adafruit.py | 14 -------------- 1 file changed, 14 deletions(-) delete mode 100644 run_robot_adafruit.py diff --git a/run_robot_adafruit.py b/run_robot_adafruit.py deleted file mode 100644 index 381f807..0000000 --- a/run_robot_adafruit.py +++ /dev/null @@ -1,14 +0,0 @@ -import time -from adafruit_servokit import ServoKit - -kit = ServoKit(channels=16) -start = time.time() -n = 100 -for i in range(n): - for a in range(4, 16): - kit.servo[a].angle = i % 180 - # time.sleep(0.001) - if i % 10 == 0: - print(i) -end = time.time() -print("Seconds per 12-actuator update: ", (end - start) / 100.0) From 6bed0a82c17278c289ecab2a9f39b8d86dd59f7f Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Tue, 10 Dec 2019 01:27:20 -0800 Subject: [PATCH 06/45] cleanup calibration script --- calibrate_servos.py | 99 +++++++++++++++++++++------------------------ 1 file changed, 47 insertions(+), 52 deletions(-) diff --git a/calibrate_servos.py b/calibrate_servos.py index cb97b43..b919845 100644 --- a/calibrate_servos.py +++ b/calibrate_servos.py @@ -1,74 +1,84 @@ import pigpio -from src.HardwareInterface import ( - send_servo_commands, - initialize_pwm, - pwm_to_duty_cycle, - send_servo_command, -) +from src.HardwareInterface import initialize_pwm, send_servo_command from src.PupperConfig import PWMParams, ServoParams import numpy as np -def getMotorName(i, j): +def get_motor_name(i, j): motor_type = {0: "Abduction", 1: "Inner", 2: "Outer"} # Top # Bottom leg_pos = {0: "Front Right", 1: "Front Left", 2: "Back Right", 3: "Back Left"} final_name = motor_type[i] + " " + leg_pos[j] return final_name -def getMotorSetPoint(i, j): +def get_motor_setpoint(i, j): data = [[0, 0, 0, 0], [45, 45, 45, 45], [45, 45, 45, 45]] return data[i][j] -def getUserInput(request): - measured_angle = float(input(request)) - return measured_angle - - -def degreesToRadians(input_array): +def degrees_to_radians(input_array): + """Converts degrees to radians. + + Parameters + ---------- + input_array : Numpy array or float + Degrees + + Returns + ------- + Numpy array or float + Radians + """ return np.pi / 180.0 * input_array -def stepUntil(servo_params, pi_board, pwm_params, kValue, i_index, j_index, set_point): - # returns the (program_angle) once the real angle matches the pre-defined set point - foundPosition = False - set_names = ["horizontal", "horizontal", "vertical"] +def step_until(servo_params, pi_board, pwm_params, kValue, i_index, j_index, set_point): + """Returns the angle offset needed to correct a given link by asking the user for input. + Returns + ------- + Float + Angle offset needed to correct the link. + """ + found_position = False + set_names = ["horizontal", "horizontal", "vertical"] offset = 0 - - while not foundPosition: - aboveOrBelow = str( - input("Is the leg 'above' or 'below' " + set_names[i_index] + "? Input: ") + while not found_position: + above_or_below = str( + input( + "Desired position: " + + set_names[i_index] + + ". Enter 'a' or 'b' to raise or lower the link. Enter 'd' when done. Input: " + ) ) - if aboveOrBelow == "above" or aboveOrBelow == "a": + if above_or_below == "above" or above_or_below == "a": offset += 1.0 send_servo_command( pi_board, pwm_params, servo_params, - degreesToRadians(set_point + offset), + degrees_to_radians(set_point + offset), i_index, j_index, ) - elif aboveOrBelow == "below" or aboveOrBelow == "b": + elif above_or_below == "below" or above_or_below == "b": offset -= 1.0 send_servo_command( pi_board, pwm_params, servo_params, - degreesToRadians(set_point + offset), + degrees_to_radians(set_point + offset), i_index, j_index, ) - elif aboveOrBelow == "done" or aboveOrBelow == "d": - foundPosition = True + elif above_or_below == "done" or above_or_below == "d": + found_position = True print("offset: ", offset, " original: ", set_point) return offset -def calibrateB(servo_params, pi_board, pwm_params): +def calibrate_b(servo_params, pi_board, pwm_params): """Calibrate the angle offset for the twelve motors on the robot. Note that servo_params is modified in-place. Parameters ---------- @@ -81,8 +91,8 @@ def calibrateB(servo_params, pi_board, pwm_params): """ # Found K value of (11.4) - kValue = getUserInput( - "Please provide a K value (microseconds per degree) for your servos: " + kValue = float( + input("Please provide a K value (microseconds per degree) for your servos: ") ) servo_params.micros_per_rad = kValue * 180 / np.pi @@ -93,22 +103,22 @@ def calibrateB(servo_params, pi_board, pwm_params): # Loop until we're satisfied with the calibration completed = False while not completed: - motor_name = getMotorName(i, j) + motor_name = get_motor_name(i, j) print("Currently calibrating " + motor_name + "...") - set_point = getMotorSetPoint(i, j) + set_point = get_motor_setpoint(i, j) # Move servo to set_point angle send_servo_command( pi_board, pwm_params, servo_params, - degreesToRadians(set_point), + degrees_to_radians(set_point), i, j, ) # Adjust the angle using keyboard input until it matches the reference angle - offset = stepUntil( + offset = step_until( servo_params, pi_board, pwm_params, kValue, i, j, set_point ) print("Final offset: ", offset) @@ -125,7 +135,7 @@ def calibrateB(servo_params, pi_board, pwm_params): pi_board, pwm_params, servo_params, - degreesToRadians([0, 45, -45][i]), + degrees_to_radians([0, 45, -45][i]), i, j, ) @@ -136,9 +146,6 @@ def calibrateB(servo_params, pi_board, pwm_params): ) completed = okay == "yes" - # (real_angle) = s*(program_angle) - (beta) - # (beta) = s*(program_angle) - (real_angle) - def main(): """Main program @@ -148,21 +155,9 @@ def main(): servo_params = ServoParams() initialize_pwm(pi_board, pwm_params) - calibrateB(servo_params, pi_board, pwm_params) + calibrate_b(servo_params, pi_board, pwm_params) print("Calibrated neutral angles:") print(servo_params.neutral_angle_degrees) - """ - servo_params.neutral_angle_degrees = np.array( - [[8, 3, 0, 0], [45, 48, 45, 45], [-50, -38, -45, -45]] - ) - - ref_position = np.pi/180.0 * np.array([[0, 0, 0, 0], [0, 0, 45, 45], [-45,-45, -45, -45]]) - send_servo_commands(pi_board, pwm_params, servo_params, ref_position) - """ main() - -# self.servo_multipliers = np.array( -# [[1, 1, 1, 1], [-1, 1, 1, -1], [1, -1, 1, -1]] -# ) From 0856a3f63796571e0fcb718c339ae3473f6cc2e7 Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Tue, 10 Dec 2019 01:28:43 -0800 Subject: [PATCH 07/45] fix typo in calibration --- calibrate_servos.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrate_servos.py b/calibrate_servos.py index b919845..5c6e7ac 100644 --- a/calibrate_servos.py +++ b/calibrate_servos.py @@ -142,7 +142,7 @@ def calibrate_b(servo_params, pi_board, pwm_params): okay = "" while okay not in ["yes", "no"]: okay = str( - input("Check angle. Are you satisfied? Enter 'yes' or 'no']") + input("Check angle. Are you satisfied? Enter 'yes' or 'no': ") ) completed = okay == "yes" From 25d6b8ae954e8fef9f1dfc0484f615a25675a33f Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Tue, 10 Dec 2019 13:42:29 -0800 Subject: [PATCH 08/45] moved the joystick parsing code into UserInput.py --- run_robot.py | 68 ++++++++++++--------------------------------- src/Controller.py | 29 ++----------------- src/PupperConfig.py | 1 - src/UserInput.py | 54 +++++++++++++++++++++++++++++++++++ 4 files changed, 73 insertions(+), 79 deletions(-) create mode 100644 src/UserInput.py diff --git a/run_robot.py b/run_robot.py index 2b48ce4..ffe6103 100644 --- a/run_robot.py +++ b/run_robot.py @@ -1,4 +1,7 @@ import pigpio +import numpy as np +import UDPComms +import time from src.Controller import step_controller, Controller from src.HardwareInterface import send_servo_commands, initialize_pwm from src.PupperConfig import ( @@ -9,9 +12,7 @@ ServoParams, PWMParams, ) -import time -import numpy as np -import UDPComms +from src.UserInput import UserInputs, get_input, update_controller def main(): @@ -19,6 +20,8 @@ def main(): """ pi_board = pigpio.pi() pwm_params = PWMParams() + initialize_pwm(pi_board, pwm_params) + servo_params = ServoParams() controller = Controller() @@ -34,68 +37,31 @@ def main(): controller.stance_params = StanceParams() controller.stance_params.delta_y = 0.08 controller.gait_params = GaitParams() - controller.gait_params.dt = 0.01 - initialize_pwm(pi_board, pwm_params) + user_input = UserInputs() - values = UDPComms.Subscriber(8830, timeout=0.3) last_loop = time.time() now = last_loop start = time.time() - gait_mode = 0 # 0 for non-walking, 1 for walking - prev_gait_toggle = 0 - - non_walking_gait = np.array( - [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]] - ) - walking_gait = np.array([[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]]) - for i in range(60000): last_loop = time.time() - step_controller(controller) - send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) - - try: - msg = values.get() - except UDPComms.timeout: - print("timeout") - msg = { - "x": 0, - "y": 0, - "twist": 0, - "pitch": 0, - "gait_toggle": 0, - "stance_movement": 0, - } - x_vel = msg["y"] / 7.0 - y_vel = -msg["x"] / 7.0 - yaw_rate = -msg["twist"] * 0.8 - gait_toggle = msg["gait_toggle"] - stance_movement = msg["stance_movement"] - - # Check for gait toggle - if prev_gait_toggle == 0 and gait_toggle == 1: - gait_mode = not gait_mode - prev_gait_toggle = gait_toggle + + # Parse the udp joystick commands and then update the robot controller's parameters + get_input(user_input) + update_controller(controller, user_input) - pitch = msg["pitch"] * 30.0 * np.pi / 180.0 - - controller.movement_reference.v_xy_ref = np.array([x_vel, y_vel]) - controller.movement_reference.wz_ref = yaw_rate - controller.movement_reference.pitch = pitch - - if gait_mode == 0: - controller.gait_params.contact_phases = non_walking_gait - else: - controller.gait_params.contact_phases = walking_gait + # Step the controller forward by dt + step_controller(controller) - # Note this is negative since it is the feet relative to the body - controller.movement_reference.z_ref -= 0.001 * stance_movement + # Update the pwm widths going to the servos + send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) + # Wait until it's time to execute again while now - last_loop < controller.gait_params.dt: now = time.time() # print("Time since last loop: ", now - last_loop) + end = time.time() print("seconds per loop: ", (end - start) / 1000.0) diff --git a/src/Controller.py b/src/Controller.py index 10e70c2..4992a9b 100644 --- a/src/Controller.py +++ b/src/Controller.py @@ -22,6 +22,7 @@ def __init__(self): self.ticks = 0 + # Set default for foot locations and joint angles self.foot_locations = ( self.stance_params.default_stance + np.array([0, 0, self.movement_reference.z_ref])[:, np.newaxis] @@ -111,30 +112,4 @@ def step_controller(controller): controller.joint_angles = four_legs_inverse_kinematics( rotated_foot_locations, controller.robot_config ) - controller.ticks += 1 - - -def run(): - """Testing function that runs the robot for one second. - - Returns - ------- - (Numpy array (3, 4, timesteps), Numpy array (3, 4, timesteps)) - (history of foot locations, history of joint angles) - """ - c = Controller() - c.movement_reference.v_xy_ref = np.array([0.2, 0.0]) - c.movement_reference.wz_ref = 0.5 - - tf = 1.0 - time_steps = int(tf / c.gait_params.dt) - - foot_loc_history = np.zeros((3, 4, time_steps)) - joint_angle_history = np.zeros((3, 4, time_steps)) - - for i in range(time_steps): - step_controller(c) - foot_loc_history[:, :, i] = c.foot_locations - joint_angle_history[:, :, i] = c.joint_angles - - return foot_loc_history, joint_angle_history + controller.ticks += 1 \ No newline at end of file diff --git a/src/PupperConfig.py b/src/PupperConfig.py index 22d48a9..2cef915 100644 --- a/src/PupperConfig.py +++ b/src/PupperConfig.py @@ -1,5 +1,4 @@ import numpy as np -import numpy as np from scipy.linalg import solve diff --git a/src/UserInput.py b/src/UserInput.py new file mode 100644 index 0000000..3f64bcb --- /dev/null +++ b/src/UserInput.py @@ -0,0 +1,54 @@ +import UDPComms +import numpy as np + + +class UserInputs: + def __init__(self, udp_port=8830): + self.x_vel = 0.0 + self.y_vel = 0.0 + self.yaw_rate = 0.0 + self.pitch = 0.0 + self.stance_movement = 0 + self.gait_toggle = 0 + self.gait_mode = 0 + self.previous_gait_toggle = 0 + self.udp_handle = UDPComms.Subscriber(udp_port, timeout=0.3) + + +def get_input(user_input_obj): + 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"] + + # 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 + user_input_obj.previous_gait_toggle = user_input_obj.gait_toggle + + except UDPComms.timeout: + print("UDP Timed out") + + +def update_controller(controller, user_input_obj): + controller.movement_reference.v_xy_ref = np.array( + [user_input_obj.x_vel, user_input_obj.y_vel] + ) + controller.movement_reference.wz_ref = user_input_obj.yaw_rate + controller.movement_reference.pitch = user_input_obj.pitch + + 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]] + ) + + # Note this is negative since it is the feet relative to the body + controller.movement_reference.z_ref -= 0.001 * user_input_obj.stance_movement From de8232b8db363b5f94bc53597ed19961d82ab72b Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Sun, 12 Jan 2020 23:01:46 +0000 Subject: [PATCH 09/45] Smoother remote input --- src/UserInput.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/UserInput.py b/src/UserInput.py index 3f64bcb..10f20c0 100644 --- a/src/UserInput.py +++ b/src/UserInput.py @@ -39,7 +39,7 @@ def update_controller(controller, user_input_obj): [user_input_obj.x_vel, user_input_obj.y_vel] ) controller.movement_reference.wz_ref = user_input_obj.yaw_rate - controller.movement_reference.pitch = user_input_obj.pitch + controller.movement_reference.pitch = controller.movement_reference.pitch * 0.9 + user_input_obj.pitch * 0.1 if user_input_obj.gait_mode == 0: controller.gait_params.contact_phases = np.array( From 62ca7b3a70b52732d79c7384418cd2155879aa71 Mon Sep 17 00:00:00 2001 From: Aaron Schultz Date: Tue, 14 Jan 2020 18:51:59 -0800 Subject: [PATCH 10/45] Added servo deactivation --- src/HardwareInterface.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/HardwareInterface.py b/src/HardwareInterface.py index 3e95a7d..ec84fcf 100644 --- a/src/HardwareInterface.py +++ b/src/HardwareInterface.py @@ -47,3 +47,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) \ No newline at end of file From 3462909e155f84817ccf1d80264d1fde23d46b8e Mon Sep 17 00:00:00 2001 From: Aaron Schultz Date: Tue, 14 Jan 2020 20:07:54 -0800 Subject: [PATCH 11/45] Made code non-blocking --- run_robot.py | 11 +++++------ src/Controller.py | 15 +++++++++++++-- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/run_robot.py b/run_robot.py index ffe6103..c55f5ce 100644 --- a/run_robot.py +++ b/run_robot.py @@ -44,7 +44,10 @@ def main(): now = last_loop start = time.time() - for i in range(60000): + while(True): + + if time.time() - last_loop < controller.gait_params.dt: + continue last_loop = time.time() # Parse the udp joystick commands and then update the robot controller's parameters @@ -57,13 +60,9 @@ def main(): # Update the pwm widths going to the servos send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) - # Wait until it's time to execute again - while now - last_loop < controller.gait_params.dt: - now = time.time() - # print("Time since last loop: ", now - last_loop) end = time.time() - print("seconds per loop: ", (end - start) / 1000.0) + # print("seconds per loop: ", (end - start) / 1000.0) main() diff --git a/src/Controller.py b/src/Controller.py index 4992a9b..4fab319 100644 --- a/src/Controller.py +++ b/src/Controller.py @@ -40,7 +40,7 @@ def step( 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 @@ -112,4 +112,15 @@ def step_controller(controller): controller.joint_angles = four_legs_inverse_kinematics( rotated_foot_locations, controller.robot_config ) - controller.ticks += 1 \ No newline at end of file + controller.ticks += 1 + + +def setPoseToDefault(controller): + 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( + controller.foot_locations, controller.robot_config + ) + \ No newline at end of file From 18a73b6f3d16972bb5cbef163d8d564ceff3f4e2 Mon Sep 17 00:00:00 2001 From: Aaron Schultz Date: Tue, 14 Jan 2020 20:17:27 -0800 Subject: [PATCH 12/45] Moved location of robot_config --- run_robot.py | 12 ++++-------- src/Controller.py | 15 ++++++--------- 2 files changed, 10 insertions(+), 17 deletions(-) diff --git a/run_robot.py b/run_robot.py index c55f5ce..55d9686 100644 --- a/run_robot.py +++ b/run_robot.py @@ -5,6 +5,7 @@ from src.Controller import step_controller, Controller from src.HardwareInterface import send_servo_commands, initialize_pwm from src.PupperConfig import ( + PupperConfig, MovementReference, GaitParams, StanceParams, @@ -22,9 +23,10 @@ def main(): pwm_params = PWMParams() initialize_pwm(pi_board, pwm_params) + robot_config = PupperConfig() servo_params = ServoParams() - controller = Controller() + controller = Controller(robot_config) controller.movement_reference = MovementReference() controller.movement_reference.v_xy_ref = np.array([0.0, 0.0]) controller.movement_reference.wz_ref = 0 @@ -41,8 +43,6 @@ def main(): user_input = UserInputs() last_loop = time.time() - now = last_loop - start = time.time() while(True): @@ -55,14 +55,10 @@ def main(): update_controller(controller, user_input) # Step the controller forward by dt - step_controller(controller) + step_controller(controller, robot_config) # Update the pwm widths going to the servos send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) - end = time.time() - # print("seconds per loop: ", (end - start) / 1000.0) - - main() diff --git a/src/Controller.py b/src/Controller.py index 4fab319..568ab9e 100644 --- a/src/Controller.py +++ b/src/Controller.py @@ -1,5 +1,4 @@ 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 @@ -13,12 +12,11 @@ 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.ticks = 0 @@ -28,7 +26,7 @@ def __init__(self): + np.array([0, 0, self.movement_reference.z_ref])[:, np.newaxis] ) self.joint_angles = four_legs_inverse_kinematics( - self.foot_locations, self.robot_config + self.foot_locations, robot_config ) @@ -83,7 +81,7 @@ def step( return new_foot_locations -def step_controller(controller): +def step_controller(controller, robot_config): """Steps the controller forward one timestep Parameters @@ -110,17 +108,16 @@ def step_controller(controller): ) controller.joint_angles = four_legs_inverse_kinematics( - rotated_foot_locations, controller.robot_config + rotated_foot_locations, robot_config ) controller.ticks += 1 -def setPoseToDefault(controller): +def setPoseToDefault(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( - controller.foot_locations, controller.robot_config + controller.foot_locations, robot_config ) - \ No newline at end of file From 3d60e3c80d60850ea795c676e173c27a7a42bbe9 Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Wed, 15 Jan 2020 04:34:07 +0000 Subject: [PATCH 13/45] Fix merge conflict --- run_robot.py | 28 ---------------------------- src/PupperConfig.py | 6 ------ 2 files changed, 34 deletions(-) diff --git a/run_robot.py b/run_robot.py index 1b60e18..5646711 100644 --- a/run_robot.py +++ b/run_robot.py @@ -43,10 +43,8 @@ def main(): user_input = UserInputs() last_loop = time.time() -<<<<<<< HEAD while(True): - if time.time() - last_loop < controller.gait_params.dt: continue last_loop = time.time() @@ -60,31 +58,5 @@ def main(): # Update the pwm widths going to the servos send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) -======= - 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) - - # Update the pwm widths going to the servos - send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) - - # Wait until it's time to execute again - while now - last_loop < controller.gait_params.dt: - now = time.time() - # print("Time since last loop: ", now - last_loop) - - end = time.time() - print("seconds per loop: ", (end - start) / 1000.0) ->>>>>>> AddGate - main() diff --git a/src/PupperConfig.py b/src/PupperConfig.py index bf9a0b2..b99cb05 100644 --- a/src/PupperConfig.py +++ b/src/PupperConfig.py @@ -16,13 +16,7 @@ def __init__(self): self.micros_per_rad = MICROS_PER_RAD # Must be calibrated # The neutral angle of the joint relative to the modeled zero-angle in degrees, for each joint -<<<<<<< HEAD - self.neutral_angle_degrees = np.array( - [[9, 5, 13, -9], [45, 47, 44, 52], [-14, -36, -15, -35]] - ) -======= self.neutral_angle_degrees = NEUTRAL_ANGLE_DEGREES ->>>>>>> AddGate self.servo_multipliers = np.array( [[1, 1, 1, 1], [-1, 1, -1, 1], [1, -1, 1, -1]] From 69423853ffb4cb5789764b9fd9cc659786533252 Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Wed, 15 Jan 2020 01:02:13 -0800 Subject: [PATCH 14/45] Update README.md --- README.md | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 79c04fe..6c262c0 100644 --- a/README.md +++ b/README.md @@ -47,24 +47,27 @@ This repository contains Python code to run Pupper, a Raspberry Pi-based quadrup ssh pi@10.0.0.xx ``` where xx is the local address you chose for the Pi -- Go into RW mode +- Once connected to the Pi, go into read-write mode ```shell rw ``` -- In a separate shell, start the joystick publisher. Instructions here: https://github.com/stanfordroboticsclub/PupperCommand/blob/master/README.md -- Go into this repo's PupperPythonSim directory +- In a separate shell, start the joystick publisher. These instructions are copied from: https://github.com/stanfordroboticsclub/PupperCommand/blob/master/README.md ```shell - cd PupperPythonSim + cd PupperCommand + sudo systemctl start ds4drv + sudo python3 joystick.py ``` -- Start the PiGPIO daemon by executing in shell: +- Now go into this repo's PupperPythonSim directory and run the robot code! ```shell - sudo pigpiod + cd PupperPythonSim + sudo python3 run_robot.py ``` -- Run the robot code: + Sudo is needed so that the script can start the pigpio daemon. +- You can interrupt and stop the program by pressing Control-C. +- To turn off the servo motors, run ```shell - python3 run_robot.py - ``` - + 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. From 58d80bcab70f1f638ff24832bbf2fa8893807756 Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Wed, 15 Jan 2020 01:34:01 -0800 Subject: [PATCH 15/45] Start daemon from script plus other --- run_robot.py | 23 ++++++++++------------- src/PupperConfig.py | 6 +++--- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/run_robot.py b/run_robot.py index 5646711..58f6e09 100644 --- a/run_robot.py +++ b/run_robot.py @@ -2,6 +2,7 @@ import numpy as np import UDPComms import time +import subprocess from src.Controller import step_controller, Controller from src.HardwareInterface import send_servo_commands, initialize_pwm from src.PupperConfig import ( @@ -16,9 +17,17 @@ from src.UserInput import UserInputs, get_input, update_controller +def start_pigpiod(): + subprocess.Popen(["sudo pkill pigpiod"]) + subprocess.Popen(["sudo pigpiod"]) + + def main(): """Main program """ + + start_pigpiod() + pi_board = pigpio.pi() pwm_params = PWMParams() initialize_pwm(pi_board, pwm_params) @@ -27,19 +36,6 @@ def main(): servo_params = ServoParams() controller = Controller(robot_config) - 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 - - 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() last_loop = time.time() @@ -59,4 +55,5 @@ def main(): # Update the pwm widths going to the servos send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles) + main() diff --git a/src/PupperConfig.py b/src/PupperConfig.py index b99cb05..1dad679 100644 --- a/src/PupperConfig.py +++ b/src/PupperConfig.py @@ -55,11 +55,11 @@ class StanceParams: def __init__(self): self.z_time_constant = 0.02 - self.z_speed = 0.02 # maximum speed [m/s] + self.z_speed = 0.03 # maximum speed [m/s] self.pitch_time_constant = 0.5 - self.roll_speed = 0.12 # maximum roll rate [rad/s] + 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 @property def default_stance(self): From 89071512ca3f5d719fdf932f374223d727f1fc6b Mon Sep 17 00:00:00 2001 From: Nathan Kau Date: Thu, 23 Jan 2020 00:04:28 -0800 Subject: [PATCH 16/45] Update pybullet simulation --- simulate_pybullet.py | 96 ++++++++++++++++---------- src/PupperConfig.py | 3 +- src/pupper_out.xml | 24 +++---- src/pupper_pybullet.xml | 132 ++++++++++++++++++++++++++++++++++++ src/pupper_pybullet_out.xml | 132 ++++++++++++++++++++++++++++++++++++ 5 files changed, 339 insertions(+), 48 deletions(-) create mode 100644 src/pupper_pybullet.xml create mode 100644 src/pupper_pybullet_out.xml diff --git a/simulate_pybullet.py b/simulate_pybullet.py index 171f6b7..fe10e5a 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): @@ -31,7 +43,8 @@ def parallel_to_serial_joint_angles(joint_matrix): 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,59 +52,71 @@ 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() +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) -# 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) + # Calculate the next joint angle commands + step_controller(controller, robot_config) + # 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, @@ -99,19 +124,20 @@ def parallel_to_serial_joint_angles(joint_matrix): targetPositions=list(serial_joint_angles.T.reshape(12)), # positionGains=[1]*12, # velocityGains=[1]*12, - forces=[2] * 12, + forces=[4] * 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 + # 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/PupperConfig.py b/src/PupperConfig.py index 1dad679..0e5439e 100644 --- a/src/PupperConfig.py +++ b/src/PupperConfig.py @@ -60,12 +60,13 @@ def __init__(self): self.roll_speed = 0.16 # maximum roll rate [rad/s] self.delta_x = 0.1 self.delta_y = 0.10 + self.x_shift = 0.0 @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], ] 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 -