Pose matrices and robotics 2D movement

36 Views Asked by At

I have a robot starting from the origin. It first turns 90 degrees (yaw), then move to (5,10), then it starts to move along its own negative x direction at each step with a translation of 1. Here is the code:

 close all
    
%% Set up the robot, with its tip pointing its own +x
x_points = [-1, 2, 3,  2, -1, -1];
y_points = [2, 2, 0.5, -1, -1, 2];
points = [x_points; y_points; ones(1, length(x_points))];
    
%% translation
x_trans = [5 1 1 1 1 1 1 1 1 1]; 
y_trans = [10 0 0 0 0 0 0 0 0 0]; 
    
%% rotation
theta = [pi/2 0 0 0 0 0 0 0 0 0];
    
clf;
plot(x_points, y_points, '-r' ); grid on
hold on; axis image
xlim([-5 20]); 
ylim([-5 20]);
xlabel('x')
ylabel('y');

%% initial transfer matrix
m = 1;
 
%% loop through the preset motion for the robot
for n = 1:length(sxx)
    t = theta(n);
    sx = x_trans(n);
    sy = y_trans(n);
    
    % transformation matrix at each step
    transf_mat = [  cos(t), -sin(t), sx;  
                    sin(t),  cos(t), sy; 
                         0,       0, 1];

    % composit transformation matrix
    m = transf_mat*m;
    
    % Compute the new points
    transf_pts = m*points;
    
    % Plot the points
    plot(transf_pts(1,:), transf_pts(2,:), '-k' ); grid on    
    hold on; 
    xlim([-5 20]); 
    ylim([-5 20]);
    drawnow;
    pause(0.2);
end  

and here is the plot of the moving trajectory of the robot: enter image description here

However, since the very first motion for the robot is to turn +90 degrees, so I am expecting the robot to start moving downwards (in a negative y direction in the world coordinates), because, now the negative x direction of the robot is actually the negative y direction in the world coordinates after the first +90 degrees (yaw) rotation. So shouldn't the plot look something like this?

enter image description here

I have been thinking about this for two days, and now I am really confused. Can anyone tell me where I did wrong perhaps? Thank you so much!