mirror of
https://github.com/apirrone/Open_Duck_Mini_Runtime.git
synced 2025-09-02 19:23:54 +00:00
rustypot control loop
This commit is contained in:
apirrone
parent
b5aa4baeb8
commit
200211b50b
3 changed files with 216 additions and 5 deletions
|
|
@ -37,11 +37,8 @@ class FeetechPWMControl:
|
|||
|
||||
def freeze(self):
|
||||
present_position = list(self.io.read_present_position(self.ids))
|
||||
present_position = np.rad2deg(present_position)
|
||||
print(present_position)
|
||||
self.io.write_goal_position(self.ids, np.deg2rad(present_position))
|
||||
self.io.write_goal_position(self.ids, present_position)
|
||||
self.io.set_mode(self.ids, 0)
|
||||
self.io.enable_torque(self.ids)
|
||||
|
||||
def update(self):
|
||||
while True:
|
||||
|
|
|
|||
213
mini_bdx_runtime/mini_bdx_runtime/rustypot_control_hwi.py
Normal file
213
mini_bdx_runtime/mini_bdx_runtime/rustypot_control_hwi.py
Normal file
|
|
@ -0,0 +1,213 @@
|
|||
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: 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)
|
||||
)
|
||||
|
||||
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))
|
||||
|
|
@ -6,7 +6,8 @@ import pygame
|
|||
import numpy as np
|
||||
import RPi.GPIO as GPIO
|
||||
|
||||
from mini_bdx_runtime.hwi_feetech_pwm_control import HWI
|
||||
# from mini_bdx_runtime.hwi_feetech_pwm_control import HWI
|
||||
from mini_bdx_runtime.rustypot_control_hwi import HWI
|
||||
from mini_bdx_runtime.onnx_infer import OnnxInfer
|
||||
from mini_bdx_runtime.rl_utils import (
|
||||
make_action_dict,
|
||||
|
|
|
|||
Loading…
Reference in a new issue