%
% Odometry 2D coordinate transformation example
%
% Chad Jenkins (cjenkins@cs.brown.edu)
% Modified: October 9, 2009
%
% show coordinate frames for a moving robot with respect to odometry and a
% stationary ball
%
% note: robot following set trajectory, not necessarily moving towards the ball
%

close all;
clear all;

%%%% ==== initialize odometric coordinate transforms ====

% set initial pose of robot in world coordinates, establishes robot's odometric coordinate frame
initial_world_location = [5 3 pi/6]; % 2D position vector and 1D yaw rotation


% build transforms from world space to odometric space
transform_rotate_world_to_odometry = [  [cos(initial_world_location(1,3)) -sin(initial_world_location(1,3)) 0]
    [sin(initial_world_location(1,3)) cos(initial_world_location(1,3)) 0]
    [0 0 1]  ];
transform_translate_world_to_odometry = [  [1 0 initial_world_location(1,1)]
    [0 1 initial_world_location(1,2)]
    [0 0 1]  ];
transform_world_to_odometry = transform_translate_world_to_odometry * transform_rotate_world_to_odometry;

% transform world space coordinate vectors into odometric frame
odometric_frame_origin = transform_world_to_odometry * [0 0 1]';
odometric_frame_x = transform_world_to_odometry * [1 0 1]';
odometric_frame_y = transform_world_to_odometry * [0 1 1]';


%%%% ==== initialize ball object and pose trajectory ====

% set trajectory of robot poses in odometric coordinates
pose_trajectory_case = 0;
if (pose_trajectory_case == 1)
    odometric_robot_pose = [1.5 3 pi/4]; % single robot pose
else
    % sequence of robot poses as concatentation of (2D pos, 1D yaw) vectors
    odometric_robot_pose = [
        [0.5+[0:0.1:1]*15]' [0.3+[0:0.1:1]*3]' [0+[0:0.1:1]*(pi/3)]'
        ];
end

