|
|
|
@ -11,6 +11,9 @@ from unitree_sdk2py.utils.crc import CRC |
|
|
|
from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( LowCmd_ as go_LowCmd, LowState_ as go_LowState) # idl for h1 |
|
|
|
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ |
|
|
|
|
|
|
|
import logging_mp |
|
|
|
logger_mp = logging_mp.get_logger(__name__) |
|
|
|
|
|
|
|
kTopicLowCommand_Debug = "rt/lowcmd" |
|
|
|
kTopicLowCommand_Motion = "rt/arm_sdk" |
|
|
|
kTopicLowState = "rt/lowstate" |
|
|
|
@ -57,7 +60,7 @@ class DataBuffer: |
|
|
|
|
|
|
|
class G1_29_ArmController: |
|
|
|
def __init__(self, debug_mode = True): |
|
|
|
print("Initialize G1_29_ArmController...") |
|
|
|
logger_mp.info("Initialize G1_29_ArmController...") |
|
|
|
self.q_target = np.zeros(14) |
|
|
|
self.tauff_target = np.zeros(14) |
|
|
|
self.debug_mode = debug_mode |
|
|
|
@ -95,7 +98,7 @@ class G1_29_ArmController: |
|
|
|
|
|
|
|
while not self.lowstate_buffer.GetData(): |
|
|
|
time.sleep(0.01) |
|
|
|
print("[G1_29_ArmController] Waiting to subscribe dds...") |
|
|
|
logger_mp.warning("[G1_29_ArmController] Waiting to subscribe dds...") |
|
|
|
|
|
|
|
# initialize hg's lowcmd msg |
|
|
|
self.crc = CRC() |
|
|
|
@ -104,9 +107,9 @@ class G1_29_ArmController: |
|
|
|
self.msg.mode_machine = self.get_mode_machine() |
|
|
|
|
|
|
|
self.all_motor_q = self.get_current_motor_q() |
|
|
|
print(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
print("Lock all joints except two arms...\n") |
|
|
|
logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
logger_mp.info("Lock all joints except two arms...\n") |
|
|
|
|
|
|
|
arm_indices = set(member.value for member in G1_29_JointArmIndex) |
|
|
|
for id in G1_29_JointIndex: |
|
|
|
@ -126,7 +129,7 @@ class G1_29_ArmController: |
|
|
|
self.msg.motor_cmd[id].kp = self.kp_high |
|
|
|
self.msg.motor_cmd[id].kd = self.kd_high |
|
|
|
self.msg.motor_cmd[id].q = self.all_motor_q[id] |
|
|
|
print("Lock OK!\n") |
|
|
|
logger_mp.info("Lock OK!\n") |
|
|
|
|
|
|
|
# initialize publish thread |
|
|
|
self.publish_thread = threading.Thread(target=self._ctrl_motor_state) |
|
|
|
@ -134,7 +137,7 @@ class G1_29_ArmController: |
|
|
|
self.publish_thread.daemon = True |
|
|
|
self.publish_thread.start() |
|
|
|
|
|
|
|
print("Initialize G1_29_ArmController OK!\n") |
|
|
|
logger_mp.info("Initialize G1_29_ArmController OK!\n") |
|
|
|
|
|
|
|
def _subscribe_motor_state(self): |
|
|
|
while True: |
|
|
|
@ -183,8 +186,8 @@ class G1_29_ArmController: |
|
|
|
all_t_elapsed = current_time - start_time |
|
|
|
sleep_time = max(0, (self.control_dt - all_t_elapsed)) |
|
|
|
time.sleep(sleep_time) |
|
|
|
# print(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# print(f"sleep_time:{sleep_time}") |
|
|
|
# logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# logger_mp.debug(f"sleep_time:{sleep_time}") |
|
|
|
|
|
|
|
def ctrl_dual_arm(self, q_target, tauff_target): |
|
|
|
'''Set control target values q & tau of the left and right arm motors.''' |
|
|
|
@ -210,7 +213,7 @@ class G1_29_ArmController: |
|
|
|
|
|
|
|
def ctrl_dual_arm_go_home(self): |
|
|
|
'''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' |
|
|
|
print("[G1_29_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
logger_mp.info("[G1_29_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
with self.ctrl_lock: |
|
|
|
self.q_target = np.zeros(14) |
|
|
|
# self.tauff_target = np.zeros(14) |
|
|
|
@ -222,7 +225,7 @@ class G1_29_ArmController: |
|
|
|
for weight in np.arange(1, 0, -0.01): |
|
|
|
self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; |
|
|
|
time.sleep(0.02) |
|
|
|
print("[G1_29_ArmController] both arms have reached the home position.") |
|
|
|
logger_mp.info("[G1_29_ArmController] both arms have reached the home position.") |
|
|
|
break |
|
|
|
time.sleep(0.05) |
|
|
|
|
|
|
|
@ -332,7 +335,7 @@ class G1_29_JointIndex(IntEnum): |
|
|
|
|
|
|
|
class G1_23_ArmController: |
|
|
|
def __init__(self): |
|
|
|
print("Initialize G1_23_ArmController...") |
|
|
|
logger_mp.info("Initialize G1_23_ArmController...") |
|
|
|
self.q_target = np.zeros(10) |
|
|
|
self.tauff_target = np.zeros(10) |
|
|
|
|
|
|
|
@ -366,7 +369,7 @@ class G1_23_ArmController: |
|
|
|
|
|
|
|
while not self.lowstate_buffer.GetData(): |
|
|
|
time.sleep(0.01) |
|
|
|
print("[G1_23_ArmController] Waiting to subscribe dds...") |
|
|
|
logger_mp.warning("[G1_23_ArmController] Waiting to subscribe dds...") |
|
|
|
|
|
|
|
# initialize hg's lowcmd msg |
|
|
|
self.crc = CRC() |
|
|
|
@ -375,9 +378,9 @@ class G1_23_ArmController: |
|
|
|
self.msg.mode_machine = self.get_mode_machine() |
|
|
|
|
|
|
|
self.all_motor_q = self.get_current_motor_q() |
|
|
|
print(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
print("Lock all joints except two arms...\n") |
|
|
|
logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
logger_mp.info("Lock all joints except two arms...\n") |
|
|
|
|
|
|
|
arm_indices = set(member.value for member in G1_23_JointArmIndex) |
|
|
|
for id in G1_23_JointIndex: |
|
|
|
@ -397,7 +400,7 @@ class G1_23_ArmController: |
|
|
|
self.msg.motor_cmd[id].kp = self.kp_high |
|
|
|
self.msg.motor_cmd[id].kd = self.kd_high |
|
|
|
self.msg.motor_cmd[id].q = self.all_motor_q[id] |
|
|
|
print("Lock OK!\n") |
|
|
|
logger_mp.info("Lock OK!\n") |
|
|
|
|
|
|
|
# initialize publish thread |
|
|
|
self.publish_thread = threading.Thread(target=self._ctrl_motor_state) |
|
|
|
@ -405,7 +408,7 @@ class G1_23_ArmController: |
|
|
|
self.publish_thread.daemon = True |
|
|
|
self.publish_thread.start() |
|
|
|
|
|
|
|
print("Initialize G1_23_ArmController OK!\n") |
|
|
|
logger_mp.info("Initialize G1_23_ArmController OK!\n") |
|
|
|
|
|
|
|
def _subscribe_motor_state(self): |
|
|
|
while True: |
|
|
|
@ -451,8 +454,8 @@ class G1_23_ArmController: |
|
|
|
all_t_elapsed = current_time - start_time |
|
|
|
sleep_time = max(0, (self.control_dt - all_t_elapsed)) |
|
|
|
time.sleep(sleep_time) |
|
|
|
# print(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# print(f"sleep_time:{sleep_time}") |
|
|
|
# logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# logger_mp.debug(f"sleep_time:{sleep_time}") |
|
|
|
|
|
|
|
def ctrl_dual_arm(self, q_target, tauff_target): |
|
|
|
'''Set control target values q & tau of the left and right arm motors.''' |
|
|
|
@ -478,7 +481,7 @@ class G1_23_ArmController: |
|
|
|
|
|
|
|
def ctrl_dual_arm_go_home(self): |
|
|
|
'''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' |
|
|
|
print("[G1_23_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
logger_mp.info("[G1_23_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
with self.ctrl_lock: |
|
|
|
self.q_target = np.zeros(10) |
|
|
|
# self.tauff_target = np.zeros(10) |
|
|
|
@ -486,7 +489,7 @@ class G1_23_ArmController: |
|
|
|
while True: |
|
|
|
current_q = self.get_current_dual_arm_q() |
|
|
|
if np.all(np.abs(current_q) < tolerance): |
|
|
|
print("[G1_23_ArmController] both arms have reached the home position.") |
|
|
|
logger_mp.info("[G1_23_ArmController] both arms have reached the home position.") |
|
|
|
break |
|
|
|
time.sleep(0.05) |
|
|
|
|
|
|
|
@ -588,7 +591,7 @@ class G1_23_JointIndex(IntEnum): |
|
|
|
|
|
|
|
class H1_2_ArmController: |
|
|
|
def __init__(self): |
|
|
|
print("Initialize H1_2_ArmController...") |
|
|
|
logger_mp.info("Initialize H1_2_ArmController...") |
|
|
|
self.q_target = np.zeros(14) |
|
|
|
self.tauff_target = np.zeros(14) |
|
|
|
|
|
|
|
@ -622,7 +625,7 @@ class H1_2_ArmController: |
|
|
|
|
|
|
|
while not self.lowstate_buffer.GetData(): |
|
|
|
time.sleep(0.01) |
|
|
|
print("[H1_2_ArmController] Waiting to subscribe dds...") |
|
|
|
logger_mp.warning("[H1_2_ArmController] Waiting to subscribe dds...") |
|
|
|
|
|
|
|
# initialize hg's lowcmd msg |
|
|
|
self.crc = CRC() |
|
|
|
@ -631,9 +634,9 @@ class H1_2_ArmController: |
|
|
|
self.msg.mode_machine = self.get_mode_machine() |
|
|
|
|
|
|
|
self.all_motor_q = self.get_current_motor_q() |
|
|
|
print(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
print("Lock all joints except two arms...\n") |
|
|
|
logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
logger_mp.info("Lock all joints except two arms...\n") |
|
|
|
|
|
|
|
arm_indices = set(member.value for member in H1_2_JointArmIndex) |
|
|
|
for id in H1_2_JointIndex: |
|
|
|
@ -653,7 +656,7 @@ class H1_2_ArmController: |
|
|
|
self.msg.motor_cmd[id].kp = self.kp_high |
|
|
|
self.msg.motor_cmd[id].kd = self.kd_high |
|
|
|
self.msg.motor_cmd[id].q = self.all_motor_q[id] |
|
|
|
print("Lock OK!\n") |
|
|
|
logger_mp.info("Lock OK!\n") |
|
|
|
|
|
|
|
# initialize publish thread |
|
|
|
self.publish_thread = threading.Thread(target=self._ctrl_motor_state) |
|
|
|
@ -661,7 +664,7 @@ class H1_2_ArmController: |
|
|
|
self.publish_thread.daemon = True |
|
|
|
self.publish_thread.start() |
|
|
|
|
|
|
|
print("Initialize H1_2_ArmController OK!\n") |
|
|
|
logger_mp.info("Initialize H1_2_ArmController OK!\n") |
|
|
|
|
|
|
|
def _subscribe_motor_state(self): |
|
|
|
while True: |
|
|
|
@ -707,8 +710,8 @@ class H1_2_ArmController: |
|
|
|
all_t_elapsed = current_time - start_time |
|
|
|
sleep_time = max(0, (self.control_dt - all_t_elapsed)) |
|
|
|
time.sleep(sleep_time) |
|
|
|
# print(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# print(f"sleep_time:{sleep_time}") |
|
|
|
# logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# logger_mp.debug(f"sleep_time:{sleep_time}") |
|
|
|
|
|
|
|
def ctrl_dual_arm(self, q_target, tauff_target): |
|
|
|
'''Set control target values q & tau of the left and right arm motors.''' |
|
|
|
@ -734,7 +737,7 @@ class H1_2_ArmController: |
|
|
|
|
|
|
|
def ctrl_dual_arm_go_home(self): |
|
|
|
'''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' |
|
|
|
print("[H1_2_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
logger_mp.info("[H1_2_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
with self.ctrl_lock: |
|
|
|
self.q_target = np.zeros(14) |
|
|
|
# self.tauff_target = np.zeros(14) |
|
|
|
@ -742,7 +745,7 @@ class H1_2_ArmController: |
|
|
|
while True: |
|
|
|
current_q = self.get_current_dual_arm_q() |
|
|
|
if np.all(np.abs(current_q) < tolerance): |
|
|
|
print("[H1_2_ArmController] both arms have reached the home position.") |
|
|
|
logger_mp.info("[H1_2_ArmController] both arms have reached the home position.") |
|
|
|
break |
|
|
|
time.sleep(0.05) |
|
|
|
|
|
|
|
@ -851,7 +854,7 @@ class H1_2_JointIndex(IntEnum): |
|
|
|
|
|
|
|
class H1_ArmController: |
|
|
|
def __init__(self): |
|
|
|
print("Initialize H1_ArmController...") |
|
|
|
logger_mp.info("Initialize H1_ArmController...") |
|
|
|
self.q_target = np.zeros(8) |
|
|
|
self.tauff_target = np.zeros(8) |
|
|
|
|
|
|
|
@ -883,7 +886,7 @@ class H1_ArmController: |
|
|
|
|
|
|
|
while not self.lowstate_buffer.GetData(): |
|
|
|
time.sleep(0.01) |
|
|
|
print("[H1_ArmController] Waiting to subscribe dds...") |
|
|
|
logger_mp.warning("[H1_ArmController] Waiting to subscribe dds...") |
|
|
|
|
|
|
|
# initialize h1's lowcmd msg |
|
|
|
self.crc = CRC() |
|
|
|
@ -894,9 +897,9 @@ class H1_ArmController: |
|
|
|
self.msg.gpio = 0 |
|
|
|
|
|
|
|
self.all_motor_q = self.get_current_motor_q() |
|
|
|
print(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
print("Lock all joints except two arms...\n") |
|
|
|
logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") |
|
|
|
logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") |
|
|
|
logger_mp.info("Lock all joints except two arms...\n") |
|
|
|
|
|
|
|
for id in H1_JointIndex: |
|
|
|
if self._Is_weak_motor(id): |
|
|
|
@ -908,7 +911,7 @@ class H1_ArmController: |
|
|
|
self.msg.motor_cmd[id].kd = self.kd_high |
|
|
|
self.msg.motor_cmd[id].mode = 0x0A |
|
|
|
self.msg.motor_cmd[id].q = self.all_motor_q[id] |
|
|
|
print("Lock OK!\n") |
|
|
|
logger_mp.info("Lock OK!\n") |
|
|
|
|
|
|
|
# initialize publish thread |
|
|
|
self.publish_thread = threading.Thread(target=self._ctrl_motor_state) |
|
|
|
@ -916,7 +919,7 @@ class H1_ArmController: |
|
|
|
self.publish_thread.daemon = True |
|
|
|
self.publish_thread.start() |
|
|
|
|
|
|
|
print("Initialize H1_ArmController OK!\n") |
|
|
|
logger_mp.info("Initialize H1_ArmController OK!\n") |
|
|
|
|
|
|
|
def _subscribe_motor_state(self): |
|
|
|
while True: |
|
|
|
@ -962,8 +965,8 @@ class H1_ArmController: |
|
|
|
all_t_elapsed = current_time - start_time |
|
|
|
sleep_time = max(0, (self.control_dt - all_t_elapsed)) |
|
|
|
time.sleep(sleep_time) |
|
|
|
# print(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# print(f"sleep_time:{sleep_time}") |
|
|
|
# logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") |
|
|
|
# logger_mp.debug(f"sleep_time:{sleep_time}") |
|
|
|
|
|
|
|
def ctrl_dual_arm(self, q_target, tauff_target): |
|
|
|
'''Set control target values q & tau of the left and right arm motors.''' |
|
|
|
@ -985,7 +988,7 @@ class H1_ArmController: |
|
|
|
|
|
|
|
def ctrl_dual_arm_go_home(self): |
|
|
|
'''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' |
|
|
|
print("[H1_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
logger_mp.info("[H1_ArmController] ctrl_dual_arm_go_home start...") |
|
|
|
with self.ctrl_lock: |
|
|
|
self.q_target = np.zeros(8) |
|
|
|
# self.tauff_target = np.zeros(8) |
|
|
|
@ -993,7 +996,7 @@ class H1_ArmController: |
|
|
|
while True: |
|
|
|
current_q = self.get_current_dual_arm_q() |
|
|
|
if np.all(np.abs(current_q) < tolerance): |
|
|
|
print("[H1_ArmController] both arms have reached the home position.") |
|
|
|
logger_mp.info("[H1_ArmController] both arms have reached the home position.") |
|
|
|
break |
|
|
|
time.sleep(0.05) |
|
|
|
|
|
|
|
|
silencht
10 months ago