def signal_handler(signal, frame):
if SimConfig.VIDEO_RECORD:
pybullet_util.make_video(video_dir)
p.disconnect()
sys.exit(0)
print("T_w_base")
print(T_w_base)
print("T_w_ee")
print(T_w_ee)
print("q_guess")
print(q_guess)
print("q_sol")
print(q_sol)
__import__('ipdb').set_trace()
print("====================================")
# Handle timings
if vis_idx == len(vis_time) - 1:
vis_idx = 0
if VIDEO_RECORD:
pybullet_util.make_video(video_dir, False)
p.disconnect()
exit()
else:
vis_idx += 1
# Visualize config
pybullet_util.set_config(robot, joint_id, link_id, base_pos, base_quat,
joint_pos)
if b_crbi and VIDEO_RECORD:
robot_sys.update_system(
sensor_data["base_com_pos"], sensor_data["base_com_quat"],
sensor_data["base_com_lin_vel"],
sensor_data["base_com_ang_vel"], sensor_data["base_joint_pos"],
sensor_data["base_joint_quat"],