% set geometry of robot in its local (pose) coordinates
robot_geometry_vertices_pose = [[0.5 -0.5 1]' [1.5 -0.0 1]' [0.5 0.5 1]' [-0.5 0.5 1]' [-0.5 -0.5 1]' ];
% sequence of 2D vertices in homogeneous coordinates traversing outline of
% robot geometry in the robot's local coordinates


%%%% ==== initialize ball object and coordinate transforms ====

% set location of ball in world coordinates
ball_location_world = [15 4]; % 2D location of the ball in world coordinates

% set geometry of ball in its local (ball) coordinates
ball_geometry_vertices_ball = [cos([0:pi/6:2*pi]); sin([0:pi/6:2*pi]); ones(size(sin([0:pi/6:2*pi])))];
% sequence of 2D vertices in homogeneous coordinates traversing outline of
% ball in the ball's local coordinates

% build transform from world space to ball space
transform_translate_world_to_ball = [  [1 0 ball_location_world(1,1)]
    [0 1 ball_location_world(1,2)]
    [0 0 1]  ];

% temporary variables to put spacing around objects in figure
display_min = [0 0 1]';
display_max = [0 0 1]';
display_min = min([display_min odometric_frame_origin odometric_frame_x odometric_frame_y],[],2);
display_max = max([display_max odometric_frame_origin odometric_frame_x odometric_frame_y],[],2);


%%%% ==== update and display loop: iterates over every pose in the trajectory ====

% for each odometric pose: compute and perform transformation, display pose space axes
for i = 1:size(odometric_robot_pose,1)

    %%%% ==== build transforms to and from pose space ====

    % build transforms from odometric space to pose space
    transform_rotate_odometry_to_pose = [  [cos(odometric_robot_pose(i,3)) -sin(odometric_robot_pose(i,3)) 0]
        [sin(odometric_robot_pose(i,3)) cos(odometric_robot_pose(i,3)) 0]
        [0 0 1]  ];
    transform_translate_odometry_to_pose = [  [1 0 odometric_robot_pose(i,1)]
        [0 1 odometric_robot_pose(i,2)]
        [0 0 1]  ];
    transform_odometry_to_pose = transform_translate_odometry_to_pose * transform_rotate_odometry_to_pose;

    % build transform from world space to pose space
    transform_world_to_pose =  transform_world_to_odometry * transform_odometry_to_pose;

    % build transforms from pose to odometry to world space; "undo" previous transforms
    transform_rotate_pose_to_odometry = transform_rotate_odometry_to_pose'; % transpose inverts rotation matrices
    transform_rotate_odometry_to_world = transform_rotate_world_to_odometry';
    transform_translate_pose_to_odometry = transform_translate_odometry_to_pose;
    transform_translate_pose_to_odometry(1:2,3) = -transform_translate_odometry_to_pose(1:2,3); % negating displacements inverts translation matrices
    transform_translate_odometry_to_world = transform_translate_world_to_odometry;
    transform_translate_odometry_to_world(1:2,3) = -transform_translate_world_to_odometry(1:2,3);
    transform_pose_to_odometry = transform_rotate_pose_to_odometry * transform_translate_pose_to_odometry;
    transform_odometry_to_world = transform_rotate_odometry_to_world * transform_translate_odometry_to_world;
    transform_pose_to_world = transform_pose_to_odometry * transform_odometry_to_world;
    

    %%%% ==== draw objects in world coordinates ====
        
    % draw world space coordinate axes
    figure(1);
    subplot(1,2,1);
    hold off;
    plot([0 1],[0 0],'g-','LineWidth',5);
    hold on;
    text(1,0,'world_x');
    plot([0 0],[0 1],'r-','LineWidth',5);
    text(0,1,'world_y');
    title('world space');

    % draw odometric coordinate axes in world coordinates
    plot([odometric_frame_origin(1,1) odometric_frame_x(1,1)],[odometric_frame_origin(2,1) odometric_frame_x(2,1)],'g-','LineWidth',5);
    hold on;
    text(odometric_frame_x(1,1),odometric_frame_x(2,1),'odometric_x');
    plot([odometric_frame_origin(1,1) odometric_frame_y(1,1)],[odometric_frame_origin(2,1) odometric_frame_y(2,1)],'r-','LineWidth',5);
    text(odometric_frame_y(1,1),odometric_frame_y(2,1),'odometric_y');

    ball_geometry_vertices_world = transform_translate_world_to_ball * ball_geometry_vertices_ball;
    patch(ball_geometry_vertices_world(1,:),ball_geometry_vertices_world(2,:), 'y');

    % draw ball in world coordinates (also vector from world origin to ball)
    % plot([0 ball_location_world(1,1)],[0 ball_location_world(1,2)],'m-','LineWidth',2);
    % plot([pose_frame_origin(1,1) ball_location_pose(1,1)],[pose_frame_origin(2,1) ball_location_pose(2,1)],'m-','LineWidth',2);

    % transform and draw robot geometry in world coordinates
    robot_geometry_vertices_world = transform_world_to_pose * robot_geometry_vertices_pose;
    patch(robot_geometry_vertices_world(1,:),robot_geometry_vertices_world(2,:), 'b');%,'FaceAlpha',0.99);
    
    % transform origin and axis vectors into robot pose frame
    pose_frame_origin = transform_world_to_pose * [0 0 1]';
    pose_frame_x = transform_world_to_pose * [1 0 1]';
    pose_frame_y = transform_world_to_pose * [0 1 1]';

    % draw pose coordinate axes in world coordinates
    plot([pose_frame_origin(1,1) pose_frame_x(1,1)],[pose_frame_origin(2,1) pose_frame_x(2,1)],'g-','LineWidth',5);
    text(pose_frame_x(1,1),pose_frame_x(2,1),sprintf('pose_{%dx}',i));
    plot([pose_frame_origin(1,1) pose_frame_y(1,1)],[pose_frame_origin(2,1) pose_frame_y(2,1)],'r-','LineWidth',5);
    text(pose_frame_y(1,1),pose_frame_y(2,1),sprintf('pose_{%dy}',i));

    

    % temporary variables to put spacing around objects in figure
    display_min = min([display_min pose_frame_origin pose_frame_x pose_frame_y],[],2);
    display_max = max([display_max pose_frame_origin pose_frame_x pose_frame_y],[],2);
    plot([display_min(1,1)-1 display_max(1,1)+1],[display_min(2,1)-1 display_max(2,1)+1],'w.');


    %%%% ==== compute direction to ball and transform into pose space ====
        
    %ball_location_pose = transform_world_to_pose * [ball_location_world(1,1) ball_location_world(1,2) 1]';

    % transform of control to ball (as vector from pose origin to ball in
    % world space) into robot pose space
    ball_location_pose = transform_pose_to_world * [[ball_location_world(1,1) ball_location_world(1,2) 1]' pose_frame_origin];

    % (less elegant) transform of control to ball into robot pose space
    % by rotating world space vector that is anchored at pose origin
    robotball_world_poseanchor = [ball_location_world(1,1) ball_location_world(1,2) 1]'-pose_frame_origin; % vector to ball in world coordinates, anchored in pose coodinates
    robotball_world_poseanchor(3,1) = 1;
    plot([pose_frame_origin(1,1) pose_frame_origin(1,1)+robotball_world_poseanchor(1,1)],[pose_frame_origin(2,1) pose_frame_origin(2,1)+robotball_world_poseanchor(2,1)],'-','LineWidth',2,'Color',[1 0.6 0]);
    plot([0 robotball_world_poseanchor(1,1)],[0 robotball_world_poseanchor(2,1)],'-','LineWidth',2,'Color',[1 0.6 0]);
    robotball_pose_poseanchor = transform_rotate_world_to_odometry' * transform_rotate_odometry_to_pose' * robotball_world_poseanchor;

    axis('equal');

    
    %%%% ==== transform objects into robot pose coordinates ====

    % transform ball into world then pose coordinates
    ball_geometry_vertices_pose = transform_pose_to_world * transform_translate_world_to_ball * ball_geometry_vertices_ball;

    
    %%%% ==== draw objects and direction to ball in robot pose coordinates ====
    
    % draw pose space coordinate axes
    %figure(2);
    subplot(1,2,2);
    hold off;
    plot([0 1],[0 0],'g-','LineWidth',5);
    hold on;
    text(1,0,'pose_x');
    plot([0 0],[0 1],'r-','LineWidth',5);
    text(0,1,'pose_y');

    % draw robot geometry, originally described in pose coordinates
    patch(robot_geometry_vertices_pose(1,:),robot_geometry_vertices_pose(2,:), 'b');
    % draw ball geometry, previously transformed to pose coordinates
    patch(ball_geometry_vertices_pose(1,:),ball_geometry_vertices_pose(2,:), 'y');
    
    % draw transformed control vector to ball
    plot(ball_location_pose(1,:),ball_location_pose(2,:),'k-','LineWidth',5);
    text(ball_location_pose(1,1),ball_location_pose(2,1),'pose_u');
    % draw global space control vector to ball
    plot([0 robotball_world_poseanchor(1,1)],[0 robotball_world_poseanchor(2,1)],'-','LineWidth',5,'Color',[1 0.6 0]);
    text(robotball_world_poseanchor(1,1),robotball_world_poseanchor(2,1),'world_u');
    % draw (alternative but equivalent) transformed control vector to ball
    plot([0 robotball_pose_poseanchor(1,1)],[0 robotball_pose_poseanchor(2,1)],'m-','LineWidth',1);

    title('robot pose space');
    axis('equal');



    %%%% ==== wait for input, or refresh and keep animating ====
    
    if (0 == 1)
    input(sprintf('pose %d rendered... press any key to continue\n',i));
    else
        refresh;
        drawnow;
        pause(0.3);
    end
end;


