mirror of
https://github.com/apirrone/Open_Duck_Mini_Runtime.git
synced 2025-09-02 19:23:54 +00:00
cleaning up
This commit is contained in:
apirrone
parent
1bc8ba2d1f
commit
cdfb571479
24 changed files with 9 additions and 2855 deletions
|
|
@ -21,3 +21,12 @@ class FeetContacts:
|
|||
if GPIO.input(RIGHT_FOOT_PIN) == GPIO.LOW:
|
||||
right = True
|
||||
return np.array([left, right])
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
|
||||
feet_contacts = FeetContacts()
|
||||
while True:
|
||||
print(feet_contacts.get())
|
||||
time.sleep(0.05)
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -1,120 +0,0 @@
|
|||
from pypot.feetech import FeetechSTS3215IO
|
||||
import time
|
||||
import numpy as np
|
||||
from threading import Thread
|
||||
|
||||
|
||||
class FeetechPWMControl:
|
||||
def __init__(self, ids, init_pos_rad, usb_port="/dev/ttyACM0"):
|
||||
self.io = FeetechSTS3215IO(
|
||||
usb_port,
|
||||
baudrate=1000000,
|
||||
use_sync_read=True,
|
||||
)
|
||||
self.ids = ids
|
||||
|
||||
self.io.set_mode({id: 2 for id in self.ids})
|
||||
self.kps = np.ones(len(self.ids)) * 32 # default kp
|
||||
|
||||
self.init_pos_rad = init_pos_rad
|
||||
self.init_pos_deg = np.rad2deg(self.init_pos_rad)
|
||||
|
||||
self.control_freq = 200 # Hz
|
||||
self.goal_positions = [0] * len(self.ids)
|
||||
self.goal_positions = self.init_pos_deg
|
||||
self.present_positions = [0] * len(self.ids)
|
||||
self.last_positions = [0] * len(self.ids)
|
||||
self.present_speeds = [0] * len(self.ids)
|
||||
self.speed_decimation = 2
|
||||
self.speed_decimation_index = 0
|
||||
self.last_t = time.time()
|
||||
|
||||
Thread(target=self.update, daemon=True).start()
|
||||
|
||||
def set_kps(self, kps):
|
||||
self.kps = np.array(kps)
|
||||
|
||||
def disable_torque(self):
|
||||
self.io.set_mode({id: 0 for id in self.ids})
|
||||
self.io.disable_torque(self.ids)
|
||||
|
||||
# def enable_torque(self):
|
||||
# self.io.enable_torque(self.ids)
|
||||
# self.io.set_mode({id: 2 for id in self.ids})
|
||||
|
||||
def freeze(self):
|
||||
present_position = list(self.io.get_present_position(self.ids))
|
||||
print(present_position)
|
||||
self.io.set_goal_position(
|
||||
{id: present_position[i] for i, id in enumerate(self.ids)}
|
||||
)
|
||||
self.io.set_mode({id: 0 for id in self.ids})
|
||||
self.io.enable_torque(self.ids)
|
||||
|
||||
def update(self):
|
||||
while True:
|
||||
s = time.time()
|
||||
try:
|
||||
self.present_positions = self.io.get_present_position(self.ids)
|
||||
# self.present_speeds = self.io.get_present_speed(self.ids)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
if len(self.present_positions) != len(self.ids):
|
||||
print("ERROR : present_positions and ids do not have the same length")
|
||||
time.sleep(1 / self.control_freq)
|
||||
continue
|
||||
errors = np.array(self.goal_positions) - np.array(self.present_positions)
|
||||
|
||||
pwms = self.kps * errors
|
||||
pwms = np.int16(pwms)
|
||||
pwms = np.clip(pwms, -1000, 1000)
|
||||
|
||||
pwm_magnitudes = abs(pwms)
|
||||
|
||||
direction_bits = [1 if pwm >= 0 else 0 for pwm in pwms]
|
||||
|
||||
goal_times = [
|
||||
(direction_bits[i] << 10) | pwm_magnitudes[i]
|
||||
for i in range(len(pwm_magnitudes))
|
||||
]
|
||||
try:
|
||||
self.io.set_goal_time({id: goal_times[i] for i, id in enumerate(self.ids)})
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
if self.speed_decimation_index % self.speed_decimation == 0:
|
||||
self.present_speeds = (
|
||||
np.array(self.present_positions) - np.array(self.last_positions)
|
||||
) / (time.time() - self.last_t)
|
||||
self.last_positions = np.array(self.present_positions).copy()
|
||||
self.last_t = time.time()
|
||||
|
||||
took = time.time() - s
|
||||
# print("Took : ", np.around(took, 3), ". Budget : ", np.around(1/self.control_freq, 3), "diff : ", ((1/self.control_freq - took)))
|
||||
# if (1 / self.control_freq - took) < 0:
|
||||
# print(
|
||||
# "Low level control budget exceded by ",
|
||||
# np.around(took - 1 / self.control_freq, 3),
|
||||
# )
|
||||
time.sleep(max(0, (1 / self.control_freq - took)))
|
||||
self.speed_decimation_index += 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ids = [1, 2]
|
||||
pwm_control = FeetechPWMControl(ids)
|
||||
pwm_control.enable_torque()
|
||||
pwm_control.set_kps([32, 32])
|
||||
|
||||
pwm_control.goal_positions = [-90, -90]
|
||||
try:
|
||||
while True:
|
||||
pwm_control.goal_positions = [90, 90]
|
||||
time.sleep(1)
|
||||
pwm_control.goal_positions = [-90, -90]
|
||||
time.sleep(1)
|
||||
except KeyboardInterrupt:
|
||||
pwm_control.disable_torque()
|
||||
|
|
@ -1,111 +0,0 @@
|
|||
import rustypot
|
||||
import time
|
||||
import numpy as np
|
||||
from threading import Thread
|
||||
|
||||
|
||||
class FeetechPWMControl:
|
||||
def __init__(self, ids, init_pos_rad, usb_port="/dev/ttyACM0"):
|
||||
self.io = rustypot.feetech(usb_port, 1000000)
|
||||
|
||||
self.ids = ids
|
||||
|
||||
self.io.set_mode(self.ids, 2)
|
||||
self.kps = np.ones(len(self.ids)) * 32 # default kp
|
||||
|
||||
self.init_pos_rad = init_pos_rad
|
||||
self.init_pos_deg = np.rad2deg(self.init_pos_rad)
|
||||
|
||||
self.control_freq = 100 # Hz
|
||||
self.goal_positions = [0] * len(self.ids)
|
||||
self.goal_positions = self.init_pos_deg
|
||||
self.present_positions = [0] * len(self.ids)
|
||||
self.last_positions = [0] * len(self.ids)
|
||||
self.present_speeds = [0] * len(self.ids)
|
||||
self.speed_decimation = 2
|
||||
self.speed_decimation_index = 0
|
||||
self.last_t = time.time()
|
||||
|
||||
Thread(target=self.update, daemon=True).start()
|
||||
|
||||
def set_kps(self, kps):
|
||||
self.kps = np.array(kps)
|
||||
|
||||
def disable_torque(self):
|
||||
self.io.set_mode(self.ids, 0)
|
||||
self.io.disable_torque(self.ids)
|
||||
|
||||
def freeze(self):
|
||||
present_position = list(self.io.read_present_position(self.ids))
|
||||
self.io.write_goal_position(self.ids, present_position)
|
||||
self.io.set_mode(self.ids, 0)
|
||||
|
||||
def update(self):
|
||||
while True:
|
||||
s = time.time()
|
||||
try:
|
||||
self.present_positions = np.rad2deg(self.io.read_present_position(self.ids))
|
||||
|
||||
# self.present_speeds = self.io.get_present_speed(self.ids)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
if len(self.present_positions) != len(self.ids):
|
||||
print("ERROR : present_positions and ids do not have the same length")
|
||||
time.sleep(1 / self.control_freq)
|
||||
continue
|
||||
errors = np.array(self.goal_positions) - np.array(self.present_positions)
|
||||
|
||||
pwms = self.kps * errors
|
||||
pwms = np.int16(pwms)
|
||||
pwms = np.clip(pwms, -1000, 1000)
|
||||
|
||||
pwm_magnitudes = abs(pwms)
|
||||
|
||||
direction_bits = [1 if pwm >= 0 else 0 for pwm in pwms]
|
||||
|
||||
goal_times = [
|
||||
(direction_bits[i] << 10) | pwm_magnitudes[i]
|
||||
for i in range(len(pwm_magnitudes))
|
||||
]
|
||||
try:
|
||||
self.io.set_goal_time(self.ids, goal_times)
|
||||
# self.io.set_goal_time({id: goal_times[i] for i, id in enumerate(self.ids)})
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
if self.speed_decimation_index % self.speed_decimation == 0:
|
||||
self.present_speeds = (
|
||||
np.array(self.present_positions) - np.array(self.last_positions)
|
||||
) / (time.time() - self.last_t)
|
||||
self.last_positions = np.array(self.present_positions).copy()
|
||||
self.last_t = time.time()
|
||||
|
||||
took = time.time() - s
|
||||
# print("Took : ", np.around(took, 3), ". Budget : ", np.around(1/self.control_freq, 3), "diff : ", ((1/self.control_freq - took)))
|
||||
if (1 / self.control_freq - took) < 0:
|
||||
print(
|
||||
"Low level control budget exceded by ",
|
||||
np.around(took - 1 / self.control_freq, 3),
|
||||
)
|
||||
time.sleep(max(0, (1 / self.control_freq - took)))
|
||||
self.speed_decimation_index += 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ids = [1, 2]
|
||||
pwm_control = FeetechPWMControl(ids)
|
||||
pwm_control.enable_torque()
|
||||
pwm_control.set_kps([32, 32])
|
||||
|
||||
pwm_control.goal_positions = [-90, -90]
|
||||
try:
|
||||
while True:
|
||||
pwm_control.goal_positions = [90, 90]
|
||||
time.sleep(1)
|
||||
pwm_control.goal_positions = [-90, -90]
|
||||
time.sleep(1)
|
||||
except KeyboardInterrupt:
|
||||
pwm_control.disable_torque()
|
||||
|
|
@ -1,188 +0,0 @@
|
|||
import time
|
||||
from typing import List
|
||||
|
||||
import numpy as np
|
||||
|
||||
from mini_bdx_runtime.io_330 import Dxl330IO
|
||||
|
||||
|
||||
class HWI:
|
||||
def __init__(self, usb_port="/dev/ttyUSB1", baudrate=3000000):
|
||||
self.dxl_io = Dxl330IO(usb_port, baudrate=baudrate, use_sync_read=True)
|
||||
self.joints = {
|
||||
"right_hip_yaw": 10,
|
||||
"right_hip_roll": 11,
|
||||
"right_hip_pitch": 12,
|
||||
"right_knee": 13,
|
||||
"right_ankle": 14,
|
||||
"left_hip_yaw": 20,
|
||||
"left_hip_roll": 21,
|
||||
"left_hip_pitch": 22,
|
||||
"left_knee": 23,
|
||||
"left_ankle": 24,
|
||||
"neck_pitch": 30,
|
||||
"head_pitch": 31,
|
||||
"head_yaw": 32,
|
||||
}
|
||||
|
||||
self.init_pos = {
|
||||
"right_hip_yaw": 0.001171696610228082,
|
||||
"right_hip_roll": 0.006726989242258406,
|
||||
"right_hip_pitch": 1.0129772861831692,
|
||||
"right_knee": -1.4829304760981399,
|
||||
"right_ankle": 0.6444901047812701,
|
||||
"left_hip_yaw": -0.002853397830292128,
|
||||
"left_hip_roll": 0.01626303761810685,
|
||||
"left_hip_pitch": 1.0105624704499077,
|
||||
"left_knee": -1.4865015965817336,
|
||||
"left_ankle": 0.6504953719748071,
|
||||
"neck_pitch": -0.17453292519943295,
|
||||
"head_pitch": -0.17453292519943295,
|
||||
"head_yaw": 0,
|
||||
"left_antenna": 0,
|
||||
"right_antenna": 0,
|
||||
}
|
||||
|
||||
# self.joints_offsets = {
|
||||
# "right_hip_yaw": -0.02,
|
||||
# "right_hip_roll": 0,
|
||||
# "right_hip_pitch": 0,
|
||||
# "right_knee": -0.05,
|
||||
# "right_ankle": -0.1,
|
||||
# "left_hip_yaw": -0.05,
|
||||
# "left_hip_roll": 0.05,
|
||||
# "left_hip_pitch": 0.02,
|
||||
# "left_knee": -0.08,
|
||||
# "left_ankle": -0.1,
|
||||
# "neck_pitch": 0,
|
||||
# "head_pitch": 0.3,
|
||||
# "head_yaw": 0,
|
||||
# # "left_antenna": 0,
|
||||
# # "right_antenna": 0,
|
||||
# }
|
||||
|
||||
self.joints_offsets = {
|
||||
"right_hip_yaw": 0.0,
|
||||
"right_hip_roll": 0,
|
||||
"right_hip_pitch": 0,
|
||||
"right_knee": 0.0,
|
||||
"right_ankle": 0.0,
|
||||
"left_hip_yaw": 0.00,
|
||||
"left_hip_roll": 0.00,
|
||||
"left_hip_pitch": 0.00,
|
||||
"left_knee": 0.00,
|
||||
"left_ankle": 0.0,
|
||||
"neck_pitch": 0,
|
||||
"head_pitch": 0.0,
|
||||
"head_yaw": 0,
|
||||
# "left_antenna": 0,
|
||||
# "right_antenna": 0,
|
||||
}
|
||||
|
||||
self.init_pos = {
|
||||
joint: position - offset
|
||||
for joint, position, offset in zip(
|
||||
self.init_pos.keys(),
|
||||
self.init_pos.values(),
|
||||
self.joints_offsets.values(),
|
||||
)
|
||||
}
|
||||
|
||||
# current based position : 0x5
|
||||
self.dxl_io.set_operating_mode({id: 0x3 for id in self.joints.values()})
|
||||
|
||||
def set_pid(self, pid, joint_name):
|
||||
self.dxl_io.set_pid_gain({self.joints[joint_name]: pid})
|
||||
|
||||
def set_pid_all(self, pid):
|
||||
self.dxl_io.set_pid_gain({id: pid for id in self.joints.values()})
|
||||
|
||||
def set_low_torque(self):
|
||||
self.dxl_io.set_pid_gain({id: [100, 0, 0] for id in self.joints.values()})
|
||||
|
||||
def set_high_torque(self):
|
||||
# https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/#position-pid-gain80-82-84-feedforward-1st2nd-gains88-90
|
||||
# 128 P factor
|
||||
# 16 D factor
|
||||
self.dxl_io.set_pid_gain(
|
||||
{id: [10 * 128, 0, int(0.5 * 16)] for id in self.joints.values()}
|
||||
)
|
||||
for name in ["neck_pitch", "head_pitch", "head_yaw"]:
|
||||
self.dxl_io.set_pid_gain({self.joints[name]: [150, 0, 0]})
|
||||
|
||||
def turn_on(self):
|
||||
# self.set_low_torque()
|
||||
self.dxl_io.enable_torque(self.joints.values())
|
||||
time.sleep(1)
|
||||
# self.goto_zero()
|
||||
self.set_position_all(self.init_pos)
|
||||
time.sleep(1)
|
||||
# self.set_high_torque()
|
||||
|
||||
def turn_off(self):
|
||||
self.dxl_io.disable_torque(self.joints.values())
|
||||
|
||||
def goto_zero(self):
|
||||
goal_dict = {joint: 0 for joint in self.joints.keys()}
|
||||
|
||||
self.set_position_all(goal_dict)
|
||||
|
||||
def set_position_all(self, joints_positions):
|
||||
"""
|
||||
joints_positions is a dictionary with joint names as keys and joint positions as values
|
||||
Warning: expects radians
|
||||
"""
|
||||
ids_positions = {
|
||||
self.joints[joint]: np.rad2deg(-position)
|
||||
for joint, position in joints_positions.items()
|
||||
}
|
||||
|
||||
# print(ids_positions)
|
||||
self.dxl_io.set_goal_position(ids_positions)
|
||||
|
||||
def set_position(self, joint_name, position):
|
||||
self.dxl_io.set_goal_position({self.joints[joint_name]: np.rad2deg(-position)})
|
||||
|
||||
def get_present_current(self, joint_name):
|
||||
return self.dxl_io.get_present_current([self.joints[joint_name]])[0]
|
||||
|
||||
def get_voltage_all(self):
|
||||
return self.dxl_io.get_present_input_voltage(self.joints.values())
|
||||
|
||||
def get_present_input_voltage(self, joint_name):
|
||||
return self.dxl_io.get_present_input_voltage([self.joints[joint_name]])[0]
|
||||
|
||||
def get_current_all(self):
|
||||
return self.dxl_io.get_present_current(self.joints.values())
|
||||
|
||||
def get_goal_current(self, joint_name):
|
||||
return self.dxl_io.get_goal_current([self.joints[joint_name]])[0]
|
||||
|
||||
def get_current_limit(self, joint_name):
|
||||
return self.dxl_io.get_current_limit([self.joints[joint_name]])[0]
|
||||
|
||||
def get_present_positions(self):
|
||||
present_position = list(
|
||||
np.around(
|
||||
np.deg2rad((self.dxl_io.get_present_position(self.joints.values()))), 3
|
||||
)
|
||||
)
|
||||
factor = np.ones(len(present_position)) * -1
|
||||
return present_position * factor
|
||||
|
||||
def get_present_velocities(self, rad_s=True) -> List[float]:
|
||||
"""
|
||||
Returns the present velocities in rad/s or rev/min
|
||||
"""
|
||||
# rev/min
|
||||
present_velocities = np.array(
|
||||
self.dxl_io.get_present_velocity(self.joints.values())
|
||||
)
|
||||
if rad_s:
|
||||
present_velocities = (2 * np.pi * present_velocities) / 60 # rad/s
|
||||
|
||||
factor = np.ones(len(present_velocities)) * -1
|
||||
return list(present_velocities * factor)
|
||||
|
||||
def get_operating_modes(self):
|
||||
return self.dxl_io.get_operating_mode(self.joints.values())
|
||||
|
|
@ -1,207 +0,0 @@
|
|||
import time
|
||||
from typing import List
|
||||
|
||||
import numpy as np
|
||||
|
||||
# from pypot.feetech import FeetechSTS3215IO
|
||||
# from mini_bdx_runtime.feetech_pwm_control import FeetechPWMControl
|
||||
from mini_bdx_runtime.feetech_pwm_control_rustypot import FeetechPWMControl
|
||||
|
||||
|
||||
class HWI:
|
||||
def __init__(self, usb_port="/dev/ttyACM0"):
|
||||
|
||||
# Order matters here
|
||||
self.joints = {
|
||||
"left_hip_yaw": 20,
|
||||
"left_hip_roll": 21,
|
||||
"left_hip_pitch": 22,
|
||||
"left_knee": 23,
|
||||
"left_ankle": 24,
|
||||
"neck_pitch": 30,
|
||||
"head_pitch": 31,
|
||||
"head_yaw": 32,
|
||||
"head_roll": 33,
|
||||
# "left_antenna": None,
|
||||
# "right_antenna": None,
|
||||
"right_hip_yaw": 10,
|
||||
"right_hip_roll": 11,
|
||||
"right_hip_pitch": 12,
|
||||
"right_knee": 13,
|
||||
"right_ankle": 14,
|
||||
}
|
||||
|
||||
self.zero_pos = {
|
||||
"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,
|
||||
# "left_antenna":0,
|
||||
# "right_antenna":0,
|
||||
"right_hip_yaw": 0,
|
||||
"right_hip_roll": 0,
|
||||
"right_hip_pitch": 0,
|
||||
"right_knee": 0,
|
||||
"right_ankle": 0,
|
||||
}
|
||||
|
||||
self.init_pos = {
|
||||
"left_hip_yaw": 0.002,
|
||||
"left_hip_roll": 0.053,
|
||||
"left_hip_pitch": -0.63,
|
||||
"left_knee": 1.368,
|
||||
"left_ankle": -0.784,
|
||||
"neck_pitch": 0.0,
|
||||
"head_pitch": 0.0,
|
||||
"head_yaw": 0,
|
||||
"head_roll": 0,
|
||||
# "left_antenna": 0,
|
||||
# "right_antenna": 0,
|
||||
"right_hip_yaw": -0.003,
|
||||
"right_hip_roll": -0.065,
|
||||
"right_hip_pitch": 0.635,
|
||||
"right_knee": 1.379,
|
||||
"right_ankle": -0.796,
|
||||
}
|
||||
|
||||
# self.init_pos = self.zero_pos # TODO REMOVE
|
||||
|
||||
# self.joints_offsets = {
|
||||
# "left_hip_yaw": 0.07,
|
||||
# "left_hip_roll": -0.1,
|
||||
# "left_hip_pitch": 0.0,
|
||||
# "left_knee": 0.05,
|
||||
# "left_ankle": -0.1,
|
||||
# "neck_pitch": 0.1,
|
||||
# "head_pitch": 0.1,
|
||||
# "head_yaw": 0,
|
||||
# "head_roll": 0.1,
|
||||
# # "left_antenna": 0,
|
||||
# # "right_antenna": 0,
|
||||
# "right_hip_yaw": -0.15,
|
||||
# "right_hip_roll": 0.15,
|
||||
# "right_hip_pitch": 0.05,
|
||||
# "right_knee": -0.05,
|
||||
# "right_ankle": -0.08,
|
||||
# }
|
||||
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,
|
||||
|
||||
}
|
||||
|
||||
init_pos_with_offsets = {
|
||||
joint: pos + self.joints_offsets[joint]
|
||||
for joint, pos in self.init_pos.items()
|
||||
}
|
||||
|
||||
self.control = FeetechPWMControl(
|
||||
ids=list(self.joints.values()),
|
||||
init_pos_rad=list(init_pos_with_offsets.values()),
|
||||
usb_port=usb_port,
|
||||
)
|
||||
|
||||
self.kps = np.ones(len(self.joints)) * 32 # default kp
|
||||
self.low_torque_kps = np.ones(len(self.joints)) * 8 # default kp
|
||||
|
||||
def set_kps(self, kps):
|
||||
self.kps = kps
|
||||
self.control.set_kps(self.kps)
|
||||
|
||||
def turn_on(self):
|
||||
self.control.set_kps(self.low_torque_kps)
|
||||
print("turn on : low KPS set")
|
||||
time.sleep(1)
|
||||
|
||||
self.set_position_all(self.init_pos)
|
||||
print("turn on : init pos set")
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
self.control.set_kps(self.kps)
|
||||
print("turn on : high kps")
|
||||
|
||||
def turn_off(self):
|
||||
self.control.disable_torque()
|
||||
|
||||
def freeze(self):
|
||||
self.control.freeze()
|
||||
|
||||
def set_position_all(self, joints_positions):
|
||||
"""
|
||||
joints_positions is a dictionary with joint names as keys and joint positions as values
|
||||
Warning: expects radians
|
||||
"""
|
||||
ids_positions = {
|
||||
self.joints[joint]: np.rad2deg(position + self.joints_offsets[joint])
|
||||
for joint, position in joints_positions.items()
|
||||
}
|
||||
|
||||
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())
|
||||
# )
|
||||
|
||||
present_positions = np.deg2rad(self.control.present_positions)
|
||||
present_positions = [
|
||||
pos - self.joints_offsets[joint]
|
||||
for joint, pos in zip(self.joints.keys(), present_positions)
|
||||
if joint not in ignore
|
||||
]
|
||||
return np.array(np.around(present_positions, 3))
|
||||
|
||||
def get_present_velocities(self, rad_s=True, ignore=[]):
|
||||
"""
|
||||
Returns the present velocities in rad/s (default) or rev/min
|
||||
"""
|
||||
# deg/s
|
||||
present_velocities = np.array(self.control.present_speeds)
|
||||
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))
|
||||
|
|
@ -1,180 +0,0 @@
|
|||
import time
|
||||
from typing import List
|
||||
|
||||
import numpy as np
|
||||
|
||||
from pypot.feetech import FeetechSTS3215IO
|
||||
|
||||
|
||||
class HWI:
|
||||
def __init__(self, usb_port="/dev/ttyACM0", baudrate=3000000):
|
||||
self.dxl_io = FeetechSTS3215IO(
|
||||
usb_port,
|
||||
baudrate=1000000,
|
||||
use_sync_read=True,
|
||||
)
|
||||
self.joints = {
|
||||
"left_hip_yaw": 20,
|
||||
"left_hip_roll": 21,
|
||||
"left_hip_pitch": 22,
|
||||
"left_knee": 23,
|
||||
"left_ankle": 24,
|
||||
"neck_pitch": 30,
|
||||
"head_pitch": 31,
|
||||
"head_yaw": 32,
|
||||
"head_roll": 33,
|
||||
# "left_antenna": None,
|
||||
# "right_antenna": None,
|
||||
"right_hip_yaw": 10,
|
||||
"right_hip_roll": 11,
|
||||
"right_hip_pitch": 12,
|
||||
"right_knee": 13,
|
||||
"right_ankle": 14,
|
||||
}
|
||||
|
||||
self.zero_pos = {
|
||||
"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,
|
||||
# "left_antenna":0,
|
||||
# "right_antenna":0,
|
||||
"right_hip_yaw": 0,
|
||||
"right_hip_roll": 0,
|
||||
"right_hip_pitch": 0,
|
||||
"right_knee": 0,
|
||||
"right_ankle": 0,
|
||||
}
|
||||
|
||||
self.init_pos = {
|
||||
"left_hip_yaw": 0.002,
|
||||
"left_hip_roll": 0.053,
|
||||
"left_hip_pitch": -0.63,
|
||||
"left_knee": 1.368,
|
||||
"left_ankle": -0.784,
|
||||
"neck_pitch": 0.002,
|
||||
"head_pitch": 0.0,
|
||||
"head_yaw": 0,
|
||||
"head_roll": 0,
|
||||
# "left_antenna": 0,
|
||||
# "right_antenna": 0,
|
||||
"right_hip_yaw": -0.003,
|
||||
"right_hip_roll": -0.065,
|
||||
"right_hip_pitch": 0.635,
|
||||
"right_knee": 1.379,
|
||||
"right_ankle": -0.796,
|
||||
}
|
||||
|
||||
# self.init_pos = self.zero_pos # TODO REMOVE
|
||||
|
||||
self.joints_offsets = {
|
||||
"left_hip_yaw": 0.07,
|
||||
"left_hip_roll": -0.1,
|
||||
"left_hip_pitch": 0.0,
|
||||
"left_knee": 0.05,
|
||||
"left_ankle": -0.1,
|
||||
"neck_pitch": 0.1,
|
||||
"head_pitch": 0.1,
|
||||
"head_yaw": 0,
|
||||
"head_roll": 0.1,
|
||||
# "left_antenna": 0,
|
||||
# "right_antenna": 0,
|
||||
"right_hip_yaw": -0.15,
|
||||
"right_hip_roll": 0.07,
|
||||
"right_hip_pitch": 0.05,
|
||||
"right_knee": -0.05,
|
||||
"right_ankle": -0.08,
|
||||
}
|
||||
|
||||
def set_pid(self, pid, joint_name):
|
||||
# TODO
|
||||
pass
|
||||
|
||||
def set_pid_all(self, pid):
|
||||
# TODO WEIRD THINGS HERE
|
||||
# If I set 32 manually, it works, but if it comes from a variable, it doesn't.
|
||||
# sometimes i get : ValueError: byte must be in range(0, 256)
|
||||
# sometimes 32 becomes 76 somehow
|
||||
# P = np.uint8(pid[0])
|
||||
# I = np.uint8(pid[1])
|
||||
# D = np.uint8(pid[2])
|
||||
|
||||
self.dxl_io.set_P_coefficient({id: 32 for id in self.joints.values()})
|
||||
self.dxl_io.set_I_coefficient({id: 0 for id in self.joints.values()})
|
||||
self.dxl_io.set_D_coefficient({id: 32 for id in self.joints.values()})
|
||||
|
||||
def get_pid_all(self):
|
||||
Ps = self.dxl_io.get_P_coefficient(self.joints.values())
|
||||
Is = self.dxl_io.get_I_coefficient(self.joints.values())
|
||||
Ds = self.dxl_io.get_D_coefficient(self.joints.values())
|
||||
print("Ps", Ps)
|
||||
print("Is", Is)
|
||||
print("Ds", Ds)
|
||||
|
||||
return Ps, Is, Ds
|
||||
|
||||
def turn_on(self):
|
||||
self.dxl_io.enable_torque(self.joints.values())
|
||||
self.dxl_io.set_mode({id: 0 for id in self.joints.values()})
|
||||
# self.dxl_io.set_mode
|
||||
self.dxl_io.set_acceleration({id: 16 for id in self.joints.values()})
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
self.set_position_all(self.init_pos)
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
for name, id in self.joints.items():
|
||||
if "neck" in name or "head" in name:
|
||||
self.dxl_io.set_acceleration({id: 32})
|
||||
else:
|
||||
self.dxl_io.set_acceleration({id: 254})
|
||||
|
||||
def turn_off(self):
|
||||
self.dxl_io.disable_torque(self.joints.values())
|
||||
|
||||
def set_position_all(self, joints_positions):
|
||||
"""
|
||||
joints_positions is a dictionary with joint names as keys and joint positions as values
|
||||
Warning: expects radians
|
||||
"""
|
||||
ids_positions = {
|
||||
self.joints[joint]: np.rad2deg(position + self.joints_offsets[joint])
|
||||
for joint, position in joints_positions.items()
|
||||
}
|
||||
|
||||
self.dxl_io.set_goal_position(ids_positions)
|
||||
|
||||
def set_position(self, joint_name, position):
|
||||
self.dxl_io.set_goal_position({self.joints[joint_name]: np.rad2deg(-position)})
|
||||
|
||||
def get_present_positions(self):
|
||||
"""
|
||||
Returns the present positions in radians
|
||||
"""
|
||||
present_positions = np.deg2rad(
|
||||
self.dxl_io.get_present_position(self.joints.values())
|
||||
)
|
||||
present_positions = [
|
||||
pos - self.joints_offsets[joint]
|
||||
for joint, pos in zip(self.joints.keys(), present_positions)
|
||||
]
|
||||
return np.array(np.around(present_positions, 3))
|
||||
|
||||
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.dxl_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))
|
||||
|
|
@ -1,139 +0,0 @@
|
|||
from mini_bdx_runtime.feetech import (
|
||||
FeetechMotorsBus,
|
||||
convert_radians_to_steps,
|
||||
convert_steps_to_radians,
|
||||
configure,
|
||||
)
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
# Everything is in radians here
|
||||
|
||||
|
||||
class HWI:
|
||||
def __init__(self, usb_port="/dev/ttyACM0"):
|
||||
self.motors = {
|
||||
"right_hip_yaw": (10, "sts3215"),
|
||||
"right_hip_roll": (11, "sts3215"),
|
||||
"right_hip_pitch": (12, "sts3215"),
|
||||
"right_knee": (13, "sts3215"),
|
||||
"right_ankle": (14, "sts3215"),
|
||||
"left_hip_yaw": (20, "sts3215"),
|
||||
"left_hip_roll": (21, "sts3215"),
|
||||
"left_hip_pitch": (22, "sts3215"),
|
||||
"left_knee": (23, "sts3215"),
|
||||
"left_ankle": (24, "sts3215"),
|
||||
# "neck_pitch": (30, "sts3215"),
|
||||
# "head_pitch": (31, "sts3215"),
|
||||
# "head_yaw": (32, "sts3215"),
|
||||
}
|
||||
|
||||
self.joints_offsets = {
|
||||
"right_hip_yaw": 0.0,
|
||||
"right_hip_roll": 0.0,
|
||||
"right_hip_pitch": 0.0,
|
||||
"right_knee": 0.0,
|
||||
"right_ankle": 0.0,
|
||||
"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,
|
||||
}
|
||||
|
||||
self.joints_sign = {
|
||||
"right_hip_yaw": -1,
|
||||
"right_hip_roll": -1,
|
||||
"right_hip_pitch": 1,
|
||||
"right_knee": -1,
|
||||
"right_ankle": -1,
|
||||
"left_hip_yaw": -1,
|
||||
"left_hip_roll": -1,
|
||||
"left_hip_pitch": 1,
|
||||
"left_knee": -1,
|
||||
"left_ankle": -1,
|
||||
# "neck_pitch": -1,
|
||||
# "head_pitch": -1,
|
||||
# "head_yaw": -1,
|
||||
}
|
||||
|
||||
self.init_pos = {
|
||||
"right_hip_yaw": 0.001171696610228082,
|
||||
"right_hip_roll": 0.006726989242258406,
|
||||
"right_hip_pitch": 1.0129772861831692,
|
||||
"right_knee": -1.4829304760981399,
|
||||
"right_ankle": 0.6444901047812701,
|
||||
"left_hip_yaw": -0.002853397830292128,
|
||||
"left_hip_roll": 0.01626303761810685,
|
||||
"left_hip_pitch": -1.0105624704499077,
|
||||
"left_knee": -1.4865015965817336,
|
||||
"left_ankle": 0.6504953719748071,
|
||||
# "neck_pitch": -0.17453292519943295,
|
||||
# "head_pitch": -0.17453292519943295,
|
||||
# "head_yaw": 0,
|
||||
}
|
||||
self.zero_pos = {
|
||||
"right_hip_yaw": 0,
|
||||
"right_hip_roll": 0,
|
||||
"right_hip_pitch": 0,
|
||||
"right_knee": 0,
|
||||
"right_ankle": 0,
|
||||
"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,
|
||||
}
|
||||
self.baudrate = 1000000
|
||||
|
||||
self.motors_bus = FeetechMotorsBus(port=usb_port, motors=self.motors)
|
||||
self.motors_bus.connect()
|
||||
configure(self.motors_bus)
|
||||
|
||||
# We add 180 to all dofs. They were mounted in a way that 180 degrees is the neutral position.
|
||||
self.global_offset = np.deg2rad(180)
|
||||
|
||||
def enable_torque(self):
|
||||
self.motors_bus.write("Torque_Enable", 1)
|
||||
|
||||
def disable_torque(self):
|
||||
self.motors_bus.write("Torque_Enable", 0)
|
||||
|
||||
def turn_on(self):
|
||||
self.enable_torque()
|
||||
self.set_position_all(self.init_pos)
|
||||
# self.set_position_all(self.zero_pos)
|
||||
time.sleep(1)
|
||||
|
||||
def set_position_all(self, joints_positions: dict):
|
||||
positions = np.array(list(joints_positions.values()))
|
||||
signs = np.array(list(self.joints_sign.values()))
|
||||
rads = positions * signs + self.global_offset
|
||||
rads = list(rads)
|
||||
|
||||
steps = convert_radians_to_steps(rads, ["sts3215"])
|
||||
self.motors_bus.write("Goal_Position", steps)
|
||||
|
||||
def get_position_all(self):
|
||||
|
||||
steps = self.motors_bus.read("Present_Position", self.motors)
|
||||
rads = np.array(convert_steps_to_radians(steps, ["sts3215"]))
|
||||
return rads - self.global_offset
|
||||
# return rads * np.array(list(self.joints_sign.values()))# - self.global_offset
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
hwi = HWI()
|
||||
hwi.turn_on()
|
||||
|
||||
# hwi.disable_torque()
|
||||
# time.sleep(1)
|
||||
# while True:
|
||||
# print(hwi.get_position_all())
|
||||
# time.sleep(0.1)
|
||||
|
|
@ -1,221 +0,0 @@
|
|||
import pypot.dynamixel.conversion as conv
|
||||
from pypot.dynamixel.io.abstract_io import AbstractDxlIO, _DxlAccess, _DxlControl
|
||||
from pypot.dynamixel.protocol import v2 as v2
|
||||
|
||||
max_pos = 4096
|
||||
max_deg = 360
|
||||
max_current = 1750
|
||||
|
||||
|
||||
def dxl_to_degree(value, model):
|
||||
return round(((max_deg * float(value)) / (max_pos - 1)) - (max_deg / 2), 2)
|
||||
|
||||
|
||||
def dxl_to_current(value, model):
|
||||
if value > 0x7FFF:
|
||||
value = value - 65536
|
||||
return value
|
||||
|
||||
|
||||
def current_to_dxl(value, model):
|
||||
if value < 0:
|
||||
value = value + 65536
|
||||
return int(value)
|
||||
|
||||
|
||||
def degree_to_dxl(value, model):
|
||||
pos = int(round((max_pos - 1) * ((max_deg / 2 + float(value)) / max_deg), 0))
|
||||
pos = min(max(pos, 0), max_pos - 1)
|
||||
|
||||
return pos
|
||||
|
||||
|
||||
def dxl_to_velocity(value, model):
|
||||
if value > 2 ** (4 * 8 - 1):
|
||||
value = value - 2 ** (4 * 8)
|
||||
return value * 0.229
|
||||
|
||||
|
||||
class Dxl330IO(AbstractDxlIO):
|
||||
_protocol = v2
|
||||
|
||||
|
||||
controls = {
|
||||
# EEPROM
|
||||
"model": {
|
||||
"address": 0x00,
|
||||
"access": _DxlAccess.readonly,
|
||||
"dxl_to_si": conv.dxl_to_model,
|
||||
},
|
||||
"firmware": {"address": 0x026, "length": 1, "access": _DxlAccess.readonly},
|
||||
"id": {
|
||||
"address": 0x07,
|
||||
"length": 1,
|
||||
"access": _DxlAccess.writeonly,
|
||||
"setter_name": "change_id",
|
||||
},
|
||||
"baudrate": {
|
||||
"address": 0x08,
|
||||
"length": 1,
|
||||
"access": _DxlAccess.writeonly,
|
||||
"setter_name": "change_baudrate",
|
||||
"si_to_dxl": conv.baudrate_to_dxl,
|
||||
},
|
||||
"return delay time": {
|
||||
"address": 0x09,
|
||||
"length": 1,
|
||||
"dxl_to_si": conv.dxl_to_rdt,
|
||||
"si_to_dxl": conv.rdt_to_dxl,
|
||||
},
|
||||
"angle limit": {
|
||||
"address": 0x30,
|
||||
"nb_elem": 2,
|
||||
"dxl_to_si": lambda value, model: (
|
||||
dxl_to_degree(value[0], model),
|
||||
dxl_to_degree(value[1], model),
|
||||
),
|
||||
"si_to_dxl": lambda value, model: (
|
||||
degree_to_dxl(value[0], model),
|
||||
degree_to_dxl(value[1], model),
|
||||
),
|
||||
},
|
||||
"control mode": {
|
||||
"address": 0x10,
|
||||
"length": 1,
|
||||
"dxl_to_si": conv.dxl_to_control_mode,
|
||||
"si_to_dxl": conv.control_mode_to_dxl,
|
||||
},
|
||||
"operating mode": {
|
||||
"address": 0xB,
|
||||
"length": 1,
|
||||
},
|
||||
"highest temperature limit": {
|
||||
"address": 0x1F,
|
||||
"length": 1,
|
||||
"dxl_to_si": conv.dxl_to_temperature,
|
||||
"si_to_dxl": conv.temperature_to_dxl,
|
||||
},
|
||||
"voltage limit": {
|
||||
"address": 0x20,
|
||||
"length": 1,
|
||||
"nb_elem": 2,
|
||||
"dxl_to_si": lambda value, model: (
|
||||
conv.dxl_to_voltage(value[0], model),
|
||||
conv.dxl_to_voltage(value[1], model),
|
||||
),
|
||||
"si_to_dxl": lambda value, model: (
|
||||
conv.voltage_to_dxl(value[0], model),
|
||||
conv.voltage_to_dxl(value[1], model),
|
||||
),
|
||||
},
|
||||
"current limit": {
|
||||
"address": 0x26,
|
||||
"length": 2,
|
||||
},
|
||||
# RAM
|
||||
"torque_enable": {
|
||||
"address": 0x40,
|
||||
"length": 1,
|
||||
"dxl_to_si": conv.dxl_to_bool,
|
||||
"si_to_dxl": conv.bool_to_dxl,
|
||||
"getter_name": "is_torque_enabled",
|
||||
"setter_name": "_set_torque_enable",
|
||||
},
|
||||
"LED": {
|
||||
"address": 0x41,
|
||||
"length": 1,
|
||||
"dxl_to_si": conv.dxl_to_bool,
|
||||
"si_to_dxl": conv.bool_to_dxl,
|
||||
"setter_name": "_set_LED",
|
||||
"getter_name": "is_led_on",
|
||||
},
|
||||
"LED color": {
|
||||
"address": 0x19,
|
||||
"length": 1,
|
||||
"dxl_to_si": conv.dxl_to_led_color,
|
||||
"si_to_dxl": conv.led_color_to_dxl,
|
||||
},
|
||||
"pid gain": {
|
||||
"address": 0x50,
|
||||
"length": 2,
|
||||
"nb_elem": 3,
|
||||
# "dxl_to_si": conv.dxl_to_pid,
|
||||
# "si_to_dxl": conv.pid_to_dxl,
|
||||
},
|
||||
"goal position": {
|
||||
"address": 0x74,
|
||||
"length": 4,
|
||||
"dxl_to_si": dxl_to_degree,
|
||||
"si_to_dxl": degree_to_dxl,
|
||||
},
|
||||
"present velocity": {
|
||||
"address": 0x80,
|
||||
"length": 4,
|
||||
"access": _DxlAccess.readonly,
|
||||
"dxl_to_si": dxl_to_velocity,
|
||||
},
|
||||
"present position": {
|
||||
"address": 0x84,
|
||||
"length": 4,
|
||||
"access": _DxlAccess.readonly,
|
||||
"dxl_to_si": dxl_to_degree,
|
||||
},
|
||||
"present current": {
|
||||
"address": 0x7E,
|
||||
"length": 2,
|
||||
"access": _DxlAccess.readonly,
|
||||
"dxl_to_si": dxl_to_current,
|
||||
},
|
||||
"goal current": {
|
||||
"address": 0x66,
|
||||
"length": 2,
|
||||
"dxl_to_si": dxl_to_current,
|
||||
"si_to_dxl": current_to_dxl,
|
||||
},
|
||||
"present input voltage": {
|
||||
"address": 0x90,
|
||||
"length": 2,
|
||||
"access": _DxlAccess.readonly,
|
||||
"dxl_to_si": conv.dxl_to_voltage,
|
||||
},
|
||||
"present temperature": {
|
||||
"address": 0x92,
|
||||
"length": 1,
|
||||
"access": _DxlAccess.readonly,
|
||||
"dxl_to_si": conv.dxl_to_temperature,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _add_control(
|
||||
name,
|
||||
address,
|
||||
length=2,
|
||||
nb_elem=1,
|
||||
access=_DxlAccess.readwrite,
|
||||
models=[
|
||||
"XL-330",
|
||||
],
|
||||
dxl_to_si=lambda val, model: val,
|
||||
si_to_dxl=lambda val, model: val,
|
||||
getter_name=None,
|
||||
setter_name=None,
|
||||
):
|
||||
control = _DxlControl(
|
||||
name,
|
||||
address,
|
||||
length,
|
||||
nb_elem,
|
||||
access,
|
||||
models,
|
||||
dxl_to_si,
|
||||
si_to_dxl,
|
||||
getter_name,
|
||||
setter_name,
|
||||
)
|
||||
|
||||
Dxl330IO._generate_accessors(control)
|
||||
|
||||
|
||||
for name, args in controls.items():
|
||||
_add_control(name, **args)
|
||||
|
|
@ -1,214 +0,0 @@
|
|||
import time
|
||||
from typing import List
|
||||
|
||||
import numpy as np
|
||||
|
||||
# from pypot.feetech import FeetechSTS3215IO
|
||||
# from mini_bdx_runtime.feetech_pwm_control import FeetechPWMControl
|
||||
# from mini_bdx_runtime.feetech_pwm_control_rustypot import FeetechPWMControl
|
||||
import rustypot
|
||||
|
||||
|
||||
class HWI:
|
||||
def __init__(self, usb_port="/dev/ttyACM0"):
|
||||
|
||||
# Order matters here
|
||||
self.joints = {
|
||||
"left_hip_yaw": 20,
|
||||
"left_hip_roll": 21,
|
||||
"left_hip_pitch": 22,
|
||||
"left_knee": 23,
|
||||
"left_ankle": 24,
|
||||
"neck_pitch": 30,
|
||||
"head_pitch": 31,
|
||||
"head_yaw": 32,
|
||||
"head_roll": 33,
|
||||
# "left_antenna": None,
|
||||
# "right_antenna": None,
|
||||
"right_hip_yaw": 10,
|
||||
"right_hip_roll": 11,
|
||||
"right_hip_pitch": 12,
|
||||
"right_knee": 13,
|
||||
"right_ankle": 14,
|
||||
}
|
||||
|
||||
self.zero_pos = {
|
||||
"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,
|
||||
# "left_antenna":0,
|
||||
# "right_antenna":0,
|
||||
"right_hip_yaw": 0,
|
||||
"right_hip_roll": 0,
|
||||
"right_hip_pitch": 0,
|
||||
"right_knee": 0,
|
||||
"right_ankle": 0,
|
||||
}
|
||||
|
||||
self.init_pos = {
|
||||
"left_hip_yaw": 0.002,
|
||||
"left_hip_roll": 0.053,
|
||||
"left_hip_pitch": -0.63,
|
||||
"left_knee": 1.368,
|
||||
"left_ankle": -0.784,
|
||||
"neck_pitch": 0.0,
|
||||
"head_pitch": 0.0,
|
||||
"head_yaw": 0,
|
||||
"head_roll": 0,
|
||||
# "left_antenna": 0,
|
||||
# "right_antenna": 0,
|
||||
"right_hip_yaw": -0.003,
|
||||
"right_hip_roll": -0.065,
|
||||
"right_hip_pitch": 0.635,
|
||||
"right_knee": 1.379,
|
||||
"right_ankle": -0.796,
|
||||
}
|
||||
|
||||
# self.init_pos = self.zero_pos # TODO REMOVE
|
||||
|
||||
# self.joints_offsets = {
|
||||
# "left_hip_yaw": 0.07,
|
||||
# "left_hip_roll": -0.1,
|
||||
# "left_hip_pitch": 0.0,
|
||||
# "left_knee": 0.05,
|
||||
# "left_ankle": -0.1,
|
||||
# "neck_pitch": 0.1,
|
||||
# "head_pitch": 0.1,
|
||||
# "head_yaw": 0,
|
||||
# "head_roll": 0.1,
|
||||
# # "left_antenna": 0,
|
||||
# # "right_antenna": 0,
|
||||
# "right_hip_yaw": -0.15,
|
||||
# "right_hip_roll": 0.15,
|
||||
# "right_hip_pitch": 0.05,
|
||||
# "right_knee": -0.05,
|
||||
# "right_ankle": -0.08,
|
||||
# }
|
||||
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,
|
||||
}
|
||||
|
||||
init_pos_with_offsets = {
|
||||
joint: np.rad2deg(pos + self.joints_offsets[joint])
|
||||
for joint, pos in self.init_pos.items()
|
||||
}
|
||||
|
||||
# self.control = FeetechPWMControl(
|
||||
# ids=list(self.joints.values()),
|
||||
# init_pos_rad=list(init_pos_with_offsets.values()),
|
||||
# usb_port=usb_port,
|
||||
# )
|
||||
|
||||
self.kps = np.ones(len(self.joints)) * 32 # default kp
|
||||
self.low_torque_kps = np.ones(len(self.joints)) * 8 # default kp
|
||||
|
||||
self.control = rustypot.FeetechController(
|
||||
usb_port, 1000000, 100, list(self.joints.values()), list(self.kps), list(init_pos_with_offsets.values())
|
||||
)
|
||||
|
||||
def set_kps(self, kps):
|
||||
self.kps = kps
|
||||
self.control.set_new_kps(self.kps)
|
||||
|
||||
def turn_on(self):
|
||||
self.control.set_new_kps(self.low_torque_kps)
|
||||
print("turn on : low KPS set")
|
||||
time.sleep(1)
|
||||
|
||||
self.set_position_all(self.init_pos)
|
||||
print("turn on : init pos set")
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
self.control.set_new_kps(self.kps)
|
||||
print("turn on : high kps")
|
||||
|
||||
def turn_off(self):
|
||||
self.control.disable_torque()
|
||||
|
||||
def freeze(self):
|
||||
self.control.freeze()
|
||||
|
||||
def set_position_all(self, joints_positions):
|
||||
"""
|
||||
joints_positions is a dictionary with joint names as keys and joint positions as values
|
||||
Warning: expects radians
|
||||
"""
|
||||
ids_positions = {
|
||||
self.joints[joint]: np.rad2deg(position + self.joints_offsets[joint])
|
||||
for joint, position in joints_positions.items()
|
||||
}
|
||||
|
||||
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())
|
||||
# )
|
||||
|
||||
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)
|
||||
if joint not in ignore
|
||||
]
|
||||
return np.array(np.around(present_positions, 3))
|
||||
|
||||
def get_present_velocities(self, rad_s=True, ignore=[]):
|
||||
"""
|
||||
Returns the present velocities in rad/s (default) or rev/min
|
||||
"""
|
||||
# deg/s
|
||||
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))
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
from pypot.feetech import FeetechSTS3215IO
|
||||
|
||||
io = FeetechSTS3215IO(
|
||||
"/dev/ttyACM0",
|
||||
baudrate=1000000,
|
||||
use_sync_read=True,
|
||||
)
|
||||
|
||||
joints = {
|
||||
"left_hip_yaw": 20,
|
||||
"left_hip_roll": 21,
|
||||
"left_hip_pitch": 22,
|
||||
"left_knee": 23,
|
||||
"left_ankle": 24,
|
||||
"neck_pitch": 30,
|
||||
"head_pitch": 31,
|
||||
"head_yaw": 32,
|
||||
"head_roll": 33,
|
||||
# "left_antenna": None,
|
||||
# "right_antenna": None,
|
||||
"right_hip_yaw": 10,
|
||||
"right_hip_roll": 11,
|
||||
"right_hip_pitch": 12,
|
||||
"right_knee": 13,
|
||||
"right_ankle": 14,
|
||||
}
|
||||
|
||||
|
||||
voltages = io.get_present_voltage(list(joints.values()))
|
||||
for i, name in enumerate(joints.keys()):
|
||||
print(name, round(voltages[i] * 0.1, 2), "V")
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
import numpy as np
|
||||
|
||||
import RPi.GPIO as GPIO
|
||||
import time
|
||||
# from time import sleep
|
||||
|
||||
GPIO.setwarnings(False)
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(12, GPIO.OUT)
|
||||
pwm = GPIO.PWM(12, 50)
|
||||
pwm.start(0) # Start the servo with 0 duty cycle ( at 0 deg position )
|
||||
|
||||
# pwm.ChangeDutyCycle(0)
|
||||
# time.sleep(1)
|
||||
|
||||
# pwm.stop(0) # Stop the servo with 0 duty cycle ( at 0 deg position )
|
||||
# GPIO.cleanup() # Clean up all the ports we've used.
|
||||
|
||||
start = time.time()
|
||||
while True:
|
||||
if time.time() - start > 3:
|
||||
break
|
||||
target_pwm = 10.0*np.sin(2*np.pi*1*time.time()) + 10
|
||||
pwm.ChangeDutyCycle(target_pwm)
|
||||
time.sleep(0.02)
|
||||
|
||||
# # Set up the PWM on pin #16 at 50Hz
|
||||
# pwm.ChangeDutyCycle(5) # Tells the servo to turn to the left ( -90 deg position )
|
||||
# sleep(0.5) # Tells the servo to Delay for 5sec
|
||||
# pwm.ChangeDutyCycle(7.5) # Tells the servo to turn to the neutral position ( at 0 deg position )
|
||||
# sleep(0.5) # Tells the servo to Delay for 5sec
|
||||
# pwm.ChangeDutyCycle(10) # Tells the servo to turn to the right ( +90 deg position )
|
||||
# sleep(0.5) # Tells the servo to Delay for 5sec
|
||||
pwm.stop(0) # Stop the servo with 0 duty cycle ( at 0 deg position )
|
||||
GPIO.cleanup() # Clean up all the ports we've used.
|
||||
|
|
@ -2,9 +2,6 @@
|
|||
Sets up the robot in init position, you control the head with the xbox controller
|
||||
"""
|
||||
|
||||
|
||||
# from pypot.feetech import FeetechSTS3215IO
|
||||
# from mini_bdx_runtime.hwi_feetech_pypot import HWI
|
||||
from mini_bdx_runtime.rustypot_position_hwi import HWI
|
||||
from mini_bdx_runtime.eyes import Eyes
|
||||
import time
|
||||
|
|
|
|||
|
|
@ -1,88 +0,0 @@
|
|||
import socket
|
||||
import time
|
||||
import numpy as np
|
||||
import pickle
|
||||
from queue import Queue
|
||||
from threading import Thread
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
from FramesViewer.viewer import Viewer
|
||||
|
||||
from mini_bdx_runtime.rl_utils import quat_rotate_inverse
|
||||
|
||||
|
||||
class IMUClient:
|
||||
def __init__(self, host, port=1234, freq=30):
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.freq = freq
|
||||
self.client_socket = socket.socket()
|
||||
self.connected = False
|
||||
while not self.connected:
|
||||
try:
|
||||
self.client_socket.connect((self.host, self.port))
|
||||
self.connected = True
|
||||
except Exception as e:
|
||||
print(e)
|
||||
time.sleep(0.5)
|
||||
self.imu_queue = Queue(maxsize=1)
|
||||
self.last_imu = [0, 0, 0, 0]
|
||||
|
||||
Thread(target=self.imu_worker, daemon=True).start()
|
||||
|
||||
def imu_worker(self):
|
||||
while True:
|
||||
try:
|
||||
data = self.client_socket.recv(1024) # receive response
|
||||
data = pickle.loads(data)
|
||||
|
||||
self.imu_queue.put(data)
|
||||
except:
|
||||
print("missed imu")
|
||||
|
||||
time.sleep(1 / self.freq)
|
||||
|
||||
def get_imu(self):
|
||||
try:
|
||||
self.last_imu = self.imu_queue.get(False)
|
||||
except:
|
||||
pass
|
||||
|
||||
return self.last_imu
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
client = IMUClient("192.168.248.253")
|
||||
fv = Viewer()
|
||||
fv.start()
|
||||
pose = np.eye(4)
|
||||
pose[:3, 3] = [0.1, 0.1, 0.1]
|
||||
projected_gravities = []
|
||||
try:
|
||||
while True:
|
||||
quat = client.get_imu()
|
||||
try:
|
||||
rot_mat = R.from_quat(quat).as_matrix()
|
||||
pose[:3, :3] = rot_mat
|
||||
fv.pushFrame(pose, "aze")
|
||||
|
||||
# euler = R.from_quat(quat).as_euler("xyz")
|
||||
# euler[2] = 0
|
||||
# quat = R.from_euler("xyz", euler).as_quat()
|
||||
|
||||
projected_gravity = quat_rotate_inverse(quat, [0, 0, -1])
|
||||
projected_gravities.append(projected_gravity)
|
||||
except Exception as e:
|
||||
print("error", e)
|
||||
pass
|
||||
time.sleep(1 / 30)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
# plot projected_gravities [[x, y, z], [x, y, z], ...]
|
||||
|
||||
# from matplotlib import pyplot as plt
|
||||
|
||||
# projected_gravities = np.array(projected_gravities)
|
||||
# plt.plot(projected_gravities)
|
||||
# plt.show()
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
import socket
|
||||
import time
|
||||
import pickle
|
||||
from mini_bdx_runtime.raw_imu import Imu
|
||||
from threading import Thread
|
||||
import time
|
||||
|
||||
import argparse
|
||||
|
||||
|
||||
class IMUServer:
|
||||
def __init__(self, imu=None):
|
||||
self.host = "0.0.0.0"
|
||||
self.port = 1234
|
||||
|
||||
self.server_socket = socket.socket()
|
||||
self.server_socket.setsockopt(
|
||||
socket.SOL_SOCKET, socket.SO_REUSEADDR, 1
|
||||
) # enable address reuse
|
||||
|
||||
self.server_socket.bind((self.host, self.port))
|
||||
|
||||
if imu is None:
|
||||
self.imu = Imu(50, user_pitch_bias=args.pitch_bias)
|
||||
else:
|
||||
self.imu = imu
|
||||
self.stop = False
|
||||
|
||||
Thread(target=self.run, daemon=True).start()
|
||||
|
||||
def run(self):
|
||||
while not self.stop:
|
||||
self.server_socket.listen(1)
|
||||
conn, address = self.server_socket.accept() # accept new connection
|
||||
print("Connection from: " + str(address))
|
||||
try:
|
||||
while True:
|
||||
data = self.imu.get_data()
|
||||
data = pickle.dumps(data)
|
||||
conn.send(data) # send data to the client
|
||||
time.sleep(1 / 30)
|
||||
except:
|
||||
pass
|
||||
|
||||
self.server_socket.close()
|
||||
print("thread closed")
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--pitch_bias", type=float, default=0, help="deg")
|
||||
args = parser.parse_args()
|
||||
imu_server = IMUServer()
|
||||
try:
|
||||
while True:
|
||||
time.sleep(0.01)
|
||||
except KeyboardInterrupt:
|
||||
print("Closing server")
|
||||
imu_server.stop = True
|
||||
|
||||
time.sleep(2)
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
import RPi.GPIO as GPIO #Importe la bibliothèque pour contrôler les GPIOs
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
GPIO.setmode(GPIO.BCM) # Use physical pin numbering
|
||||
# GPIO.setmode(GPIO.BOARD) #Définit le mode de numérotation (Board)
|
||||
GPIO.setwarnings(False) #On désactive les messages d'alerte
|
||||
LED = 24 #Définit le numéro du port GPIO qui alimente la led
|
||||
|
||||
GPIO.setup(LED, GPIO.OUT) #Active le contrôle du GPIO
|
||||
|
||||
# state = GPIO.input(LED) #Lit l'état actuel du GPIO, vrai si allumé, faux si éteint
|
||||
|
||||
GPIO.output(LED, GPIO.HIGH) #On l'allume
|
||||
|
||||
clignottement_time = 0.1
|
||||
|
||||
cli_s = time.time()
|
||||
low = False
|
||||
while True:
|
||||
if not low and np.random.rand() > 0.99:
|
||||
print("cli")
|
||||
GPIO.output(LED, GPIO.LOW)
|
||||
cli_s = time.time()
|
||||
low = True
|
||||
|
||||
if time.time() - cli_s > clignottement_time:
|
||||
GPIO.output(LED, GPIO.HIGH)
|
||||
low = False
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
|
||||
|
||||
# if state : #Si GPIO allumé
|
||||
# GPIO.output(LED, GPIO.LOW) #On l’éteint
|
||||
# else : #Sinon
|
||||
# GPIO.output(LED, GPIO.HIGH) #On l'allume
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
import pickle
|
||||
|
||||
imu_data = pickle.load(open("imu_data.pkl", "rb"))
|
||||
|
||||
gyros = imu_data["gyros"]
|
||||
acceleros = imu_data["accels"]
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
plt.plot(gyros)
|
||||
plt.show()
|
||||
|
||||
plt.plot(acceleros)
|
||||
plt.show()
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
import RPi.GPIO as GPIO
|
||||
import time
|
||||
|
||||
class ServoController:
|
||||
def __init__(self, pin1, pin2):
|
||||
self.pin1 = pin1
|
||||
self.pin2 = pin2
|
||||
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(self.pin1, GPIO.OUT)
|
||||
GPIO.setup(self.pin2, GPIO.OUT)
|
||||
|
||||
# Create PWM objects for both servos (50Hz frequency)
|
||||
self.pwm1 = GPIO.PWM(self.pin1, 50)
|
||||
self.pwm2 = GPIO.PWM(self.pin2, 50)
|
||||
|
||||
self.pwm1.start(0)
|
||||
self.pwm2.start(0)
|
||||
|
||||
def map_input_to_angle(self, value):
|
||||
"""
|
||||
Maps an input range of [-1, 1] to an angle range of [0°, 180°].
|
||||
"""
|
||||
return 90 + (value * 90) # Maps -1 to 0°, 0 to 90°, and 1 to 180°
|
||||
|
||||
def set_position(self, servo, value):
|
||||
"""
|
||||
Moves the servo based on an input value in the range [-1, 1].
|
||||
:param servo: 1 for the first servo, 2 for the second servo
|
||||
:param value: A float between -1 and 1
|
||||
"""
|
||||
if -1 <= value <= 1:
|
||||
angle = self.map_input_to_angle(value)
|
||||
duty = 2 + (angle / 18) # Convert angle to duty cycle (1ms-2ms)
|
||||
if servo == 1:
|
||||
self.pwm1.ChangeDutyCycle(duty)
|
||||
elif servo == 2:
|
||||
self.pwm2.ChangeDutyCycle(duty)
|
||||
else:
|
||||
print("Invalid servo number!")
|
||||
# time.sleep(0.3) # Allow time for movement
|
||||
else:
|
||||
print("Invalid input! Enter a value between -1 and 1.")
|
||||
|
||||
def stop(self):
|
||||
"""Stops PWM and cleans up GPIO."""
|
||||
self.pwm1.stop()
|
||||
self.pwm2.stop()
|
||||
GPIO.cleanup()
|
||||
|
||||
# Initialize the ServoController for GPIO12 and GPIO13
|
||||
servo_control = ServoController(pin1=12, pin2=13)
|
||||
|
||||
try:
|
||||
while True:
|
||||
servo_num = int(input("Enter servo number (1 or 2): "))
|
||||
value = float(input("Enter position (-1 to 1): "))
|
||||
servo_control.set_position(servo_num, value)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("Stopping servos...")
|
||||
servo_control.stop()
|
||||
|
|
@ -1,75 +0,0 @@
|
|||
''' pg_midi_sound101.py
|
||||
play midi music files (also mp3 files) using pygame
|
||||
tested with Python273/331 and pygame192 by vegaseat
|
||||
'''
|
||||
#code modified by James DeVito from here: https://www.daniweb.com/programming/software-development/code/454835/let-pygame-play-your-midi-or-mp3-files
|
||||
|
||||
|
||||
import sys
|
||||
import pygame as pg
|
||||
import os
|
||||
import time
|
||||
|
||||
|
||||
def play_music(music_file):
|
||||
'''
|
||||
stream music with mixer.music module in blocking manner
|
||||
this will stream the sound from disk while playing
|
||||
'''
|
||||
clock = pg.time.Clock()
|
||||
try:
|
||||
pg.mixer.music.load(music_file)
|
||||
print("Music file {} loaded!".format(music_file))
|
||||
except pygame.error:
|
||||
print("File {} not found! {}".format(music_file, pg.get_error()))
|
||||
return
|
||||
|
||||
pg.mixer.music.play()
|
||||
|
||||
# If you want to fade in the audio...
|
||||
# for x in range(0,100):
|
||||
# pg.mixer.music.set_volume(float(x)/100.0)
|
||||
# time.sleep(.0075)
|
||||
# # check if playback has finished
|
||||
while pg.mixer.music.get_busy():
|
||||
clock.tick(30)
|
||||
|
||||
|
||||
freq = 44100 # audio CD quality
|
||||
bitsize = -16 # unsigned 16 bit
|
||||
channels = 1 # 1 is mono, 2 is stereo
|
||||
buffer = 2048 # number of samples (experiment to get right sound)
|
||||
pg.mixer.init(freq, bitsize, channels, buffer)
|
||||
|
||||
|
||||
if len(sys.argv) > 1:
|
||||
|
||||
try:
|
||||
user_volume = float(sys.argv[1])
|
||||
except ValueError:
|
||||
print("Volume argument invalid. Please use a float (0.0 - 1.0)")
|
||||
pg.mixer.music.fadeout(1000)
|
||||
pg.mixer.music.stop()
|
||||
raise SystemExit
|
||||
|
||||
print("Playing at volume: " + str(user_volume)+ "\n")
|
||||
pg.mixer.music.set_volume(user_volume)
|
||||
mp3s = []
|
||||
for file in os.listdir("."):
|
||||
if file.endswith(".wav"):
|
||||
mp3s.append(file)
|
||||
|
||||
print(mp3s)
|
||||
|
||||
for x in mp3s:
|
||||
try:
|
||||
play_music(x)
|
||||
time.sleep(.25)
|
||||
except KeyboardInterrupt:
|
||||
# if user hits Ctrl/C then exit
|
||||
# (works only in console mode)
|
||||
pg.mixer.music.fadeout(1000)
|
||||
pg.mixer.music.stop()
|
||||
raise SystemExit
|
||||
else:
|
||||
print("Please specify volume as a float! (0.0 - 1.0)")
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
import RPi.GPIO as GPIO
|
||||
|
||||
LEFT_FOOT_PIN = 22
|
||||
RIGHT_FOOT_PIN = 27
|
||||
GPIO.setwarnings(False) # Ignore warning for now
|
||||
GPIO.setmode(GPIO.BCM) # Use physical pin numbering
|
||||
GPIO.setup(LEFT_FOOT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
|
||||
GPIO.setup(RIGHT_FOOT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
|
||||
|
||||
while True: # Run forever
|
||||
print("=")
|
||||
if GPIO.input(LEFT_FOOT_PIN) == GPIO.LOW:
|
||||
print("LEFT!")
|
||||
if GPIO.input(RIGHT_FOOT_PIN) == GPIO.LOW:
|
||||
print("RIGHT! ")
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
from mini_bdx_runtime.xbox_controller import XBoxController
|
||||
import time
|
||||
|
||||
xbox = XBoxController(50)
|
||||
while True:
|
||||
print(xbox.get_last_command())
|
||||
time.sleep(0.1)
|
||||
|
|
@ -1,4 +1,3 @@
|
|||
# from mini_bdx_runtime.hwi_feetech_pypot import HWI
|
||||
from mini_bdx_runtime.rustypot_control_hwi import HWI
|
||||
|
||||
hwi = HWI()
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# from mini_bdx_runtime.hwi_feetech_pypot import HWI
|
||||
from mini_bdx_runtime.rustypot_position_hwi import HWI
|
||||
import time
|
||||
|
||||
|
|
|
|||
|
|
@ -1,11 +1,8 @@
|
|||
import time
|
||||
import pickle
|
||||
|
||||
# import pygame
|
||||
import numpy as np
|
||||
|
||||
# from mini_bdx_runtime.hwi_feetech_pwm_control import HWI
|
||||
# from mini_bdx_runtime.rustypot_control_hwi import HWI
|
||||
from mini_bdx_runtime.rustypot_position_hwi import HWI
|
||||
from mini_bdx_runtime.onnx_infer import OnnxInfer
|
||||
from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter
|
||||
|
|
|
|||
Loading…
Reference in a new issue