silencht
1 year ago
6 changed files with 165 additions and 352 deletions
-
3LICENSE
-
62README.md
-
128README_ja-JP.md
-
61README_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