Isaac Sim
Isaac Gym

Qmini Robot Training Series - Tune URDF

Author

Tony Wang

Date Published

1# This file is used to tune PID without RL learning, run it to see how well it will behave
2# Parameters 'p' & 'd' can be changed in line 118 & 119
3# Data will be saved in foldr 'excel/tune_PID'
4import time
5import math
6import numpy as np
7from isaacgym import gymapi, gymutil, gymtorch
8import os
9from math import pi
10import matplotlib.pyplot as plt
11import pandas as pd
12import warnings
13from os.path import join
14from collections import deque
15from env.legged_robot import get_euler_xyz, to_torch
16import torch
17

18warnings.filterwarnings('ignore')
19

20

21def _from_2pi_to_pi(rpy):
22    return rpy.cpu() - 2 * pi * np.floor((rpy.cpu() + pi) / (2 * pi))
23

24

25def get_euler_from_quat(quat):
26    base_rpy = get_euler_xyz(quat)
27    r = to_torch(_from_2pi_to_pi(base_rpy[0]))
28    p = to_torch(_from_2pi_to_pi(base_rpy[1]))
29    y = to_torch(_from_2pi_to_pi(base_rpy[2]))
30    return torch.t(torch.vstack((r, p, y)))
31

32

33# Z-up axis in this file, (x,y,z)
34

35def clamp(x, min_value, max_value):
36    return max(min(x, max_value), min_value)
37

38

39def exp_filter(history, present, weight):
40    """
41    exponential filter
42    result = history * weight + present * (1. - weight)
43    """
44    return history * weight + present * (1. - weight)
45

46

47# initialize gym
48gym = gymapi.acquire_gym()
49

50# configure sim
51sim_params = gymapi.SimParams()
52sim_params.dt = 0.001  # dt*action_repeat=0.01
53sim_params.up_axis = gymapi.UP_AXIS_Z
54sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.81)  # -9.81
55# sim_params.gravity = gymapi.Vec3(0.0, 0.0, -0)  # -9.81
56

57sim_params.substeps = 1
58sim_params.use_gpu_pipeline = False
59print("WARNING: Forcing CPU pipeline.")
60

61# physx parameters
62sim_params.physx.solver_type = 1
63sim_params.physx.num_position_iterations = 4
64sim_params.physx.num_velocity_iterations = 0
65sim_params.physx.num_threads = 10
66sim_params.physx.contact_offset = 0.01
67sim_params.physx.rest_offset = 0.0
68sim_params.physx.bounce_threshold_velocity = 0.5
69sim_params.physx.max_depenetration_velocity = 1.0
70sim_params.physx.max_gpu_contact_pairs = 2 ** 23
71sim_params.physx.default_buffer_size_multiplier = 5
72sim_params.physx.use_gpu = True
73

74asset_options = gymapi.AssetOptions()
75asset_options.angular_damping = 0.
76asset_options.collapse_fixed_joints = True
77asset_options.default_dof_drive_mode = 3
78asset_options.density = 0.001
79asset_options.max_angular_velocity = 100.
80asset_options.replace_cylinder_with_capsule = True
81asset_options.thickness = 0.01
82asset_options.flip_visual_attachments = False
83

84render = True
85asset_options.fix_base_link = True  # todo "on rack  if True"
86

87asset_options.use_mesh_materials = False  # color!!! False have color
88sim = gym.create_sim(0, 0, gymapi.SIM_PHYSX, sim_params)
89if sim is None:
90    print("*** Failed to create sim")
91    quit()
92

93plane_params = gymapi.PlaneParams()
94plane_params.normal = gymapi.Vec3(0, 0, 1)
95plane_params.restitution = 0
96gym.add_ground(sim, plane_params)
97

98# create viewer
99if render:
100    viewer = gym.create_viewer(sim, gymapi.CameraProperties())
101    if viewer is None:
102        print("*** Failed to create viewer")
103        quit()
104

105# load asset
106asset_path = "assets/q1/urdf/q1.urdf"
107

108asset_root = os.path.dirname(asset_path)
109asset_file = os.path.basename(asset_path)
110

111asset = gym.load_asset(sim, asset_root, asset_file, asset_options)
112

113# some parameters from 'rhloc_v1/tune_PID'
114action_repeat = 15
115dt = action_repeat * sim_params.dt
116action, motor_position, motor_torque, motor_velocity = [], [], [], []
117

118pose = gymapi.Transform()
119pose.p = gymapi.Vec3(-0., 0., 0.6)  # todo base init position
120

121pose.r = gymapi.Quat(0, -0, -0, 1)  # actor init orientation
122

123# set up the env grid
124num_envs = 1
125num_per_row = 1
126spacing = 3.
127env_lower = gymapi.Vec3(-spacing, -spacing, 0.0)
128env_upper = gymapi.Vec3(spacing, spacing, spacing)
129if render:
130    cam_pos = gymapi.Vec3(1, -1., 0.55)
131    cam_target = gymapi.Vec3(0, 0, 0.55)
132    gym.viewer_camera_look_at(viewer, None, cam_pos, cam_target)
133

