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:

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?
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!
