rafaelcaricio/Open_Duck_Mini_Runtime
1
0
Fork 0
mirror of https://github.com/apirrone/Open_Duck_Mini_Runtime.git synced 2025-09-02 11:13:55 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge branch 'v2' into automatic_port_detection

Browse source
This commit is contained in:
apirrone 2025-04-25 17:18:34 +02:00
commit 4ce2bca4d5
20 changed files with 326 additions and 270 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
README.md
View file

@ -127,9 +127,17 @@ You can also run `python3 scripts/imu_server.py` on the robot and `python3 scrip
> To find the ip address of the robot, run `ifconfig` on the robot
## Make your duck_config.json
Copy `example_config.json` in the home directory of your duck and rename it `duck_config.json`.
`cp example_config.json ~/duck_config.json`
In this file, you can configure some stuff, like registering if you installed the expression features, installed the imu upside down or and other stuff. You also write the joints offsets of your duck here
## Find the joints offsets
This script will guide you through finding the joints offsets of your robot, that you can then write in `rustypot_position_hwi.py` in `self.joints_offsets`
This script will guide you through finding the joints offsets of your robot that you can then write in your `duck_config.json`
> This procedure won't be necessary in the future as we will be flashing the offsets directly in each motor's eeprom.

0
aze Normal file
View file

29
example_config.json Normal file
View file

@ -0,0 +1,29 @@
{
"start_paused": false,
"imu_upside_down": false,
"phase_frequency_factor_offset": 0.0,
"expression_features": {
"eyes": false,
"projector": false,
"antennas": false,
"speaker": false,
"microphone": false,
"camera": false
},
"joints_offsets": {
"left_hip_yaw": 0.0,
"left_hip_roll": 0.0,
"left_hip_pitch": 0.0,
"left_knee": 0.0,
"left_ankle": 0.0,
"neck_pitch": 0.0,
"head_pitch": 0.0,
"head_yaw": 0.0,
"head_roll": 0.00,
"right_hip_yaw": 0.0,
"right_hip_roll": 0.0,
"right_hip_pitch": 0.0,
"right_knee": 0.0,
"right_ankle": 0.0
}
}

BIN
mini_bdx_runtime/assets/beep1.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/beep2.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/happy1.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/happy2.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/happy3.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/lamp.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/lamp2.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/lamp3.wav Normal file
View file

Binary file not shown.

BIN
mini_bdx_runtime/assets/motor.wav Normal file
View file

Binary file not shown.

83
mini_bdx_runtime/mini_bdx_runtime/duck_config.py Normal file
View file

@ -0,0 +1,83 @@
import json
from typing import Optional
import os
HOME_DIR = os.path.expanduser("~")
class DuckConfig:
def __init__(
self,
config_json_path: Optional[str] = f"{HOME_DIR}/duck_config.json",
ignore_default: bool = False,
):
"""
Looks for duck_config.json in the home directory by default.
If not found, uses default values.
"""
self.default = False
try:
self.json_config = (
json.load(open(config_json_path, "r")) if config_json_path else {}
)
except FileNotFoundError:
print(
f"Warning : didn't find the config json file at {config_json_path}, using default values"
)
self.json_config = {}
self.default = True
if config_json_path is None:
print("Warning : didn't provide a config json path, using default values")
self.default = True
if self.default and not ignore_default:
print("")
print("")
print("")
print("")
print("======")
print(
"WARNING : Running with default values probably won't work well. Please make a duck_config.json file and set the parameters."
)
res = input("Do you still want to run ? (y/N)")
if res.lower() != "y":
print("Exiting...")
exit(1)
self.start_paused = self.json_config.get("start_paused", False)
self.imu_upside_down = self.json_config.get("imu_upside_down", False)
self.phase_frequency_factor_offset = self.json_config.get(
"phase_frequency_factor_offset", 0.0
)
expression_features = self.json_config.get("expression_features", {})
self.eyes = expression_features.get("eyes", False)
self.projector = expression_features.get("projector", False)
self.antennas = expression_features.get("antennas", False)
self.speaker = expression_features.get("speaker", False)
self.microphone = expression_features.get("microphone", False)
self.camera = expression_features.get("camera", False)
# default joints offsets are 0.0
self.joints_offset = self.json_config.get(
"joints_offsets",
{
"left_hip_yaw": 0.0,
"left_hip_roll": 0.0,
"left_hip_pitch": 0.0,
"left_knee": 0.0,
"left_ankle": 0.0,
"neck_pitch": 0.0,
"head_pitch": 0.0,
"head_yaw": 0.0,
"head_roll": 0.00,
"right_hip_yaw": 0.0,
"right_hip_roll": 0.0,
"right_hip_pitch": 0.0,
"right_knee": 0.0,
"right_ankle": 0.0,
},
)

111
mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py
View file

@ -1,12 +1,14 @@
import time
from typing import List
import numpy as np
import rustypot
from mini_bdx_runtime.duck_config import DuckConfig
class HWI:
def __init__(self, usb_port="/dev/ttyACM0"):
def __init__(self, duck_config: DuckConfig, usb_port: str = "/dev/ttyACM0"):
self.duck_config = duck_config
# Order matters here
self.joints = {
@ -66,87 +68,27 @@ class HWI:
"right_ankle": -0.796,
}
# self.joints_offsets = {
# "left_hip_yaw": 0.084,
# "left_hip_roll": -0.112,
# "left_hip_pitch": -0.024,
# "left_knee": 0.005,
# "left_ankle": -0.09,
# "neck_pitch": 0.017,
# "head_pitch": 0.114,
# "head_yaw": -0.083,
# "head_roll": 0.072,
# # "left_antenna": 0,
# # "right_antenna": 0,
# "right_hip_yaw": -0.13499999999999998,
# "right_hip_roll": 0.205,
# "right_hip_pitch": 0.064,
# "right_knee": -0.027999999999999997,
# "right_ankle": -0.09799999999999999,
# }
# self.joints_offsets = {
# "left_hip_yaw": 0.14,
# "left_hip_roll": -0.05,
# "left_hip_pitch": -0.011,
# "left_knee": 0.0,
# "left_ankle": -0.107,
# "neck_pitch": 0.061,
# "head_pitch": 0.108,
# "head_yaw": -0.08399999999999999,
# "head_roll": 0.063,
# "right_hip_yaw": -0.131,
# "right_hip_roll": 0.089,
# "right_hip_pitch": 0.06799999999999999,
# "right_knee": 0.002,
# "right_ankle": -0.093,
# }
self.joints_offsets = {
"left_hip_yaw" : 0.082,
"left_hip_roll" : -0.089,
"left_hip_pitch" : -0.004,
"left_knee" : 0.077,
"left_ankle" : -0.1,
"neck_pitch" : 0.057,
"head_pitch" : 0.069,
"head_yaw" : -0.101,
"head_roll" : 0.08,
"right_hip_yaw" : -0.166,
"right_hip_roll" : 0.115,
"right_hip_pitch" : 0.089,
"right_knee" : -0.098,
"right_ankle" : -0.125
}
init_pos_with_offsets = {
joint: np.rad2deg(pos + self.joints_offsets[joint])
for joint, pos in self.init_pos.items()
}
self.joints_offsets = self.duck_config.joints_offset
self.kps = np.ones(len(self.joints)) * 32 # default kp
self.kds = np.ones(len(self.joints)) * 0 # default kd
self.low_torque_kps = np.ones(len(self.joints)) * 2
# self.control = rustypot.FeetechController(
# usb_port, 1000000, 100, list(self.joints.values()), list(self.kps), list(init_pos_with_offsets.values())
# )
self.io = rustypot.feetech(usb_port, 1000000)
def set_kps(self, kps):
self.kps = kps
self.io.set_kps(list(self.joints.values()), self.kps)
# self.control.set_new_kps(self.kps)
def set_kds(self, kds):
self.kds = kds
self.io.set_kds(list(self.joints.values()), self.kds)
def set_kp(self, id, kp):
# self.kps[id] = kp
self.io.set_kps([id], [kp])
def turn_on(self):
self.io.set_kps(list(self.joints.values()), self.low_torque_kps)
# self.control.set_new_kps(self.low_torque_kps)
print("turn on : low KPS set")
time.sleep(1)
@ -160,10 +102,6 @@ class HWI:
def turn_off(self):
self.io.disable_torque(list(self.joints.values()))
# self.control.disable_torque()
# def freeze(self):
# self.control.freeze()
def set_position(self, joint_name, pos):
"""
@ -172,7 +110,6 @@ class HWI:
id = self.joints[joint_name]
pos = pos + self.joints_offsets[joint_name]
self.io.write_goal_position([id], [pos])
# self.control.set_new_target([pos])
def set_position_all(self, joints_positions):
"""
@ -187,23 +124,20 @@ class HWI:
self.io.write_goal_position(
list(self.joints.values()), list(ids_positions.values())
)
# self.control.set_new_target(list(ids_positions.values()))
# self.control.goal_positions = list(ids_positions.values())
def get_present_positions(self, ignore=[]):
"""
Returns the present positions in radians
"""
# present_positions = np.deg2rad(
# self.control.io.get_present_position(self.joints.values())
# )
try:
present_positions = self.io.read_present_position(list(self.joints.values()))
present_positions = self.io.read_present_position(
list(self.joints.values())
)
except Exception as e:
print(e)
return None
# present_positions = np.deg2rad(self.control.get_present_position())
present_positions = [
pos - self.joints_offsets[joint]
for joint, pos in zip(self.joints.keys(), present_positions)
@ -216,34 +150,17 @@ class HWI:
Returns the present velocities in rad/s (default) or rev/min
"""
try:
present_velocities = self.io.read_present_velocity(list(self.joints.values()))
present_velocities = self.io.read_present_velocity(
list(self.joints.values())
)
except Exception as e:
print(e)
return None
# present_velocities = np.array(self.control.get_current_speed())
present_velocities = [
vel
for joint, vel in zip(self.joints.keys(), present_velocities)
if joint not in ignore
]
# present_velocities = np.array(
# self.control.io.get_present_speed(self.joints.values())
# )
# if rad_s:
# present_velocities = np.deg2rad(present_velocities) # rad/s
return np.array(np.around(present_velocities, 3))
# def get_present_voltages(self):
# return np.array(self.control.io.get_present_voltage(self.joints.values())) * 0.1
# def get_present_velocities(self, rad_s=True):
# """
# Returns the present velocities in rad/s (default) or rev/min
# """
# # rev/min
# present_velocities = np.array(
# self.control.io.get_present_speed(self.joints.values())
# )
# if rad_s:
# present_velocities = (2 * np.pi * present_velocities) / 60 # rad/s
# return np.array(np.around(present_velocities, 3))

15
mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py
View file

@ -18,10 +18,10 @@ HEAD_ROLL_RANGE = [-0.5, 0.5]
class XBoxController:
def __init__(self, command_freq, standing=False):
def __init__(self, command_freq, only_head_control=False):
self.command_freq = command_freq
self.standing = standing
self.head_control_mode = self.standing
self.head_control_mode = only_head_control
self.only_head_control = only_head_control
self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.last_left_trigger = 0.0
@ -129,7 +129,8 @@ class XBoxController:
if self.p1.get_button(4): # Y button
self.Y_pressed = True
self.head_control_mode = not self.head_control_mode
if not self.only_head_control:
self.head_control_mode = not self.head_control_mode
if self.p1.get_button(6): # LB button
self.LB_pressed = True
@ -162,7 +163,7 @@ class XBoxController:
self.RB_pressed,
left_trigger,
right_trigger,
up_down
up_down,
)
def get_last_command(self):
@ -184,7 +185,7 @@ class XBoxController:
RB_pressed,
self.last_left_trigger,
self.last_right_trigger,
up_down
up_down,
) = self.cmd_queue.get(
False
) # non blocking
@ -201,7 +202,7 @@ class XBoxController:
RB_pressed,
self.last_left_trigger,
self.last_right_trigger,
up_down
up_down,
)

32
scripts/find_soft_offsets.py
View file

@ -2,30 +2,24 @@
Find the offsets to set in self.joints_offsets in hwi_feetech_pwm_control.py
"""
# from mini_bdx_runtime.hwi_feetech_pwm_control import HWI
from mini_bdx_runtime.rustypot_position_hwi import HWI
from mini_bdx_runtime.duck_config import DuckConfig
import time
dummy_config = DuckConfig(config_json_path=None, ignore_default=True)
print("======")
print(
"Warning : this script will move the robot to its zero position quiclky, make sure it is safe to do so"
)
print("======")
print("")
input(
"Press any key to start. The robot will move to its zero position. Make sure it is safe to do so. At any time, press ctrl+c to stop, the motors will be turned off."
)
hwi = HWI()
hwi.joints_offsets = {
"left_hip_yaw": 0,
"left_hip_roll": 0,
"left_hip_pitch": 0,
"left_knee": 0,
"left_ankle": 0,
"neck_pitch": 0,
"head_pitch": 0,
"head_yaw": 0,
"head_roll": 0,
"right_hip_yaw": 0,
"right_hip_roll": 0,
"right_hip_pitch": 0,
"right_knee": 0,
"right_ankle": 0,
}
hwi = HWI(dummy_config)
hwi.init_pos = hwi.zero_pos
hwi.set_kds([0] * len(hwi.joints))
@ -83,7 +77,7 @@ try:
print("===")
print("Done ! ")
print("Now you can copy the offsets in the hwi_feetech_pwm_control.py file")
print("Now you can copy the offsets in your duck_config.json")
except KeyboardInterrupt:

161
scripts/head_puppet.py
View file

@ -2,23 +2,34 @@
Sets up the robot in init position, you control the head with the xbox controller
"""
from mini_bdx_runtime.rustypot_position_hwi import HWI
from mini_bdx_runtime.eyes import Eyes
import time
import pygame
import numpy as np
from mini_bdx_runtime.rustypot_position_hwi import HWI
from mini_bdx_runtime.duck_config import DuckConfig
from mini_bdx_runtime.xbox_controller import XBoxController
from mini_bdx_runtime.buttons import Buttons
from mini_bdx_runtime.eyes import Eyes
from mini_bdx_runtime.sounds import Sounds
from mini_bdx_runtime.antennas import Antennas
from mini_bdx_runtime.projector import Projector
sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/")
antennas = Antennas()
duck_config = DuckConfig()
eyes = Eyes()
projector = Projector()
hwi = HWI()
xbox_controller = XBoxController(50, only_head_control=True)
buttons = Buttons()
if duck_config.speaker:
sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/")
if duck_config.antennas:
antennas = Antennas()
if duck_config.eyes:
eyes = Eyes()
if duck_config.projector:
projector = Projector()
hwi = HWI(duck_config)
kps = [8] * 14
kds = [0] * 14
@ -27,81 +38,87 @@ hwi.set_kps(kps)
hwi.set_kds(kds)
hwi.turn_on()
pygame.init()
_p1 = pygame.joystick.Joystick(0)
_p1.init()
print(f"Loaded joystick with {_p1.get_numaxes()} axes.")
limits = {
"neck_pitch" : [-20, 60],
"head_pitch" : [-60, 45],
"head_yaw" : [-60, 60],
"head_roll" : [-20, 20],
"neck_pitch": [-20, 60],
"head_pitch": [-60, 45],
"head_yaw": [-60, 60],
"head_roll": [-20, 20],
}
l_x = 0
l_y = 0
r_x = 0
r_y = 0
left_trigger = 0
right_trigger = 0
while True:
X_pressed = False
A_pressed = False
try:
while True:
(
last_commands,
A_pressed,
B_pressed,
X_pressed,
Y_pressed,
LB_pressed,
RB_pressed,
left_trigger,
right_trigger,
up_down,
) = xbox_controller.get_last_command()
for event in pygame.event.get():
l_x = round(_p1.get_axis(0), 3)
l_y = round(_p1.get_axis(1), 3)
r_x = round(_p1.get_axis(2), 3)
r_y = round(_p1.get_axis(3), 3)
buttons.update(
A_pressed,
B_pressed,
X_pressed,
Y_pressed,
LB_pressed,
RB_pressed,
up_down == 1,
up_down == -1,
)
l_x = last_commands[5]
l_y = last_commands[4]
r_x = last_commands[6]
# r_y = last_commands[3]
right_trigger = np.around((_p1.get_axis(4) + 1) / 2, 3)
left_trigger = np.around((_p1.get_axis(5) + 1) / 2, 3)
head_yaw_deg = (
l_x * (limits["head_yaw"][1] - limits["head_yaw"][0]) / 2
+ (limits["head_yaw"][1] + limits["head_yaw"][0]) / 2
)
head_yaw_pos_rad = np.deg2rad(head_yaw_deg)
if _p1.get_button(0): # A button
A_pressed = True
head_roll_deg = (
r_x * (limits["head_roll"][1] - limits["head_roll"][0]) / 2
+ (limits["head_roll"][1] + limits["head_roll"][0]) / 2
)
head_roll_pos_rad = np.deg2rad(head_roll_deg)
head_pitch_deg = (
l_y * (limits["head_pitch"][1] - limits["head_pitch"][0]) / 2
+ (limits["head_pitch"][1] + limits["head_pitch"][0]) / 2
)
head_pitch_pos_rad = np.deg2rad(head_pitch_deg)
if _p1.get_button(3): # X button
X_pressed = True
# neck_pitch_deg = (
# -r_y * (limits["neck_pitch"][1] - limits["neck_pitch"][0]) / 2
# + (limits["neck_pitch"][1] + limits["neck_pitch"][0]) / 2
# )
# neck_pitch_pos_rad = np.deg2rad(neck_pitch_deg)
# print("l_x", l_x)
# print("l_y", l_y)
# print("r_x", r_x)
# print("r_y", r_y)
# print("===")
hwi.set_position("head_yaw", head_yaw_pos_rad)
hwi.set_position("head_roll", head_roll_pos_rad)
hwi.set_position("head_pitch", head_pitch_pos_rad)
# hwi.set_position("neck_pitch", neck_pitch_pos_rad)
if duck_config.antennas:
antennas.set_position_left(right_trigger)
antennas.set_position_right(left_trigger)
head_yaw_deg = l_x * (limits["head_yaw"][1] - limits["head_yaw"][0]) / 2 + (limits["head_yaw"][1] + limits["head_yaw"][0]) / 2
head_yaw_pos_rad = np.deg2rad(head_yaw_deg)
if buttons.B.triggered:
if duck_config.speaker:
sounds.play_random_sound()
head_roll_deg = r_x * (limits["head_roll"][1] - limits["head_roll"][0]) / 2 + (limits["head_roll"][1] + limits["head_roll"][0]) / 2
head_roll_pos_rad = np.deg2rad(head_roll_deg)
if buttons.X.triggered:
if duck_config.projector:
projector.switch()
head_pitch_deg = l_y * (limits["head_pitch"][1] - limits["head_pitch"][0]) / 2 + (limits["head_pitch"][1] + limits["head_pitch"][0]) / 2
head_pitch_pos_rad = np.deg2rad(head_pitch_deg)
neck_pitch_deg = -r_y * (limits["neck_pitch"][1] - limits["neck_pitch"][0]) / 2 + (limits["neck_pitch"][1] + limits["neck_pitch"][0]) / 2
neck_pitch_pos_rad = np.deg2rad(neck_pitch_deg)
hwi.set_position("head_yaw", head_yaw_pos_rad)
hwi.set_position("head_roll", head_roll_pos_rad)
hwi.set_position("head_pitch", head_pitch_pos_rad)
hwi.set_position("neck_pitch", neck_pitch_pos_rad)
antennas.set_position_left(right_trigger)
antennas.set_position_right(left_trigger)
if X_pressed:
sounds.play_random_sound()
if A_pressed:
projector.switch()
pygame.event.pump() # process event queue
time.sleep(1/60)
# pygame.event.pump() # process event queue
time.sleep(1 / 60)
except KeyboardInterrupt:
if duck_config.antennas:
antennas.stop()

7
scripts/turn_off.py
View file

@ -1,4 +1,9 @@
from mini_bdx_runtime.rustypot_position_hwi import HWI
from mini_bdx_runtime.duck_config import DuckConfig
import time
hwi = HWI()
duck_config = DuckConfig()
hwi = HWI(duck_config)
hwi.turn_off()
time.sleep(1)

5
scripts/turn_on.py
View file

@ -1,6 +1,9 @@
from mini_bdx_runtime.rustypot_position_hwi import HWI
from mini_bdx_runtime.duck_config import DuckConfig
import time
hwi = HWI()
duck_config = DuckConfig()
hwi = HWI(duck_config)
hwi.turn_on()
time.sleep(1)

143
scripts/v2_rl_walk_mujoco.py
View file

@ -2,7 +2,6 @@ import time
import pickle
import numpy as np
from mini_bdx_runtime.rustypot_position_hwi import HWI
from mini_bdx_runtime.onnx_infer import OnnxInfer
@ -16,15 +15,21 @@ from mini_bdx_runtime.antennas import Antennas
from mini_bdx_runtime.projector import Projector
from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter
from mini_bdx_runtime.buttons import Buttons
from mini_bdx_runtime.duck_config import DuckConfig
import os
HOME_DIR = os.path.expanduser("~")
class RLWalk:
def __init__(
self,
onnx_model_path: str,
duck_config_path: str = f"{HOME_DIR}/duck_config.json",
serial_port: str = "/dev/ttyACM0",
control_freq: float = 50,
pid=[32, 0, 0],
pid=[30, 0, 0],
action_scale=0.25,
commands=False,
pitch_bias=0,
@ -32,6 +37,9 @@ class RLWalk:
replay_obs=None,
cutoff_frequency=None,
):
self.duck_config = DuckConfig(config_json_path=duck_config_path)
self.commands = commands
self.pitch_bias = pitch_bias
@ -45,25 +53,6 @@ class RLWalk:
self.control_freq = control_freq
self.pid = pid
self.joints_order = [
"left_hip_yaw",
"left_hip_roll",
"left_hip_pitch",
"left_knee",
"left_ankle",
"neck_pitch",
"head_pitch",
"head_yaw",
"head_roll",
# "left_antenna",
# "right_antenna",
"right_hip_yaw",
"right_hip_roll",
"right_hip_pitch",
"right_knee",
"right_ankle",
]
self.save_obs = save_obs
if self.save_obs:
self.saved_obs = []
@ -78,18 +67,16 @@ class RLWalk:
self.control_freq, cutoff_frequency
)
self.hwi = HWI(serial_port)
self.hwi = HWI(self.duck_config, serial_port)
self.start()
self.imu = Imu(
sampling_freq=int(self.control_freq),
user_pitch_bias=self.pitch_bias,
upside_down=False,
upside_down=self.duck_config.imu_upside_down,
)
self.eyes = Eyes()
self.projector = Projector()
self.feet_contacts = FeetContacts()
# Scales
@ -99,49 +86,41 @@ class RLWalk:
self.last_last_action = np.zeros(self.num_dofs)
self.last_last_last_action = np.zeros(self.num_dofs)
self.init_pos = [
0.002,
0.053,
-0.63,
1.368,
-0.784,
0,
0,
0,
0,
-0.003,
-0.065,
0.635,
1.379,
-0.796,
]
self.init_pos = list(self.hwi.init_pos.values())
self.motor_targets = np.array(self.init_pos.copy())
self.prev_motor_targets = np.array(self.init_pos.copy())
self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.paused = False
self.sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/")
self.antennas = Antennas()
self.paused = self.duck_config.start_paused
self.command_freq = 20 # hz
if self.commands:
self.xbox_controller = XBoxController(self.command_freq)
self.buttons = Buttons()
# Reference motion, but we only really need the length of one phase
# TODO
self.PRM = PolyReferenceMotion("./polynomial_coefficients.pkl")
self.imitation_i = 0
self.imitation_phase = np.array([0, 0])
self.phase_frequency_factor = 1.0
self.phase_frequency_factor_offset = 0.0
self.phase_frequency_factor_offset = (
self.duck_config.phase_frequency_factor_offset
)
def add_fake_head(self, pos):
# add just the antennas now
assert len(pos) == self.num_dofs
pos_with_head = np.insert(pos, 9, [0, 0])
return np.array(pos_with_head)
# Optional expression features
if self.duck_config.eyes:
self.eyes = Eyes()
if self.duck_config.projector:
self.projector = Projector()
if self.duck_config.speaker:
self.sounds = Sounds(
volume=1.0, sound_directory="../mini_bdx_runtime/assets/"
)
if self.duck_config.antennas:
self.antennas = Antennas()
def get_obs(self):
@ -198,6 +177,7 @@ class RLWalk:
kps = [self.pid[0]] * 14
kds = [self.pid[2]] * 14
# lower head kps
kps[5:9] = [8, 8, 8, 8]
self.hwi.set_kps(kps)
@ -251,16 +231,20 @@ class RLWalk:
LB_pressed,
RB_pressed,
up_down == 1,
up_down == -1
up_down == -1,
)
if self.buttons.dpad_up.triggered:
self.phase_frequency_factor_offset += 0.05
print(f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}")
print(
f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}"
)
if self.buttons.dpad_down.triggered:
self.phase_frequency_factor_offset -= 0.05
print(f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}")
print(
f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}"
)
if self.buttons.LB.is_pressed:
self.phase_frequency_factor = 1.3
@ -271,13 +255,16 @@ class RLWalk:
# print(f"Phase frequency factor {self.phase_frequency_factor}")
if self.buttons.X.triggered:
self.projector.switch()
if self.duck_config.projector:
self.projector.switch()
if self.buttons.B.triggered:
self.sounds.play_random_sound()
if self.duck_config.speaker:
self.sounds.play_random_sound()
self.antennas.set_position_left(right_trigger)
self.antennas.set_position_right(left_trigger)
if self.duck_config.antennas:
self.antennas.set_position_left(right_trigger)
self.antennas.set_position_right(left_trigger)
if self.buttons.A.triggered:
self.paused = not self.paused
@ -294,7 +281,9 @@ class RLWalk:
if obs is None:
continue
self.imitation_i += 1 * (self.phase_frequency_factor + self.phase_frequency_factor_offset)
self.imitation_i += 1 * (
self.phase_frequency_factor + self.phase_frequency_factor_offset
)
self.imitation_i = self.imitation_i % self.PRM.nb_steps_in_period
self.imitation_phase = np.array(
[
@ -327,13 +316,13 @@ class RLWalk:
self.motor_targets = self.init_pos + action * self.action_scale
self.motor_targets = np.clip(
self.motor_targets,
self.prev_motor_targets
- self.max_motor_velocity * (1 / self.control_freq), # control dt
self.prev_motor_targets
+ self.max_motor_velocity * (1 / self.control_freq), # control dt
)
# self.motor_targets = np.clip(
# self.motor_targets,
# self.prev_motor_targets
# - self.max_motor_velocity * (1 / self.control_freq), # control dt
# self.prev_motor_targets
# + self.max_motor_velocity * (1 / self.control_freq), # control dt
# )
if self.action_filter is not None:
self.action_filter.push(self.motor_targets)
@ -348,7 +337,9 @@ class RLWalk:
head_motor_targets = self.last_commands[3:] + self.motor_targets[5:9]
self.motor_targets[5:9] = head_motor_targets
action_dict = make_action_dict(self.motor_targets, self.joints_order)
action_dict = make_action_dict(
self.motor_targets, list(self.hwi.joints.keys())
)
self.hwi.set_position_all(action_dict)
@ -364,7 +355,8 @@ class RLWalk:
time.sleep(max(0, 1 / self.control_freq - took))
except KeyboardInterrupt:
pass
if self.duck_config.antennas:
self.antennas.stop()
if self.save_obs:
pickle.dump(self.saved_obs, open("robot_saved_obs.pkl", "wb"))
@ -376,8 +368,14 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--onnx_model_path", type=str, required=True)
parser.add_argument(
"--duck_config_path",
type=str,
required=False,
default=f"{HOME_DIR}/duck_config.json",
)
parser.add_argument("-a", "--action_scale", type=float, default=0.25)
parser.add_argument("-p", type=int, default=32)
parser.add_argument("-p", type=int, default=30)
parser.add_argument("-i", type=int, default=0)
parser.add_argument("-d", type=int, default=0)
parser.add_argument("-c", "--control_freq", type=int, default=50)
@ -402,7 +400,7 @@ if __name__ == "__main__":
default=None,
help="replay the observations from a previous run (can be from the robot or from mujoco)",
)
parser.add_argument("--cutoff_frequency", type=float, default=40)
parser.add_argument("--cutoff_frequency", type=float, default=None)
args = parser.parse_args()
pid = [args.p, args.i, args.d]
@ -410,6 +408,7 @@ if __name__ == "__main__":
print("Done parsing args")
rl_walk = RLWalk(
args.onnx_model_path,
duck_config_path=args.duck_config_path,
action_scale=args.action_scale,
pid=pid,
control_freq=args.control_freq,