epilectrik
/
teleop
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

[fix] Unified Naming.

main
silencht 8 months ago
parent
984345da22
commit
d1510db0ab
2 changed files with 28 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      teleop/robot_control/robot_hand_unitree.py
  2. 42
      teleop/teleop_hand_and_arm.py

14
teleop/robot_control/robot_hand_unitree.py
View File

@ -232,7 +232,7 @@ Gripper_Num_Motors = 2
kTopicGripperCommand = "rt/unitree_actuator/cmd" kTopicGripperCommand = "rt/unitree_actuator/cmd"
kTopicGripperState = "rt/unitree_actuator/state" kTopicGripperState = "rt/unitree_actuator/state"
class Gripper_Controller:
class Dex1_1_Gripper_Controller:
def __init__(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None, def __init__(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None,
filter = True, fps = 200.0, Unit_Test = False, simulation_mode = False): filter = True, fps = 200.0, Unit_Test = False, simulation_mode = False):
""" """
@ -253,7 +253,7 @@ class Gripper_Controller:
Unit_Test: Whether to enable unit testing Unit_Test: Whether to enable unit testing
""" """
logger_mp.info("Initialize Gripper_Controller...")
logger_mp.info("Initialize Dex1_1_Gripper_Controller...")
self.fps = fps self.fps = fps
self.Unit_Test = Unit_Test self.Unit_Test = Unit_Test
@ -287,15 +287,15 @@ class Gripper_Controller:
if any(state != 0.0 for state in self.dual_gripper_state): if any(state != 0.0 for state in self.dual_gripper_state):
break break
time.sleep(0.01) time.sleep(0.01)
logger_mp.warning("[Gripper_Controller] Waiting to subscribe dds...")
logger_mp.info("[Gripper_Controller] Subscribe dds ok.")
logger_mp.warning("[Dex1_1_Gripper_Controller] Waiting to subscribe dds...")
logger_mp.info("[Dex1_1_Gripper_Controller] Subscribe dds ok.")
self.gripper_control_thread = threading.Thread(target=self.control_thread, args=(left_gripper_value_in, right_gripper_value_in, self.dual_gripper_state, self.gripper_control_thread = threading.Thread(target=self.control_thread, args=(left_gripper_value_in, right_gripper_value_in, self.dual_gripper_state,
dual_gripper_data_lock, dual_gripper_state_out, dual_gripper_action_out)) dual_gripper_data_lock, dual_gripper_state_out, dual_gripper_action_out))
self.gripper_control_thread.daemon = True self.gripper_control_thread.daemon = True
self.gripper_control_thread.start() self.gripper_control_thread.start()
logger_mp.info("Initialize Gripper_Controller OK!\n")
logger_mp.info("Initialize Dex1_1_Gripper_Controller OK!\n")
def _subscribe_gripper_state(self): def _subscribe_gripper_state(self):
while True: while True:
@ -394,7 +394,7 @@ class Gripper_Controller:
sleep_time = max(0, (1 / self.fps) - time_elapsed) sleep_time = max(0, (1 / self.fps) - time_elapsed)
time.sleep(sleep_time) time.sleep(sleep_time)
finally: finally:
logger_mp.info("Gripper_Controller has been closed.")
logger_mp.info("Dex1_1_Gripper_Controller has been closed.")
class Gripper_JointIndex(IntEnum): class Gripper_JointIndex(IntEnum):
kLeftGripper = 0 kLeftGripper = 0
@ -454,7 +454,7 @@ if __name__ == "__main__":
dual_gripper_data_lock = Lock() dual_gripper_data_lock = Lock()
dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data.
dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data.
gripper_ctrl = Gripper_Controller(left_hand_array, right_hand_array, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, Unit_Test = True)
gripper_ctrl = Dex1_1_Gripper_Controller(left_hand_array, right_hand_array, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, Unit_Test = True)
user_input = input("Please enter the start signal (enter 's' to start the subsequent program):\n") user_input = input("Please enter the start signal (enter 's' to start the subsequent program):\n")

42
teleop/teleop_hand_and_arm.py
View File