134# get array of DOF properties
135init_dof_props = gym.get_asset_dof_properties(asset)
136

137init_dof_props['driveMode'].fill(gymapi.DOF_MODE_EFFORT)
138

139joint_pos_lower_limit = init_dof_props['lower']
140joint_pos_upper_limit = init_dof_props['upper']
141torque_lower_limits = -init_dof_props["effort"]
142torque_upper_limits = init_dof_props["effort"]
143

144# create an array of DOF states that will be used to update the actors
145num_dofs = gym.get_asset_dof_count(asset)
146names_dofs = gym.get_asset_dof_names(asset)
147

148kp = torch.zeros(num_dofs, dtype=torch.float, requires_grad=False)
149kd = torch.zeros(num_dofs, dtype=torch.float, requires_grad=False)
150kp_real = torch.zeros(num_dofs, dtype=torch.float, requires_grad=False)
151kd_real = torch.zeros(num_dofs, dtype=torch.float, requires_grad=False)
152

153#
154stiffness = {'hip_yaw': 50., 'hip_roll': 100., 'hip_pitch': 70., 'knee': 40., 'ankle': 25.}  # [N*m/rad]
155damping = {'hip_yaw': 0.3, 'hip_roll': 2.5, 'hip_pitch': 0.3, 'knee': 0.5, 'ankle': 0.25}  # [N*m*s/rad]
156

157plt_id = list(range(0, 10))
158

159joint_tor_offset = torch.tensor([0.6, 1, 0., 0.7, 0.] + [-0.6, -1, -0., -0.7, 0.], dtype=torch.float)
160joint_vel_sign = torch.tensor([0., 1, 0., 0., 0.] * 2, dtype=torch.float)
161

162s_nsteps_ago = -30
163enable_d_ctrl, a_nsteps_agp = 0., -1
164

165for i in range(num_dofs):
166    for gain_name in stiffness:
167        if gain_name in names_dofs[i]:
168            kp[i] = stiffness[gain_name]
169            kd[i] = damping[gain_name]
170            kp_real[i] = stiffness[gain_name]
171            kd_real[i] = damping[gain_name]
172debug_dir = join('experiments', 'tune_urdf')  # sin_test birl10_on_rack
173ref_act = torch.tensor([0.4, 0.02, -2.3, -0.1, -1.4,
174                             -0.4, -0.02, 2.3, 0.1, 1.4], dtype=torch.float)
175

176os.makedirs(debug_dir, exist_ok=True)
177torque_limits = torch.zeros(num_dofs, dtype=torch.float, requires_grad=False)
178for i in range(num_dofs):
179    torque_limits[i] = init_dof_props["effort"][i].item()
180

181env = gym.create_env(sim, env_lower, env_upper, num_per_row)
182actor_handle = gym.create_actor(env, asset, pose, "actor", 0, 1)
183# reload parameters
184gym.set_actor_dof_properties(env, actor_handle, init_dof_props)
185dof_props = gym.get_actor_dof_properties(env, actor_handle)
186gym.enable_actor_dof_force_sensors(env, actor_handle)
187

188

189def compute_torques(actions, joint_pos, joint_vel, joint_pos_inc):
190    error = actions - joint_pos
191    torques = kp * error + kd * (joint_pos_inc * enable_d_ctrl - joint_vel) - 3.5 * torch.sign(joint_vel) * joint_vel_sign + joint_tor_offset
192    return torch.clip(torques, torch.tensor(torque_lower_limits), torch.tensor(torque_upper_limits))
193

194

195reset_act = ref_act.clone()  # torch.tensor([0.] * num_dofs, dtype=torch.float)
196init_dof_states = np.array(reset_act, dtype=gymapi.DofState.dtype)
197init_dof_vel = np.array([0] * num_dofs, dtype=gymapi.DofState.dtype)
198

199gym.set_actor_dof_states(env, actor_handle, init_dof_states, gymapi.STATE_POS)
200# set init dof velocity
201gym.set_actor_dof_states(env, actor_handle, init_dof_vel, gymapi.STATE_VEL)
202

203dof_state_tensor = gym.acquire_dof_state_tensor(sim)
204dof_states = gymtorch.wrap_tensor(dof_state_tensor)
205

206actor_root_state = gym.acquire_actor_root_state_tensor(sim)
207root_states = gymtorch.wrap_tensor(actor_root_state)
208

209gym.simulate(sim)
210gym.fetch_results(sim, True)
211gym.refresh_dof_force_tensor(sim)
212gym.refresh_dof_state_tensor(sim)
213

214joint_pos = dof_states.view(num_dofs, 2)[..., 0]
215joint_vel = dof_states.view(num_dofs, 2)[..., 1]
216base_quat = root_states[:, 3:7]
217

