Browse Source

add 'default=gripper' and update tpoic

main
wei.li 9 months ago
parent
commit
c346a63734
  1. 8
      teleop/teleop_hand_and_arm.py
  2. 8
      teleop/utils/server_api.py
  3. 48
      teleop/utils/sim_state_client.py

8
teleop/teleop_hand_and_arm.py

@ -58,7 +58,7 @@ if __name__ == '__main__':
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'], help='Select end effector controller')
parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'],default="gripper", help='Select end effector controller')
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')
@ -201,12 +201,12 @@ if __name__ == '__main__':
tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2))
cv2.imshow("record image", tv_resized_image) cv2.imshow("record image", tv_resized_image)
key = cv2.waitKey(1) & 0xFF key = cv2.waitKey(1) & 0xFF
if key == ord('s'):
if key == ord('q'):
stop_listening() stop_listening()
running = False running = False
if args.sim: if args.sim:
publish_reset_category(2, reset_pose_publisher) publish_reset_category(2, reset_pose_publisher)
elif key == ord('q'):
elif key == ord('s'):
should_toggle_recording = True should_toggle_recording = True
elif key == ord('a'): elif key == ord('a'):
if args.sim: if args.sim:
@ -386,7 +386,7 @@ if __name__ == '__main__':
"qpos": current_body_action, "qpos": current_body_action,
}, },
} }
if args.is_use_sim:
if args.sim:
sim_state = sim_state_subscriber.read_data() sim_state = sim_state_subscriber.read_data()
recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, sim_state=sim_state) recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, sim_state=sim_state)
else: else:

8
teleop/utils/server_api.py

@ -1,8 +0,0 @@
# service name
SERVICE_NAME = "get_sim_state"
# api version
API_VERSION = "1.0.0.1"
# api id
API_ID_GET_SIM_STATE = 1008

48
teleop/utils/sim_state_client.py

@ -1,48 +0,0 @@
import time
import json
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
from unitree_sdk2py.rpc.client import Client
from utils.server_api import SERVICE_NAME, API_VERSION, API_ID_GET_SIM_STATE
class SimStateClient(Client):
def __init__(self, enableLease: bool = False):
super().__init__(SERVICE_NAME, enableLease)
def Init(self):
self._RegistApi(API_ID_GET_SIM_STATE, 0)
self._SetApiVerson(API_VERSION)
def GetSimState_client_call(self):
c, d = self._Call(API_ID_GET_SIM_STATE, json.dumps(""))
return c, d
if __name__ == "__main__":
# initialize channel factory.
ChannelFactoryInitialize(0)
# create client
client = SimStateClient()
client.Init()
client.SetTimeout(5.0)
# get server version
code, serverApiVersion = client.GetServerApiVersion()
print("server api version:", serverApiVersion)
# wait lease applied
client.WaitLeaseApplied()
print("lease applied")
# test api
while True:
try:
c, d = client.GetSimState_client_call()
print("client get sim state ret:", c)
print("sim state data:", d)
except Exception as e:
print("Error getting sim state:", e)
time.sleep(1.0)
Loading…
Cancel
Save