File size: 4,554 Bytes
f6aca0c
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140

import math
from isaacgym import gymapi
from isaacgym import gymutil
import numpy as np
from isaacgym.torch_utils import *
from termcolor import cprint
import argparse


# initialize gym
gym = gymapi.acquire_gym()

# create a simulator
sim_params = gymapi.SimParams()
sim_params.substeps = 2
sim_params.dt = 1.0 / 60.0

sim_params.physx.solver_type = 1
sim_params.physx.num_position_iterations = 4
sim_params.physx.num_velocity_iterations = 1

sim_params.physx.num_threads = 4
sim_params.physx.use_gpu = True

sim_params.use_gpu_pipeline = False
device_id = 1
# sim = gym.create_sim(args.compute_device_id, args.graphics_device_id, args.physics_engine, sim_params)
sim = gym.create_sim(device_id, device_id, gymapi.SIM_PHYSX, sim_params)
if sim is None:
    print("*** Failed to create sim")
    quit()

# create viewer using the default camera properties
viewer = gym.create_viewer(sim, gymapi.CameraProperties())
if viewer is None:
    raise ValueError('*** Failed to create viewer')

# add ground plane
plane_params = gymapi.PlaneParams()
gym.add_ground(sim, gymapi.PlaneParams())

# set up the env grid
num_envs = 4
spacing = 1.5
env_lower = gymapi.Vec3(-spacing, 0.0, -spacing)
env_upper = gymapi.Vec3(spacing, 0.0, spacing)


asset_root = f"../assets/"
# asset_file = "agibot_a2/urdf/model.urdf"
# asset_file = "unitree_h1/h1.urdf"
# asset_file = "adam_lite/adam_lite.urdf"
# asset_file = "openloong/AzureLoong.urdf"
# asset_file = "booster_t1/T1_serial.xml"
# asset_file = "unitree_h1_2/h1_2.urdf"
# asset_file = "unitree_h1_2/h1_2_handless.urdf"
asset_file = "pnd_adam_lite/adam_lite.urdf"

# Load asset with default control type of position for all joints
asset_options = gymapi.AssetOptions()
asset_options.fix_base_link = True
asset_options.default_dof_drive_mode = gymapi.DOF_MODE_POS
asset_options.collapse_fixed_joints = False
asset_options.use_mesh_materials = False
# asset_options.vhacd_enabled = False
asset_options.vhacd_enabled = True 
asset_options.vhacd_params = gymapi.VhacdParams() 
asset_options.vhacd_params.resolution = 200000 

asset_options.flip_visual_attachments = True
# asset_options.flip_visual_attachments = False

print("Loading asset '%s' from '%s'" % (asset_file, asset_root))
robot_asset = gym.load_asset(sim, asset_root, asset_file, asset_options)

robot_link_dict = gym.get_asset_rigid_body_dict(robot_asset)
robot_dof_dict = gym.get_asset_dof_dict(robot_asset)
ordered_body_dict = {k: v for k, v in sorted(robot_link_dict.items(), key=lambda item: item[1])}
cprint(f'[HumanoidRobot] Full robot body dict. NumBody: {len(robot_link_dict.keys())}', 'blue')
for k, v in ordered_body_dict.items():
    cprint(f'\t {k} {v}', 'blue')

ordered_dof_dict = {k: v for k, v in sorted(robot_dof_dict.items(), key=lambda item: item[1])}
cprint(f'[HumanoidRobot] Full robot dof dict. DoF: {len(ordered_dof_dict.keys())}', 'green')
for k, v in ordered_dof_dict.items():
    cprint(f'\t {k} {v}', 'green')

# print body names and dof names in the format: [""]
print("Body names: ", ordered_body_dict.keys())
print("DoF names: ", ordered_dof_dict.keys())

# initial root pose
initial_pose = gymapi.Transform()
initial_pose.p = gymapi.Vec3(0.0, 1.5, 0.0) # since isaacgym uses y-up coordinate system
initial_pose.r = gymapi.Quat(-0.707107, 0.0, 0.0, 0.707107)
# initial_pose.r = gymapi.Quat(0.0, 0.0, 0.0, 1.0)

env = gym.create_env(sim, env_lower, env_upper, 0)
robot = gym.create_actor(env, robot_asset, initial_pose, 'robot', 0, 1)


# Configure DOF properties
props = gym.get_actor_dof_properties(env, robot)
props['driveMode'][:] = gymapi.DOF_MODE_POS  # Set all joints to position control
props['stiffness'][:] = 400.0  # Set stiffness for better control
props['damping'][:] = 40.0    # Set damping to reduce oscillations
gym.set_actor_dof_properties(env, robot, props)


# Look at the first env
cam_pos = gymapi.Vec3(8, 4, 1.5)
cam_target = gymapi.Vec3(0, 2, 1.5)
gym.viewer_camera_look_at(viewer, env, cam_pos, cam_target)

num_dofs = gym.get_actor_dof_count(env, robot)
initial_dof_positions = np.zeros(num_dofs, dtype=np.float32)  # Adjust as per robot
gym.set_actor_dof_position_targets(env, robot, initial_dof_positions)


# Simulate
while not gym.query_viewer_has_closed(viewer):

    # step the physics
    gym.simulate(sim)
    gym.fetch_results(sim, True)

    # update the viewer
    gym.step_graphics(sim)
    gym.draw_viewer(viewer, sim, True)

    # This synchronizes the physics simulation with the rendering rate.
    gym.sync_frame_time(sim)



print('Done')

gym.destroy_viewer(viewer)
gym.destroy_sim(sim)