silencht
1 year ago
6 changed files with 165 additions and 352 deletions
-
3LICENSE
-
68README.md
-
128README_ja-JP.md
-
67README_zh-CN.md
-
4requirements.txt
-
247teleop/teleop_test_gym.py
@ -1,247 +0,0 @@ |
|||||
# TODO: this file need to be fixed. |
|
||||
from isaacgym import gymapi |
|
||||
from isaacgym import gymtorch |
|
||||
import math |
|
||||
import numpy as np |
|
||||
import torch |
|
||||
import time |
|
||||
from pytransform3d import rotations |
|
||||
|
|
||||
import os |
|
||||
import sys |
|
||||
current_dir = os.path.dirname(os.path.abspath(__file__)) |
|
||||
parent_dir = os.path.dirname(current_dir) |
|
||||
sys.path.append(parent_dir) |
|
||||
|
|
||||
from teleop.open_television.tv_wrapper import TeleVisionWrapper |
|
||||
from multiprocessing import Process, shared_memory, Array |
|
||||
from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller |
|
||||
|
|
||||
class Sim: |
|
||||
def __init__(self, |
|
||||
print_freq=False): |
|
||||
self.print_freq = print_freq |
|
||||
|
|
||||
# initialize gym |
|
||||
self.gym = gymapi.acquire_gym() |
|
||||
|
|
||||
# configure sim |
|
||||
sim_params = gymapi.SimParams() |
|
||||
sim_params.dt = 1 / 60 |
|
||||
sim_params.substeps = 2 |
|
||||
sim_params.up_axis = gymapi.UP_AXIS_Z |
|
||||
sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.81) |
|
||||
sim_params.physx.solver_type = 1 |
|
||||
sim_params.physx.num_position_iterations = 4 |
|
||||
sim_params.physx.num_velocity_iterations = 1 |
|
||||
sim_params.physx.max_gpu_contact_pairs = 8388608 |
|
||||
sim_params.physx.contact_offset = 0.002 |
|
||||
sim_params.physx.friction_offset_threshold = 0.001 |
|
||||
sim_params.physx.friction_correlation_distance = 0.0005 |
|
||||
sim_params.physx.rest_offset = 0.0 |
|
||||
sim_params.physx.use_gpu = True |
|
||||
sim_params.use_gpu_pipeline = False |
|
||||
|
|
||||
self.sim = self.gym.create_sim(0, 0, gymapi.SIM_PHYSX, sim_params) |
|
||||
if self.sim is None: |
|
||||
print("*** Failed to create sim") |
|
||||
quit() |
|
||||
|
|
||||
plane_params = gymapi.PlaneParams() |
|
||||
plane_params.distance = 0.0 |
|
||||
plane_params.normal = gymapi.Vec3(0.0, 0.0, 1.0) |
|
||||
self.gym.add_ground(self.sim, plane_params) |
|
||||
|
|
||||
# load table asset |
|
||||
table_asset_options = gymapi.AssetOptions() |
|
||||
table_asset_options.disable_gravity = True |
|
||||
table_asset_options.fix_base_link = True |
|
||||
table_asset = self.gym.create_box(self.sim, 0.8, 0.8, 0.1, table_asset_options) |
|
||||
|
|
||||
# load cube asset |
|
||||
cube_asset_options = gymapi.AssetOptions() |
|
||||
cube_asset_options.density = 10 |
|
||||
cube_asset = self.gym.create_box(self.sim, 0.05, 0.05, 0.05, cube_asset_options) |
|
||||
|
|
||||
asset_root = "../assets" |
|
||||
left_asset_path = "unitree_hand/unitree_dex3_left.urdf" |
|
||||
right_asset_path = "unitree_hand/unitree_dex3_right.urdf" |
|
||||
asset_options = gymapi.AssetOptions() |
|
||||
asset_options.fix_base_link = True |
|
||||
asset_options.default_dof_drive_mode = gymapi.DOF_MODE_POS |
|
||||
left_asset = self.gym.load_asset(self.sim, asset_root, left_asset_path, asset_options) |
|
||||
right_asset = self.gym.load_asset(self.sim, asset_root, right_asset_path, asset_options) |
|
||||
self.dof = self.gym.get_asset_dof_count(left_asset) |
|
||||
print(f"dof: {self.dof}") |
|
||||
# set up the env grid |
|
||||
num_envs = 1 |
|
||||
num_per_row = int(math.sqrt(num_envs)) |
|
||||
env_spacing = 1.25 |
|
||||
env_lower = gymapi.Vec3(-env_spacing, 0.0, -env_spacing) |
|
||||
env_upper = gymapi.Vec3(env_spacing, env_spacing, env_spacing) |
|
||||
np.random.seed(0) |
|
||||
self.env = self.gym.create_env(self.sim, env_lower, env_upper, num_per_row) |
|
||||
|
|
||||
# table |
|
||||
pose = gymapi.Transform() |
|
||||
pose.p = gymapi.Vec3(0, 0, 1.2) |
|
||||
pose.r = gymapi.Quat(0, 0, 0, 1) |
|
||||
table_handle = self.gym.create_actor(self.env, table_asset, pose, 'table', 0) |
|
||||
color = gymapi.Vec3(0.5, 0.5, 0.5) |
|
||||
self.gym.set_rigid_body_color(self.env, table_handle, 0, gymapi.MESH_VISUAL_AND_COLLISION, color) |
|
||||
|
|
||||
# cube |
|
||||
pose = gymapi.Transform() |
|
||||
pose.p = gymapi.Vec3(0, 0, 1.25) |
|
||||
pose.r = gymapi.Quat(0, 0, 0, 1) |
|
||||
cube_handle = self.gym.create_actor(self.env, cube_asset, pose, 'cube', 0) |
|
||||
color = gymapi.Vec3(1, 0.5, 0.5) |
|
||||
self.gym.set_rigid_body_color(self.env, cube_handle, 0, gymapi.MESH_VISUAL_AND_COLLISION, color) |
|
||||
|
|
||||
# left_hand |
|
||||
pose = gymapi.Transform() |
|
||||
pose.p = gymapi.Vec3(-0.6, 0, 1.6) |
|
||||
pose.r = gymapi.Quat(0, 0, 0, 1) |
|
||||
self.left_handle = self.gym.create_actor(self.env, left_asset, pose, 'left', 1, 1) |
|
||||
self.gym.set_actor_dof_states(self.env, self.left_handle, np.zeros(self.dof, gymapi.DofState.dtype), |
|
||||
gymapi.STATE_ALL) |
|
||||
left_idx = self.gym.get_actor_index(self.env, self.left_handle, gymapi.DOMAIN_SIM) |
|
||||
|
|
||||
# right_hand |
|
||||
pose = gymapi.Transform() |
|
||||
pose.p = gymapi.Vec3(-0.6, 0, 1.6) |
|
||||
pose.r = gymapi.Quat(0, 0, 0, 1) |
|
||||
self.right_handle = self.gym.create_actor(self.env, right_asset, pose, 'right', 1, 1) |
|
||||
self.gym.set_actor_dof_states(self.env, self.right_handle, np.zeros(self.dof, gymapi.DofState.dtype), |
|
||||
gymapi.STATE_ALL) |
|
||||
right_idx = self.gym.get_actor_index(self.env, self.right_handle, gymapi.DOMAIN_SIM) |
|
||||
|
|
||||
self.root_state_tensor = self.gym.acquire_actor_root_state_tensor(self.sim) |
|
||||
self.gym.refresh_actor_root_state_tensor(self.sim) |
|
||||
self.root_states = gymtorch.wrap_tensor(self.root_state_tensor) |
|
||||
self.left_root_states = self.root_states[left_idx] |
|
||||
self.right_root_states = self.root_states[right_idx] |
|
||||
|
|
||||
# create default viewer |
|
||||
self.viewer = self.gym.create_viewer(self.sim, gymapi.CameraProperties()) |
|
||||
if self.viewer is None: |
|
||||
print("*** Failed to create viewer") |
|
||||
quit() |
|
||||
cam_pos = gymapi.Vec3(1, 1, 2) |
|
||||
cam_target = gymapi.Vec3(0, 0, 1) |
|
||||
self.gym.viewer_camera_look_at(self.viewer, None, cam_pos, cam_target) |
|
||||
|
|
||||
self.cam_lookat_offset = np.array([1, 0, 0]) |
|
||||
self.left_cam_offset = np.array([0, 0.033, 0]) |
|
||||
self.right_cam_offset = np.array([0, -0.033, 0]) |
|
||||
self.cam_pos = np.array([-0.6, 0, 1.6]) |
|
||||
|
|
||||
# create left 1st preson viewer |
|
||||
camera_props = gymapi.CameraProperties() |
|
||||
camera_props.width = 1280 |
|
||||
camera_props.height = 720 |
|
||||
self.left_camera_handle = self.gym.create_camera_sensor(self.env, camera_props) |
|
||||
self.gym.set_camera_location(self.left_camera_handle, |
|
||||
self.env, |
|
||||
gymapi.Vec3(*(self.cam_pos + self.left_cam_offset)), |
|
||||
gymapi.Vec3(*(self.cam_pos + self.left_cam_offset + self.cam_lookat_offset))) |
|
||||
|
|
||||
# create right 1st preson viewer |
|
||||
camera_props = gymapi.CameraProperties() |
|
||||
camera_props.width = 1280 |
|
||||
camera_props.height = 720 |
|
||||
self.right_camera_handle = self.gym.create_camera_sensor(self.env, camera_props) |
|
||||
self.gym.set_camera_location(self.right_camera_handle, |
|
||||
self.env, |
|
||||
gymapi.Vec3(*(self.cam_pos + self.right_cam_offset)), |
|
||||
gymapi.Vec3(*(self.cam_pos + self.right_cam_offset + self.cam_lookat_offset))) |
|
||||
|
|
||||
def step(self, head_rmat, left_pose, right_pose, left_qpos, right_qpos): |
|
||||
|
|
||||
if self.print_freq: |
|
||||
start = time.time() |
|
||||
|
|
||||
self.left_root_states[0:7] = torch.tensor(left_pose, dtype=float) |
|
||||
self.right_root_states[0:7] = torch.tensor(right_pose, dtype=float) |
|
||||
self.gym.set_actor_root_state_tensor(self.sim, gymtorch.unwrap_tensor(self.root_states)) |
|
||||
|
|
||||
left_states = np.zeros(self.dof, dtype=gymapi.DofState.dtype) |
|
||||
left_states['pos'] = left_qpos |
|
||||
self.gym.set_actor_dof_states(self.env, self.left_handle, left_states, gymapi.STATE_POS) |
|
||||
|
|
||||
right_states = np.zeros(self.dof, dtype=gymapi.DofState.dtype) |
|
||||
right_states['pos'] = right_qpos |
|
||||
self.gym.set_actor_dof_states(self.env, self.right_handle, right_states, gymapi.STATE_POS) |
|
||||
|
|
||||
# step the physics |
|
||||
self.gym.simulate(self.sim) |
|
||||
self.gym.fetch_results(self.sim, True) |
|
||||
self.gym.step_graphics(self.sim) |
|
||||
self.gym.render_all_camera_sensors(self.sim) |
|
||||
self.gym.refresh_actor_root_state_tensor(self.sim) |
|
||||
|
|
||||
curr_lookat_offset = self.cam_lookat_offset @ head_rmat.T |
|
||||
curr_left_offset = self.left_cam_offset @ head_rmat.T |
|
||||
curr_right_offset = self.right_cam_offset @ head_rmat.T |
|
||||
|
|
||||
self.gym.set_camera_location(self.left_camera_handle, |
|
||||
self.env, |
|
||||
gymapi.Vec3(*(self.cam_pos + curr_left_offset)), |
|
||||
gymapi.Vec3(*(self.cam_pos + curr_left_offset + curr_lookat_offset))) |
|
||||
self.gym.set_camera_location(self.right_camera_handle, |
|
||||
self.env, |
|
||||
gymapi.Vec3(*(self.cam_pos + curr_right_offset)), |
|
||||
gymapi.Vec3(*(self.cam_pos + curr_right_offset + curr_lookat_offset))) |
|
||||
left_image = self.gym.get_camera_image(self.sim, self.env, self.left_camera_handle, gymapi.IMAGE_COLOR) |
|
||||
right_image = self.gym.get_camera_image(self.sim, self.env, self.right_camera_handle, gymapi.IMAGE_COLOR) |
|
||||
left_image = left_image.reshape(left_image.shape[0], -1, 4)[..., :3] |
|
||||
right_image = right_image.reshape(right_image.shape[0], -1, 4)[..., :3] |
|
||||
|
|
||||
self.gym.draw_viewer(self.viewer, self.sim, True) |
|
||||
self.gym.sync_frame_time(self.sim) |
|
||||
|
|
||||
if self.print_freq: |
|
||||
end = time.time() |
|
||||
print('Frequency:', 1 / (end - start)) |
|
||||
|
|
||||
return left_image, right_image |
|
||||
|
|
||||
def end(self): |
|
||||
self.gym.destroy_viewer(self.viewer) |
|
||||
self.gym.destroy_sim(self.sim) |
|
||||
|
|
||||
|
|
||||
if __name__ == '__main__': |
|
||||
# image and television |
|
||||
img_shape = (720, 2560, 3) |
|
||||
img_shm = shared_memory.SharedMemory(create=True, size=np.prod(img_shape) * np.uint8().itemsize) |
|
||||
img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=img_shm.buf) |
|
||||
tv_wrapper = TeleVisionWrapper(img_shape, img_shm.name) |
|
||||
# hand |
|
||||
left_hand_array = Array('d', 75, lock=True) |
|
||||
right_hand_array = Array('d', 75, lock=True) |
|
||||
lr_hand_state_array = Array('d', 14, lock=True) # current left, right hand state data |
|
||||
lr_hand_aciton_array = Array('d', 14, lock=True) # current left and right hand action data to be controlled |
|
||||
hand_ctrl = Dex3_1_Controller(lr_hand_state_array, lr_hand_aciton_array) |
|
||||
hand_control_process = Process(target=hand_ctrl.control_process, args=(left_hand_array, right_hand_array)) |
|
||||
hand_control_process.daemon = True |
|
||||
hand_control_process.start() |
|
||||
simulator = Sim() |
|
||||
|
|
||||
try: |
|
||||
while True: |
|
||||
user_input = input("Please enter the start signal (enter 's' to start the subsequent program):") |
|
||||
if user_input.lower() == 's': |
|
||||
while True: |
|
||||
head_rmat, left_wrist, right_wrist, left_hand, right_hand = tv_wrapper.get_data() |
|
||||
left_pose = np.concatenate([left_wrist[:3, 3] + np.array([-0.6, 0, 1.6]), |
|
||||
rotations.quaternion_from_matrix(left_wrist[:3, :3])[[1, 2, 3, 0]]]) |
|
||||
right_pose = np.concatenate([right_wrist[:3, 3] + np.array([-0.6, 0, 1.6]), |
|
||||
rotations.quaternion_from_matrix(right_wrist[:3, :3])[[1, 2, 3, 0]]]) |
|
||||
left_hand_array[:] = left_hand.flatten() |
|
||||
right_hand_array[:] = right_hand.flatten() |
|
||||
left_img, right_img = simulator.step(head_rmat, left_pose, right_pose, lr_hand_aciton_array[:7], lr_hand_aciton_array[-7:]) |
|
||||
np.copyto(img_array, np.hstack((left_img, right_img))) |
|
||||
except KeyboardInterrupt: |
|
||||
simulator.end() |
|
||||
exit(0) |
|
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue