silencht
2 years ago
3 changed files with 87 additions and 379 deletions
-
144teleop/robot_control/robot_arm.py
-
316teleop/robot_control/robot_arm_tem.py
-
6teleop/teleop_hand_and_arm.py
@ -1,316 +0,0 @@ |
|||
import numpy as np |
|||
import threading |
|||
import time |
|||
|
|||
from unitree_dds_wrapper.idl import unitree_hx |
|||
from unitree_dds_wrapper.publisher import Publisher |
|||
from unitree_dds_wrapper.subscription import Subscription |
|||
from unitree_dds_wrapper.utils.crc import crc32 |
|||
import struct |
|||
from enum import IntEnum |
|||
import copy |
|||
kTopicLowCommand = "rt/arm_sdk" |
|||
kTopicLowState = "rt/lowstate_hx" |
|||
kNumMotors = 30 |
|||
|
|||
|
|||
# 定义电机命令类 |
|||
class MotorCommand: |
|||
def __init__(self): |
|||
self.q_ref = np.zeros(kNumMotors) |
|||
self.dq_ref = np.zeros(kNumMotors) |
|||
self.tau_ff = np.zeros(kNumMotors) |
|||
self.kp = np.zeros(kNumMotors) |
|||
self.kd = np.zeros(kNumMotors) |
|||
|
|||
|
|||
class MotorState: |
|||
def __init__(self): |
|||
self.q = np.zeros(kNumMotors) |
|||
self.dq = np.zeros(kNumMotors) |
|||
|
|||
class BaseState: |
|||
def __init__(self): |
|||
self.omega = np.zeros(3) |
|||
self.rpy = np.zeros(3) |
|||
|
|||
class DataBuffer: |
|||
def __init__(self): |
|||
self.data = None |
|||
self.lock = threading.Lock() |
|||
|
|||
def GetData(self): |
|||
with self.lock: |
|||
return self.data |
|||
|
|||
def SetData(self, data): |
|||
with self.lock: |
|||
self.data = data |
|||
|
|||
np.set_printoptions(linewidth=240) |
|||
|
|||
class H1ArmController: |
|||
def __init__(self): |
|||
print("Initialize channel factory.") |
|||
self.q_desList = np.zeros(kNumMotors) |
|||
self.q_tau_ff = np.zeros(kNumMotors) |
|||
self.msg = unitree_hx.msg.dds_.LowCmd_() |
|||
self.msg.head = [0xFE, 0xEF] # 设置消息头 |
|||
self.lowcmd_publisher = Publisher(unitree_hx.msg.dds_.LowCmd_, kTopicLowCommand) |
|||
self.lowstate_subscriber = Subscription(unitree_hx.msg.dds_.LowState_, kTopicLowState) |
|||
|
|||
self.motor_state_buffer = DataBuffer() |
|||
self.motor_command_buffer = DataBuffer() |
|||
self.base_state_buffer = DataBuffer() |
|||
|
|||
self.kp_low = 140.0 |
|||
self.kd_low = 7.5 |
|||
|
|||
self.kp_high = 200.0 |
|||
self.kd_high = 5.0 |
|||
|
|||
self.kp_wrist = 35.0 |
|||
self.kd_wrist = 6.0 |
|||
|
|||
self.control_dt = 0.01 |
|||
self.hip_pitch_init_pos = -0.5 |
|||
self.knee_init_pos = 1.0 |
|||
self.ankle_init_pos = -0.5 |
|||
self.shoulder_pitch_init_pos = -1.4 |
|||
self.time = 0.0 |
|||
self.init_duration = 10.0 |
|||
self.report_dt = 0.1 |
|||
self.ratio = 0.0 |
|||
self.q_target = [] |
|||
while not self.lowstate_subscriber.msg: |
|||
time.sleep(0.01) |
|||
|
|||
for id in JointIndex: |
|||
self.msg.motor_cmd[id].q = self.lowstate_subscriber.msg.motor_state[id].q |
|||
self.q_target.append(self.msg.motor_cmd[id].q) |
|||
|
|||
duration = 1000 |
|||
init_q = np.array([self.lowstate_subscriber.msg.motor_state[id].q for id in JointIndex]) |
|||
print("Lock Leg...") |
|||
for i in range(duration): |
|||
time.sleep(0.001) |
|||
q_t = init_q + (self.q_target - init_q) * i / duration |
|||
for i, id in enumerate(JointIndex): |
|||
self.msg.motor_cmd[id].mode = 1 |
|||
if id not in JointArmIndex: |
|||
self.msg.motor_cmd[id].kp = 200 |
|||
self.msg.motor_cmd[id].kd = 5 |
|||
self.msg.motor_cmd[id].q = q_t[i] |
|||
self.pre_communication() |
|||
self.lowcmd_publisher.msg = self.msg |
|||
self.lowcmd_publisher.write() # 发布命令 |
|||
|
|||
|
|||
self.report_rpy_thread = threading.Thread(target=self.SubscribeState) |
|||
self.report_rpy_thread.start() |
|||
|
|||
self.control_thread = threading.Thread(target=self.Control) |
|||
self.control_thread.start() |
|||
|
|||
self.command_writer_thread = threading.Thread(target=self.LowCommandWriter) |
|||
self.command_writer_thread.start() |
|||
|
|||
|
|||
def LowStateHandler(self, message): |
|||
low_state = message |
|||
self.RecordMotorState(low_state) |
|||
self.RecordBaseState(low_state) |
|||
|
|||
def SetMotorPose(self,q_desList,q_tau_ff): |
|||
self.q_desList = q_desList |
|||
self.q_tau_ff = q_tau_ff |
|||
|
|||
def pre_communication(self): |
|||
self.__pack_crc() |
|||
def __pack_crc(self): |
|||
rawdata = [] |
|||
rawdata.extend(self.msg.head) |
|||
rawdata.extend(self.msg.version) |
|||
for i in range(len(self.msg.motor_cmd)): |
|||
rawdata.append(self.msg.motor_cmd[i].mode) |
|||
rawdata.append(self.msg.motor_cmd[i].q) |
|||
rawdata.append(self.msg.motor_cmd[i].dq) |
|||
rawdata.append(self.msg.motor_cmd[i].tau) |
|||
rawdata.append(self.msg.motor_cmd[i].kp) |
|||
rawdata.append(self.msg.motor_cmd[i].kd) |
|||
rawdata.extend(self.msg.motor_cmd[i].reserve) |
|||
rawdata.append(self.msg.bms_cmd.cmd) |
|||
rawdata.extend(self.msg.bms_cmd.reserve) |
|||
rawdata.extend(self.msg.led_cmd) |
|||
rawdata.extend(self.msg.fan_cmd) |
|||
rawdata.extend(self.msg.cmd) |
|||
rawdata.extend(self.msg.data) |
|||
rawdata.extend(self.msg.reserve) |
|||
rawdata.append(self.msg.crc) |
|||
|
|||
packdata = struct.pack(self.__packCRCformat, *rawdata) |
|||
calcdata = [] |
|||
calclen = (len(packdata)>>2)-1 |
|||
for i in range(calclen): |
|||
d = ((packdata[i*4+3] << 24) | (packdata[i*4+2] << 16) | (packdata[i*4+1] << 8) | (packdata[i*4])) |
|||
calcdata.append(d) |
|||
|
|||
self.msg.crc = crc32(calcdata) |
|||
|
|||
def LowCommandWriter(self): |
|||
while True: |
|||
mc_tmp_ptr = self.motor_command_buffer.GetData() |
|||
# rawdata.extend(self.msg.version) |
|||
if mc_tmp_ptr: |
|||
for i in JointArmIndex: |
|||
self.msg.motor_cmd[i].tau = mc_tmp_ptr.tau_ff[i] |
|||
self.msg.motor_cmd[i].q = mc_tmp_ptr.q_ref[i] |
|||
self.msg.motor_cmd[i].dq = mc_tmp_ptr.dq_ref[i] |
|||
self.msg.motor_cmd[i].kp = mc_tmp_ptr.kp[i] |
|||
self.msg.motor_cmd[i].kd = mc_tmp_ptr.kd[i] |
|||
self.pre_communication() |
|||
self.lowcmd_publisher.msg = self.msg |
|||
self.lowcmd_publisher.write() # 发布命令 |
|||
time.sleep(0.002) # 控制循环周期 |
|||
|
|||
def Control(self): |
|||
while True: |
|||
ms_tmp_ptr_0 = self.motor_state_buffer.GetData() |
|||
if ms_tmp_ptr_0: |
|||
tem_q_desList = copy.deepcopy(self.q_desList) |
|||
tem_q_tau_ff = copy.deepcopy(self.q_tau_ff) |
|||
motor_command_tmp = MotorCommand() |
|||
ms_tmp_ptr = self.motor_state_buffer.GetData() |
|||
self.time += self.control_dt |
|||
self.time = min(max(self.time, 0.0), self.init_duration) |
|||
self.ratio = self.time / self.init_duration |
|||
for i in range(kNumMotors): |
|||
if self.IsWeakMotor(i): |
|||
motor_command_tmp.kp[i] = self.kp_low |
|||
motor_command_tmp.kd[i] = self.kd_low |
|||
elif self.IsWristMotor(i): |
|||
motor_command_tmp.kp[i] = self.kp_wrist |
|||
motor_command_tmp.kd[i] = self.kd_wrist |
|||
else: |
|||
motor_command_tmp.kp[i] = self.kp_high |
|||
motor_command_tmp.kd[i] = self.kd_high |
|||
motor_command_tmp.dq_ref[i] = 0.0 |
|||
motor_command_tmp.tau_ff[i] = tem_q_tau_ff[i] |
|||
q_des = tem_q_desList[i] |
|||
|
|||
q_des = (q_des - ms_tmp_ptr.q[i]) * self.ratio + ms_tmp_ptr.q[i] |
|||
motor_command_tmp.q_ref[i] = q_des |
|||
self.motor_command_buffer.SetData(motor_command_tmp) |
|||
time.sleep(0.002) |
|||
|
|||
def GetMotorState(self): |
|||
ms_tmp_ptr = self.motor_state_buffer.GetData() |
|||
if ms_tmp_ptr: |
|||
return ms_tmp_ptr.q[13:27],ms_tmp_ptr.dq[13:27] |
|||
else: |
|||
return None,None |
|||
|
|||
def SubscribeState(self): |
|||
while True: |
|||
if self.lowstate_subscriber.msg: |
|||
self.LowStateHandler(self.lowstate_subscriber.msg) |
|||
time.sleep(0.002) |
|||
|
|||
def RecordMotorState(self, msg): |
|||
ms_tmp = MotorState() |
|||
for i in range(kNumMotors): |
|||
ms_tmp.q[i] = msg.motor_state[i].q |
|||
ms_tmp.dq[i] = msg.motor_state[i].dq |
|||
self.motor_state_buffer.SetData(ms_tmp) |
|||
|
|||
def RecordBaseState(self, msg): |
|||
bs_tmp = BaseState() |
|||
bs_tmp.omega = msg.imu_state.gyroscope |
|||
bs_tmp.rpy = msg.imu_state.rpy |
|||
self.base_state_buffer.SetData(bs_tmp) |
|||
|
|||
def IsWeakMotor(self, motor_index): |
|||
weak_motors = [ |
|||
JointIndex.kLeftAnkle, |
|||
JointIndex.kRightAnkle, |
|||
JointIndex.kRightShoulderPitch, |
|||
JointIndex.kRightShoulderRoll, |
|||
JointIndex.kRightShoulderYaw, |
|||
JointIndex.kRightElbow, |
|||
JointIndex.kLeftShoulderPitch, |
|||
JointIndex.kLeftShoulderRoll, |
|||
JointIndex.kLeftShoulderYaw, |
|||
JointIndex.kLeftElbow |
|||
] |
|||
return motor_index in weak_motors |
|||
|
|||
def IsWristMotor(self, motor_index): |
|||
weak_motors = [ |
|||
JointIndex.kLeftWristYaw, |
|||
JointIndex.kLeftWristPitch, |
|||
JointIndex.kLeftWristRoll, |
|||
JointIndex.kRightWristYaw, |
|||
JointIndex.kRightWristPitch, |
|||
JointIndex.kRightWristRoll |
|||
] |
|||
return motor_index in weak_motors |
|||
|
|||
class JointArmIndex(IntEnum): |
|||
# Left arm |
|||
kLeftShoulderPitch = 13 |
|||
kLeftShoulderRoll = 14 |
|||
kLeftShoulderYaw = 15 |
|||
kLeftElbow = 16 |
|||
kLeftWristYaw = 17 |
|||
kLeftWristPitch = 18 |
|||
kLeftWristRoll = 19 |
|||
|
|||
# Right arm |
|||
kRightShoulderPitch = 20 |
|||
kRightShoulderRoll = 21 |
|||
kRightShoulderYaw = 22 |
|||
kRightElbow = 23 |
|||
kRightWristYaw = 24 |
|||
kRightWristPitch = 25 |
|||
kRightWristRoll = 26 |
|||
|
|||
class JointIndex: |
|||
# Left leg |
|||
kLeftHipYaw = 0 |
|||
kLeftHipRoll = 1 |
|||
kLeftHipPitch = 2 |
|||
kLeftKnee = 3 |
|||
kLeftAnkle = 4 |
|||
kLeftAnkleRoll = 5 |
|||
|
|||
# Right leg |
|||
kRightHipYaw = 6 |
|||
kRightHipRoll = 7 |
|||
kRightHipPitch = 8 |
|||
kRightKnee = 9 |
|||
kRightAnkle = 10 |
|||
kRightAnkleRoll = 11 |
|||
|
|||
kWaistYaw = 12 |
|||
|
|||
# Left arm |
|||
kLeftShoulderPitch = 13 |
|||
kLeftShoulderRoll = 14 |
|||
kLeftShoulderYaw = 15 |
|||
kLeftElbow = 16 |
|||
kLeftWristYaw = 17 |
|||
kLeftWristPitch = 18 |
|||
kLeftWristRoll = 19 |
|||
|
|||
# Right arm |
|||
kRightShoulderPitch = 20 |
|||
kRightShoulderRoll = 21 |
|||
kRightShoulderYaw = 22 |
|||
kRightElbow = 23 |
|||
kRightWristYaw = 24 |
|||
kRightWristPitch = 25 |
|||
kRightWristRoll = 26 |
|||
|
|||
kNotUsedJoint = 27 |
|||
kNotUsedJoint1 = 28 |
|||
kNotUsedJoint2 = 29 |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue