-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtest_bounding_with_preference.py
194 lines (155 loc) · 8.71 KB
/
test_bounding_with_preference.py
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
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
import os
import numpy
import numpy as np
from scipy.spatial.transform import Rotation as R
from datetime import datetime
import sys
import distutils.version
import isaacgym
from repo.unitree_rl_gym.legged_gym.envs import *
from repo.unitree_rl_gym.legged_gym.utils import get_args, export_policy_as_jit, task_registry_pref, Logger
import torch
import cv2
from isaacgym import gymapi
from custom_env.llmpref_go2_robot_flat import LLMPrefGo2RobotFlat
from custom_env.llmpref_go2_robot_flat_config import LLMPrefGo2RobotFlatCfg, LLMPrefGo2RobotFlatCfgPPO
# Create the environment
task_registry_pref.register('gpt_go2', LLMPrefGo2RobotFlat, LLMPrefGo2RobotFlatCfg, LLMPrefGo2RobotFlatCfgPPO)
def play(args):
env_cfg, train_cfg = task_registry_pref.get_cfgs(name=args.task)
# override some parameters for testing
env_cfg.env.num_envs = min(env_cfg.env.num_envs, 100)
# env_cfg.terrain.num_rows = 5
# env_cfg.terrain.num_cols = 5
# env_cfg.terrain.curriculum = False
# PJ: set record to get device id
env_cfg.viewer.record = args.record
env_cfg.terrain.num_rows = 10
env_cfg.terrain.num_cols = 20
env_cfg.terrain.curriculum = False # Disable curriculum to use only the selected terrain
env_cfg.noise.add_noise = False
env_cfg.domain_rand.randomize_friction = False
env_cfg.domain_rand.push_robots = False
env_cfg.env.test = True
env_cfg.commands.resampling_time = 1e6 # Don't resample command during the testing, so set it to a huge number.
# prepare environment
env, _ = task_registry_pref.make_env(name=args.task, args=args, env_cfg=env_cfg)
# print(env.commands)
obs = env.get_observations()
# load policy
train_cfg.runner.resume = True
ppo_runner, train_cfg = task_registry_pref.make_alg_runner(env=env, name=args.task, args=args, train_cfg=train_cfg, log_root=args.log_root)
policy = ppo_runner.get_inference_policy(device=env.device)
if args.test_direct == "forward": # [1.5, 0.0, 0.0, 0.0]
env.commands = torch.tensor([1.5, 0.0, 0.0, 0.0], device=env.device).repeat(env.num_envs, 1)
elif args.test_direct == "backward":
env.commands = torch.tensor([-0.9, 0.0, 0.0, 0.0], device=env.device).repeat(env.num_envs, 1)
elif args.test_direct == "left":
env.commands = torch.tensor([0.0, 0.8, 0.0, 0.0], device=env.device).repeat(env.num_envs, 1)
elif args.test_direct == "left_turn":
env.commands = torch.tensor([0.7, 0.0, 0.2, 1.57], device=env.device).repeat(env.num_envs, 1)
else:
print("command wrong! Set to a very large forward speed")
env.commands = torch.tensor([8.0, 0.0, 0.0, 0.0], device=env.device).repeat(env.num_envs, 1)
# Create camera properties
camera_props = gymapi.CameraProperties()
camera_props.width = 1280 # Set desired width
camera_props.height = 720 # Set desired height
# --- Fixed Camera Setup ---
# Create camera sensor in the first environment
fixed_camera_handle = env.gym.create_camera_sensor(env.envs[0], camera_props)
print(fixed_camera_handle)
# Set camera position and target
# record_cam_position = env_cfg.viewer.pos
fixed_record_cam_position = env.base_pos[0].cpu().numpy() + numpy.array([5.0, -5.0, 5.0])
# record_cam_lookat = env_cfg.viewer.lookat
fixed_record_cam_lookat = env.base_pos[0].cpu().numpy() + numpy.array([5.0, 5.0, 0.0])
fixed_camera_position = gymapi.Vec3(fixed_record_cam_position[0], fixed_record_cam_position[1], fixed_record_cam_position[2])
fixed_camera_target = gymapi.Vec3(fixed_record_cam_lookat[0], fixed_record_cam_lookat[1], fixed_record_cam_lookat[2])
env.gym.set_camera_location(fixed_camera_handle, env.envs[0], fixed_camera_position, fixed_camera_target)
# Set up VideoWriter for fixed camera
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out_fixed = cv2.VideoWriter(args.log_root + '/' + args.load_run + '/' + args.test_direct + f'_fixed{args.checkpoint}.mp4', fourcc, 60.0, (camera_props.width, camera_props.height))
print("save fixed video to:")
print(args.log_root + '/' + args.load_run + '/' + args.test_direct + f'fixed{args.checkpoint}.mp4')
# --- Dynamic Camera Setup ---
# Create dynamic camera sensor in the first environment
dynamic_camera_handle = env.gym.create_camera_sensor(env.envs[0], camera_props)
print(dynamic_camera_handle)
# Set initial dynamic camera position and target
LOCAL_CAMERA_OFFSET = np.array([0.0, -2.0, 0.7])
LOCAL_LOOKAT_OFFSET = np.array([0.0, 0.0, 0.0])
dynamic_cam_position = env.base_pos[0].cpu().numpy() + LOCAL_CAMERA_OFFSET # Initial offset
dynamic_cam_lookat = env.base_pos[0].cpu().numpy() # Look at the robot
dynamic_camera_position = gymapi.Vec3(*dynamic_cam_position) # PJ: set in a different way as the fixed one, but result is the same
dynamic_camera_target = gymapi.Vec3(*dynamic_cam_lookat)
env.gym.set_camera_location(dynamic_camera_handle, env.envs[0], dynamic_camera_position, dynamic_camera_target)
# Set up VideoWriter for dynamic camera
out_dynamic = cv2.VideoWriter(args.log_root + '/' + args.load_run + '/' + args.test_direct + f'_right{args.checkpoint}.mp4', fourcc, 60.0, (camera_props.width, camera_props.height))
print("save dynamic video to:")
print(args.log_root + '/' + args.load_run + '/' + args.test_direct + f'_right{args.checkpoint}.mp4')
# export policy as a jit module (used to run it from C++)
if EXPORT_POLICY:
path = os.path.join(LEGGED_GYM_ROOT_DIR, 'logs', train_cfg.runner.experiment_name, 'exported', 'policies')
export_policy_as_jit(ppo_runner.alg.actor_critic, path)
print('Exported policy as jit script to: ', path)
print('max_episode_length: ', env.max_episode_length)
for i in range(100): # int(env.max_episode_length), 500
actions = policy(obs.detach())
obs, _, rews, dones, infos = env.step(actions.detach())
# print the body position
contact = env.contact_forces[:, env.feet_indices, 2] > 1.
print(env.base_lin_vel[0].cpu().numpy())
# print(contact[0].int().cpu().numpy())
# Read base pos/quaternion from env
base_pos = env.base_pos[0].cpu().numpy() # shape: (3,)
base_quat = env.base_quat[0].cpu().numpy() # shape: (4,) in [x, y, z, w] order
# Convert to Euler
r_full = R.from_quat(base_quat) # if indeed [x, y, z, w]
roll, pitch, yaw = r_full.as_euler('xyz', degrees=False)
# Build yaw-only rotation, Keep only yaw
r_yaw_only = R.from_euler('xyz', [0.0, 0.0, yaw], degrees=False)
rot_mat_yaw_only = r_yaw_only.as_matrix()
# Rotate offset, place camera
world_offset = rot_mat_yaw_only @ LOCAL_CAMERA_OFFSET
dynamic_cam_position = base_pos + world_offset
# Rotate look-at offset, place look-at
world_lookat_offset = rot_mat_yaw_only @ LOCAL_LOOKAT_OFFSET
dynamic_cam_lookat = base_pos + world_lookat_offset
# Set camera
env.gym.set_camera_location(
dynamic_camera_handle,
env.envs[0],
gymapi.Vec3(*dynamic_cam_position),
gymapi.Vec3(*dynamic_cam_lookat)
)
# Render camera sensors
env.gym.render_all_camera_sensors(env.sim)
# --- Capture image from fixed camera ---
image_buffer_fixed = env.gym.get_camera_image(env.sim, env.envs[0], fixed_camera_handle, gymapi.IMAGE_COLOR)
# Convert image to numpy array and reshape
image_data_fixed = np.frombuffer(image_buffer_fixed, dtype=np.uint8).reshape((camera_props.height, camera_props.width, 4))
# Convert from RGBA to BGR format
image_bgr_fixed = image_data_fixed[:, :, :3]
# Write frame to fixed camera video
out_fixed.write(image_bgr_fixed)
# --- Capture image from dynamic camera ---
image_buffer_dynamic = env.gym.get_camera_image(env.sim, env.envs[0], dynamic_camera_handle, gymapi.IMAGE_COLOR)
# Convert image to numpy array and reshape
image_data_dynamic = np.frombuffer(image_buffer_dynamic, dtype=np.uint8).reshape((camera_props.height, camera_props.width, 4))
# Convert from RGBA to BGR format
image_bgr_dynamic = image_data_dynamic[:, :, :3]
# Write frame to dynamic camera video
out_dynamic.write(image_bgr_dynamic)
# *** Save right-camera images every 2 steps if enabled ***
if args.save_right_camera_figures and (i % 2 == 0) and (i < 100):
cv2.imwrite(os.path.join("results_fig/right_video_figs", args.video_figures_path, f"right_{i}.png"), image_bgr_dynamic)
# Release the VideoWriter
out_fixed.release()
out_dynamic.release()
if __name__ == '__main__':
EXPORT_POLICY = False # oroginally true
RECORD_FRAMES = False
MOVE_CAMERA = False
args = get_args()
play(args)