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