File size: 5,907 Bytes
8cf4ead | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 | from typing import Dict
import numpy as np
from unitree_sdk2py.core.channel import ChannelPublisher
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_
from unitree_sdk2py.utils.crc import CRC
class BodyCommandSender:
def __init__(self, config: Dict):
self.config = config
if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2":
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
self.low_cmd = unitree_go_msg_dds__LowCmd_()
elif (
self.config["ROBOT_TYPE"] == "g1_29dof"
or self.config["ROBOT_TYPE"] == "h1-2_21dof"
or self.config["ROBOT_TYPE"] == "h1-2_27dof"
):
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
self.low_cmd = unitree_hg_msg_dds__LowCmd_()
else:
raise NotImplementedError(
f"Robot type {self.config['ROBOT_TYPE']} is not supported yet"
)
# init kp kd
self.kp_level = 1.0
self.waist_kp_level = 1.0
self.robot_kp = np.zeros(self.config["NUM_MOTORS"])
self.robot_kd = np.zeros(self.config["NUM_MOTORS"])
# set kp level
for i in range(len(self.config["MOTOR_KP"])):
self.robot_kp[i] = self.config["MOTOR_KP"][i] * self.kp_level
for i in range(len(self.config["MOTOR_KD"])):
self.robot_kd[i] = self.config["MOTOR_KD"][i] * 1.0
self.weak_motor_joint_index = []
for _, value in self.config["WeakMotorJointIndex"].items():
self.weak_motor_joint_index.append(value)
# init low cmd publisher
self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
self.lowcmd_publisher_.Init()
self.InitLowCmd()
self.low_state = None
self.crc = CRC()
def InitLowCmd(self):
# h1/go2:
if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2":
self.low_cmd.head[0] = 0xFE
self.low_cmd.head[1] = 0xEF
else:
pass
self.low_cmd.level_flag = 0xFF
self.low_cmd.gpio = 0
for i in range(self.config["NUM_MOTORS"]):
if self.is_weak_motor(i):
self.low_cmd.motor_cmd[i].mode = 0x01
else:
self.low_cmd.motor_cmd[i].mode = 0x0A
self.low_cmd.motor_cmd[i].q = self.config["UNITREE_LEGGED_CONST"]["PosStopF"]
self.low_cmd.motor_cmd[i].kp = 0
self.low_cmd.motor_cmd[i].dq = self.config["UNITREE_LEGGED_CONST"]["VelStopF"]
self.low_cmd.motor_cmd[i].kd = 0
self.low_cmd.motor_cmd[i].tau = 0
if (
self.config["ROBOT_TYPE"] == "g1_29dof"
or self.config["ROBOT_TYPE"] == "h1-2_21dof"
or self.config["ROBOT_TYPE"] == "h1-2_27dof"
):
self.low_cmd.mode_machine = self.config["UNITREE_LEGGED_CONST"]["MODE_MACHINE"]
self.low_cmd.mode_pr = self.config["UNITREE_LEGGED_CONST"]["MODE_PR"]
else:
pass
def is_weak_motor(self, motor_index: int) -> bool:
return motor_index in self.weak_motor_joint_index
def send_command(self, cmd_q: np.ndarray, cmd_dq: np.ndarray, cmd_tau: np.ndarray):
for i in range(self.config["NUM_MOTORS"]):
motor_index = self.config["JOINT2MOTOR"][i]
joint_index = self.config["MOTOR2JOINT"][i]
# print(f"motor_index: {motor_index}, joint_index: {joint_index}")
if joint_index == -1:
# send default joint position command
self.low_cmd.motor_cmd[motor_index].q = self.config["DEFAULT_MOTOR_ANGLES"][
motor_index
]
self.low_cmd.motor_cmd[motor_index].dq = 0.0
self.low_cmd.motor_cmd[motor_index].tau = 0.0
else:
self.low_cmd.motor_cmd[motor_index].q = cmd_q[joint_index]
self.low_cmd.motor_cmd[motor_index].dq = cmd_dq[joint_index]
self.low_cmd.motor_cmd[motor_index].tau = cmd_tau[joint_index]
# kp kd
self.low_cmd.motor_cmd[motor_index].kp = self.robot_kp[motor_index]
self.low_cmd.motor_cmd[motor_index].kd = self.robot_kd[motor_index]
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
self.lowcmd_publisher_.Write(self.low_cmd)
def make_hand_mode(motor_index: int) -> int:
status = 0x01
timeout = 0x01
mode = motor_index & 0x0F
mode |= status << 4 # bits [4..6]
mode |= timeout << 7 # bit 7
return mode
class HandCommandSender:
def __init__(self, is_left: bool = True):
self.is_left = is_left
if self.is_left:
self.cmd_pub = ChannelPublisher("rt/dex3/left/cmd", HandCmd_)
else:
self.cmd_pub = ChannelPublisher("rt/dex3/right/cmd", HandCmd_)
self.cmd_pub.Init()
self.cmd = unitree_hg_msg_dds__HandCmd_()
self.hand_dof = 7
self.kp = [1.0] * self.hand_dof
self.kd = [0.2] * self.hand_dof
self.kp[0] = 2.0
self.kd[0] = 0.5
def send_command(self, cmd: np.ndarray):
for i in range(self.hand_dof):
# Build the bitfield mode (see your C++ example)
mode_val = make_hand_mode(i)
self.cmd.motor_cmd[i].mode = mode_val
self.cmd.motor_cmd[i].q = cmd[i]
self.cmd.motor_cmd[i].dq = 0.0
self.cmd.motor_cmd[i].tau = 0.0
self.cmd.motor_cmd[i].kp = self.kp[i]
self.cmd.motor_cmd[i].kd = self.kd[i]
self.cmd_pub.Write(self.cmd)
|