0
$\begingroup$

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!

$\endgroup$

1 Answer 1

0
$\begingroup$

Matrix multiplication is not associative. In order to calculate the transform composition in the robot moving frame you should do:

m = m*transf_mat;

when you do:

m = transf_mat*m;

you are transforming the robot frame relative to your fixed (inertial) axis.

plot after changing the multiplication order:

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.