interface.interrupt_logic.b_interrupt_button_three = True
# Compute Command
if SimConfig.PRINT_TIME:
start_time = time.time()
command = interface.get_command(copy.deepcopy(sensor_data))
if SimConfig.PRINT_TIME:
end_time = time.time()
print("ctrl computation time: ", end_time - start_time)
# Apply Trq
pybullet_util.set_motor_trq(robot, joint_id, command['joint_trq'])
# Save Image
if (SimConfig.VIDEO_RECORD) and (count % SimConfig.RECORD_FREQ == 0):
# frame = pybullet_util.get_camera_image([1.2, 0.5, 1.], 2.0, 120,
# -15, 0, 60., 1920, 1080,
# 0.1, 100.)
frame = pybullet_util.get_camera_image([0.5, 0.5, 1.], 2.0, 210,
-10, 0, 60., 1920, 1080,
0.1, 100.)
frame = frame[:, :, [2, 1, 0]] # << RGB to BGR
filename = video_dir + '/step%06d.jpg' % count
cv2.imwrite(filename, frame)
p.stepSimulation()
# time.sleep(dt)
t += dt
count += 1
if SimConfig.PRINT_TIME:
start_time = time.time()
command = interface.get_command(copy.deepcopy(sensor_data))
# print("count :", count)
# print(command['joint_trq'])
# if count == 3:
# exit()
if SimConfig.PRINT_TIME:
end_time = time.time()
print("ctrl computation time: ", end_time - start_time)
# Apply Command
pybullet_util.set_motor_trq(robot, joint_id, command['joint_trq'])
# Save Image
if (SimConfig.VIDEO_RECORD) and (count % SimConfig.RECORD_FREQ == 0):
frame = pybullet_util.get_camera_image([1., 0.5, 1.], 1.0, 120,
-15, 0, 60., 1920, 1080,
0.1, 100.)
frame = frame[:, :, [2, 1, 0]] # << RGB to BGR
filename = video_dir + '/step%06d.jpg' % count
cv2.imwrite(filename, frame)
p.stepSimulation()
time.sleep(dt)
t += dt
count += 1
denormalized_output, output = evaluate_crbi_model_using_tf(
crbi_model, sensor_data["base_com_pos"], lf_iso[0:3, 3],
rf_iso[0:3, 3], input_mean, input_std, output_mean, output_std)
local_I_est = inertia_from_one_hot_vec(denormalized_output)
data_saver.add('gt_inertia', inertia_to_one_hot_vec(local_I))
data_saver.add(
'gt_inertia_normalized',
util.normalize(inertia_to_one_hot_vec(local_I), output_mean,
output_std))
data_saver.add('est_inertia_normalized',
np.copy(np.squeeze(output)))
data_saver.add('est_inertia', np.copy(denormalized_output))
data_saver.advance()
# Save Image
if (VIDEO_RECORD) and (count % RECORD_FREQ == 0):
frame = pybullet_util.get_camera_image([1.4, 0.5, 1.], 2.0, 120,
-15, 0, 60, 1920, 1080, 0.1,
500.)
frame = frame[:, :, [2, 1, 0]] # << RGB to BGR
filename = video_dir + '/step%06d.jpg' % count
cv2.imwrite(filename, frame)
# Disable forward step
# p.stepSimulation()
time.sleep(dt)
t += dt
count += 1