@ -17,7 +17,7 @@ sys.path.append(parent_dir)
from televuer import TeleVuerWrapper from televuer import TeleVuerWrapper
from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController
from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK
from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_Controller
from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Dex1_1_Gripper_Controller
from teleop.robot_control.robot_hand_inspire import Inspire_Controller from teleop.robot_control.robot_hand_inspire import Inspire_Controller
from teleop.robot_control.robot_hand_brainco import Brainco_Controller from teleop.robot_control.robot_hand_brainco import Brainco_Controller
from teleop.image_server.image_client import ImageClient from teleop.image_server.image_client import ImageClient
@ -60,7 +60,7 @@ if __name__ == '__main__':
# basic control parameters # basic control parameters
parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source')
parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller')
parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1', 'brainco'], help='Select end effector controller')
parser.add_argument('--ee', type=str, choices=['dex1', 'dex3', 'inspire1', 'brainco'], help='Select end effector controller')
# mode flags # mode flags
parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') parser.add_argument('--record', action = 'store_true', help = 'Enable data recording')
parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode') parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode')
@ -131,20 +131,20 @@ if __name__ == '__main__':
image_receive_thread.start() image_receive_thread.start()
# television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device.
tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == 'hand', img_shape=tv_img_shape, img_shm_name=tv_img_shm.name,
tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == "hand", img_shape=tv_img_shape, img_shm_name=tv_img_shm.name,
return_state_data=True, return_hand_rot_data = False) return_state_data=True, return_hand_rot_data = False)
# arm # arm
if args.arm == 'G1_29':
if args.arm == "G1_29":
arm_ctrl = G1_29_ArmController(motion_mode=args.motion, simulation_mode=args.sim) arm_ctrl = G1_29_ArmController(motion_mode=args.motion, simulation_mode=args.sim)
arm_ik = G1_29_ArmIK() arm_ik = G1_29_ArmIK()
elif args.arm == 'G1_23':
elif args.arm == "G1_23":
arm_ctrl = G1_23_ArmController(simulation_mode=args.sim) arm_ctrl = G1_23_ArmController(simulation_mode=args.sim)
arm_ik = G1_23_ArmIK() arm_ik = G1_23_ArmIK()
elif args.arm == 'H1_2':
elif args.arm == "H1_2":
arm_ctrl = H1_2_ArmController(simulation_mode=args.sim) arm_ctrl = H1_2_ArmController(simulation_mode=args.sim)
arm_ik = H1_2_ArmIK() arm_ik = H1_2_ArmIK()
elif args.arm == 'H1':
elif args.arm == "H1":
arm_ctrl = H1_ArmController(simulation_mode=args.sim) arm_ctrl = H1_ArmController(simulation_mode=args.sim)
arm_ik = H1_ArmIK() arm_ik = H1_ArmIK()
@ -155,28 +155,28 @@ if __name__ == '__main__':
dual_hand_data_lock = Lock() dual_hand_data_lock = Lock()
dual_hand_state_array = Array('d', 14, lock = False) # [output] current left, right hand state(14) data. dual_hand_state_array = Array('d', 14, lock = False) # [output] current left, right hand state(14) data.
dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data. dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data.
hand_ctrl = Dex3_1_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)
elif args.ee == "gripper":
hand_ctrl = Dex3_1_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim)
elif args.ee == "dex1":
left_gripper_value = Value('d', 0.0, lock=True) # [input] left_gripper_value = Value('d', 0.0, lock=True) # [input]
right_gripper_value = Value('d', 0.0, lock=True) # [input] right_gripper_value = Value('d', 0.0, lock=True) # [input]
dual_gripper_data_lock = Lock() dual_gripper_data_lock = Lock()
dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data.
dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data.
gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim)
gripper_ctrl = Dex1_1_Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim)
elif args.ee == "inspire1": elif args.ee == "inspire1":
left_hand_pos_array = Array('d', 75, lock = True) # [input] left_hand_pos_array = Array('d', 75, lock = True) # [input]
right_hand_pos_array = Array('d', 75, lock = True) # [input] right_hand_pos_array = Array('d', 75, lock = True) # [input]
dual_hand_data_lock = Lock() dual_hand_data_lock = Lock()
dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data.
dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data.
hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)
hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim)
elif args.ee == "brainco": elif args.ee == "brainco":
left_hand_pos_array = Array('d', 75, lock = True) # [input] left_hand_pos_array = Array('d', 75, lock = True) # [input]
right_hand_pos_array = Array('d', 75, lock = True) # [input] right_hand_pos_array = Array('d', 75, lock = True) # [input]
dual_hand_data_lock = Lock() dual_hand_data_lock = Lock()
dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data.
dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data.
hand_ctrl = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)
hand_ctrl = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim)
else: else:
pass pass
@ -188,7 +188,7 @@ if __name__ == '__main__':
sim_state_subscriber = start_sim_state_subscribe() sim_state_subscriber = start_sim_state_subscribe()
# controller + motion mode # controller + motion mode
if args.xr_mode == 'controller' and args.motion:
if args.xr_mode == "controller" and args.motion:
from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
sport_client = LocoClient() sport_client = LocoClient()
sport_client.SetTimeout(0.0001) sport_client.SetTimeout(0.0001)
@ -237,17 +237,17 @@ if __name__ == '__main__':
publish_reset_category(1, reset_pose_publisher) publish_reset_category(1, reset_pose_publisher)
# get input data # get input data
tele_data = tv_wrapper.get_motion_state_data() tele_data = tv_wrapper.get_motion_state_data()
if (args.ee == 'dex3' or args.ee == 'inspire1' or args.ee == 'brainco') and args.xr_mode == 'hand':
if (args.ee == "dex3" or args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand":
with left_hand_pos_array.get_lock(): with left_hand_pos_array.get_lock():
left_hand_pos_array[:] = tele_data.left_hand_pos.flatten() left_hand_pos_array[:] = tele_data.left_hand_pos.flatten()
with right_hand_pos_array.get_lock(): with right_hand_pos_array.get_lock():
right_hand_pos_array[:] = tele_data.right_hand_pos.flatten() right_hand_pos_array[:] = tele_data.right_hand_pos.flatten()
elif args.ee == 'gripper' and args.xr_mode == 'controller':
elif args.ee == "dex1" and args.xr_mode == "controller":
with left_gripper_value.get_lock(): with left_gripper_value.get_lock():
left_gripper_value.value = tele_data.left_trigger_value left_gripper_value.value = tele_data.left_trigger_value
with right_gripper_value.get_lock(): with right_gripper_value.get_lock():
right_gripper_value.value = tele_data.right_trigger_value right_gripper_value.value = tele_data.right_trigger_value
elif args.ee == 'gripper' and args.xr_mode == 'hand':
elif args.ee == "dex1" and args.xr_mode == "hand":
with left_gripper_value.get_lock(): with left_gripper_value.get_lock():
left_gripper_value.value = tele_data.left_pinch_value left_gripper_value.value = tele_data.left_pinch_value
with right_gripper_value.get_lock(): with right_gripper_value.get_lock():
@ -256,7 +256,7 @@ if __name__ == '__main__':
pass pass
# high level control # high level control
if args.xr_mode == 'controller' and args.motion:
if args.xr_mode == "controller" and args.motion:
# quit teleoperate # quit teleoperate
if tele_data.tele_state.right_aButton: if tele_data.tele_state.right_aButton:
running = False running = False
@ -282,7 +282,7 @@ if __name__ == '__main__':
# record data # record data
if args.record: if args.record:
# dex hand or gripper # dex hand or gripper
if args.ee == "dex3" and args.xr_mode == 'hand':
if args.ee == "dex3" and args.xr_mode == "hand":
with dual_hand_data_lock: with dual_hand_data_lock:
left_ee_state = dual_hand_state_array[:7] left_ee_state = dual_hand_state_array[:7]
right_ee_state = dual_hand_state_array[-7:] right_ee_state = dual_hand_state_array[-7:]
@ -290,7 +290,7 @@ if __name__ == '__main__':
right_hand_action = dual_hand_action_array[-7:] right_hand_action = dual_hand_action_array[-7:]
current_body_state = [] current_body_state = []
current_body_action = [] current_body_action = []
elif args.ee == "gripper" and args.xr_mode == 'hand':
elif args.ee == "dex1" and args.xr_mode == "hand":
with dual_gripper_data_lock: with dual_gripper_data_lock:
left_ee_state = [dual_gripper_state_array[1]] left_ee_state = [dual_gripper_state_array[1]]
right_ee_state = [dual_gripper_state_array[0]] right_ee_state = [dual_gripper_state_array[0]]
@ -298,7 +298,7 @@ if __name__ == '__main__':
right_hand_action = [dual_gripper_action_array[0]] right_hand_action = [dual_gripper_action_array[0]]
current_body_state = [] current_body_state = []
current_body_action = [] current_body_action = []
elif args.ee == "gripper" and args.xr_mode == 'controller':
elif args.ee == "dex1" and args.xr_mode == "controller":
with dual_gripper_data_lock: with dual_gripper_data_lock:
left_ee_state = [dual_gripper_state_array[1]] left_ee_state = [dual_gripper_state_array[1]]
right_ee_state = [dual_gripper_state_array[0]] right_ee_state = [dual_gripper_state_array[0]]
@ -308,7 +308,7 @@ if __name__ == '__main__':
current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3,
-tele_data.tele_state.left_thumbstick_value[0] * 0.3, -tele_data.tele_state.left_thumbstick_value[0] * 0.3,
-tele_data.tele_state.right_thumbstick_value[0] * 0.3] -tele_data.tele_state.right_thumbstick_value[0] * 0.3]
elif (args.ee == "inspire1" or args.ee == 'brainco') and args.xr_mode == 'hand':
elif (args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand":
with dual_hand_data_lock: with dual_hand_data_lock:
left_ee_state = dual_hand_state_array[:6] left_ee_state = dual_hand_state_array[:6]
right_ee_state = dual_hand_state_array[-6:] right_ee_state = dual_hand_state_array[-6:]

Write Preview
Loading…
Cancel
Save