DexGarmentLab/TeleOp_Env.py at main · wayrise/DexGarmentLab · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
from isaacsim import SimulationApp
simulation_app = SimulationApp({"headless": False})

# load external package
import os
import sys
import time
import numpy as np
import open3d as o3d
from termcolor import cprint
import threading

# load isaac-relevant package
import omni.replicator.core as rep
import isaacsim.core.utils.prims as prims_utils
from pxr import UsdGeom,UsdPhysics,PhysxSchema, Gf
from isaacsim.core.api import World
from isaacsim.core.api import SimulationContext
from isaacsim.core.api.objects import DynamicCuboid, FixedCuboid, VisualCuboid
from isaacsim.core.utils.prims import is_prim_path_valid, set_prim_visibility
from isaacsim.core.utils.string import find_unique_string_name
from isaacsim.core.utils.viewports import set_camera_view
from isaacsim.core.utils.stage import add_reference_to_stage, is_stage_loading
from isaacsim.core.prims import SingleXFormPrim, SingleClothPrim, SingleRigidPrim, SingleGeometryPrim, SingleParticleSystem, SingleDeformablePrim
from isaacsim.core.prims import XFormPrim, ClothPrim, RigidPrim, GeometryPrim, ParticleSystem
from isaacsim.core.utils.types import ArticulationAction
from omni.physx.scripts import deformableUtils,particleUtils,physicsUtils


# load custom package
sys.path.append(os.getcwd())
from Env_StandAlone.BaseEnv import BaseEnv
from Env_Config.Garment.Particle_Garment import Particle_Garment
from Env_Config.Garment.Deformable_Garment import Deformable_Garment
from Env_Config.Robot.BimanualDex_Ur10e import Bimanual_Ur10e
from Env_Config.Camera.Recording_Camera import Recording_Camera
from Env_Config.Room.Real_Ground import Real_Ground
from Env_Config.Utils_Project.Code_Tools import get_unique_filename, normalize_columns
from Env_Config.Utils_Project.Parse import parse_args_record
from Env_Config.Utils_Project.Flatten_Judge import judge_fling
from Env_Config.Room.Object_Tools import set_prim_visible_group, delete_prim_group
from Model_HALO.GAM.GAM_Encapsulation import GAM_Encapsulation
from Env_Config.Teleoperation.Listener import Listener


class TeleOp_Env(BaseEnv):
    def __init__(self):
        # load BaseEnv
        super().__init__()

        # ------------------------------------ #
        # ---        Add Env Assets        --- #
        # ------------------------------------ #
        self.ground = Real_Ground(
            self.scene,
            visual_material_usd = None,
            # you can use materials in 'Assets/Material/Floor' to change the texture of ground.
        )

        # load bimanual_dex
        self.bimanual_dex = Bimanual_Ur10e(
            self.world,
            dexleft_pos=np.array([-0.5, 0.0, 0.5]),
            dexleft_ori=np.array([0.0, 0.0, 0.0]),
            dexright_pos=np.array([0.5, 0.0, 0.5]),
            dexright_ori=np.array([0.0, 0.0, 0.0]),
        )

        # ------------------------------------ #
        # --- Initialize World to be Ready --- #
        # ------------------------------------ #
        # initialize world
        self.reset()

        # step world to make it ready
        for i in range(100):
            self.step()

        cprint("World Ready!", "green", "on_green")

if __name__=="__main__":

    env = TeleOp_Env()

    env.listener = Listener(simulation_app, "handler")

    env.listener.launch()

    while simulation_app.is_running():

        env.step()

        hand_pose_rawR, arm_pose_rawR, hand_joint_poseR, wrist_posR, wrist_oriR = env.listener.get_pose("right")

        left_arm_pose = ArticulationAction(
            joint_positions=np.array([
                -1.57, -1.84, -2.5, -1.89, -1.57, 0.0
            ]),
            joint_indices=np.array(env.bimanual_dex.dexleft.arm_dof_indices)
        )

        right_arm_pose = ArticulationAction(
            joint_positions=np.array([
                1.57, -1.3, 2.5, -1.25, 1.57, 0.0
            ]),
            joint_indices=np.array(env.bimanual_dex.dexright.arm_dof_indices)
        )

        env.bimanual_dex.dexleft.apply_action(left_arm_pose)

        env.bimanual_dex.dexright.apply_action(right_arm_pose)

        if hand_joint_poseR is not None:
            env.bimanual_dex.dexright.set_joint_positions(hand_joint_poseR, env.bimanual_dex.dexright.hand_dof_indices)
            env.bimanual_dex.dexleft.set_joint_positions(hand_joint_poseR, env.bimanual_dex.dexright.hand_dof_indices)
            print(hand_joint_poseR)


    while simulation_app.is_running():
        simulation_app.update()


simulation_app.close()