218noisy_joint_pos = joint_pos.clone()
219noisy_joint_vel = joint_vel.clone()
220joint_pos_his, joint_vel_his, joint_tau_his, joint_act_his = deque(maxlen=150), deque(maxlen=150), deque(maxlen=150), deque(maxlen=150)
221torques = compute_torques(torch.tensor(reset_act), joint_pos, joint_vel, joint_pos * 0.)
222

223start = time.time()
224act = reset_act
225

226act = reset_act.clone()
227act_inc = torch.tensor([0.] * num_dofs, dtype=torch.float)
228

229for _ in range(joint_pos_his.maxlen):
230    joint_pos_his.append(joint_pos.clone())
231    joint_vel_his.append(joint_vel.clone())
232    joint_tau_his.append(torques.clone())
233    joint_act_his.append(joint_pos.clone())
234joint_pos_noise = (2. * torch.rand_like(joint_pos) - 1.) * 0.1
235joint_vel_noise = (2. * torch.rand_like(joint_pos) - 1.) * 1
236joint_tau_noise = (2. * torch.rand_like(joint_pos) - 1.) * 1
237while True:
238    act = ref_act
239    joint_act_his.append(act.clone())
240    for i in range(action_repeat):
241        joint_pos_noise = (2. * torch.rand_like(joint_pos) - 1.) * 0.
242        joint_vel_noise = (2. * torch.rand_like(joint_pos) - 1.) * 0
243        joint_tau_noise = (2. * torch.rand_like(joint_pos) - 1.) * 0
244        selected_actions = joint_act_his[a_nsteps_agp]
245

246        torques = compute_torques(selected_actions,
247                                  joint_pos_his[a_nsteps_agp].clone(),
248                                  joint_vel_his[a_nsteps_agp].clone(),
249                                  act_inc.clone())
250        gym.set_dof_actuation_force_tensor(sim, gymtorch.unwrap_tensor(torques))
251        gym.simulate(sim)
252        gym.fetch_results(sim, True)
253        gym.refresh_dof_force_tensor(sim)
254        gym.refresh_dof_state_tensor(sim)
255        gym.refresh_actor_root_state_tensor(sim)
256

257        mt = gym.get_actor_dof_forces(env, actor_handle)
258        joint_pos_filtered = exp_filter(joint_pos_his[-1], joint_pos + joint_pos_noise, 0.)
259        joint_vel_filtered = exp_filter(joint_vel_his[-1], joint_vel + joint_vel_noise, 0.)
260        joint_tau_filtered = exp_filter(joint_tau_his[-1], torques + joint_tau_noise, 0.)
261

262        joint_pos_his.append(joint_pos_filtered.clone())
263        joint_vel_his.append(joint_vel_filtered.clone())
264        joint_tau_his.append(joint_tau_filtered.clone())
265    base_quat = root_states[:, 3:7]
266    base_rpy = get_euler_from_quat(base_quat)
267    # print("base_quat", base_quat)
268    # print("base_rpy:", base_rpy)
269

270    noisy_joint_pos = joint_pos_his[s_nsteps_ago]
271    noisy_joint_vel = joint_vel_his[s_nsteps_ago]
272    noisy_joint_tau = joint_tau_his[s_nsteps_ago]
273    action.append(act.tolist().copy())
274    mp = noisy_joint_pos.tolist().copy()
275    motor_position.append(mp)
276    mv = noisy_joint_vel.tolist().copy()
277    motor_velocity.append(mv)
278    motor_torque.append(noisy_joint_tau.tolist().copy())
279    if render:
280        # update the viewer
281        gym.step_graphics(sim)
282        gym.draw_viewer(viewer, sim, True)
283        gym.sync_frame_time(sim)
284

285end = time.time()
286print(end - start)
287print("Done")
288

289# convert them from 1 dimension to 2 dimension
290joint_action = np.stack(action.copy())
291joint_position = np.stack(motor_position.copy())
292joint_velocity = np.stack(motor_velocity.copy())
293joint_torque = np.stack(motor_torque.copy())
294

295path = join(debug_dir, f'tune_urdf.xlsx')
296with pd.ExcelWriter(path) as f:
297    pd.DataFrame(np.hstack([joint_action]), columns=names_dofs).to_excel(f, 'joint_act', index=False)
298    pd.DataFrame(np.hstack([joint_position]), columns=names_dofs).to_excel(f, 'joint_pos', index=False)
299    pd.DataFrame(np.hstack([joint_velocity]), columns=names_dofs).to_excel(f, 'joint_vel', index=False)
300    pd.DataFrame(np.hstack([joint_torque]), columns=names_dofs).to_excel(f, 'joint_tau', index=False)
301print(f"Debug data has been saved to {path}.")
302

303if render:
304    gym.destroy_viewer(viewer)
305gym.destroy_sim(sim)
306