mirror of
https://github.com/apirrone/Open_Duck_Mini_Runtime.git
synced 2025-09-05 05:27:49 +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:
|
if GPIO.input(RIGHT_FOOT_PIN) == GPIO.LOW:
|
||||||
right = True
|
right = True
|
||||||
return np.array([left, right])
|
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
|
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.rustypot_position_hwi import HWI
|
||||||
from mini_bdx_runtime.eyes import Eyes
|
from mini_bdx_runtime.eyes import Eyes
|
||||||
import time
|
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
|
from mini_bdx_runtime.rustypot_control_hwi import HWI
|
||||||
|
|
||||||
hwi = 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
|
from mini_bdx_runtime.rustypot_position_hwi import HWI
|
||||||
import time
|
import time
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,11 +1,8 @@
|
||||||
import time
|
import time
|
||||||
import pickle
|
import pickle
|
||||||
|
|
||||||
# import pygame
|
|
||||||
import numpy as np
|
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.rustypot_position_hwi import HWI
|
||||||
from mini_bdx_runtime.onnx_infer import OnnxInfer
|
from mini_bdx_runtime.onnx_infer import OnnxInfer
|
||||||
from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter
|
from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue