function [x_axis, y_axis, z_axis] = calculate_frame(p, R, coord_ax_length, z_offset)
% Return plot data for a coordinate frame

if nargin < 4
    coord_ax_length = 20;
    z_offset = 0.5; % Shift frame up to avoid occlusion with base or platform geometry
end

x_axis = [0, coord_ax_length; 0, 0; z_offset, z_offset];
y_axis = [0, 0; 0, coord_ax_length; z_offset, z_offset];
z_axis = [0, 0; 0, 0; z_offset, coord_ax_length+z_offset];

% Rigid transformation
x_axis = p + R*x_axis;
y_axis = p + R*y_axis;
z_axis = p + R*z_axis;

end