CODEWITHSUNDEEP
matlab3dtransformkinematics
Blackdrive
Blackdrive

Reputation: 1

Translate global to local coordinates

I've got a moving rigid body for which I need to calculate the position of a point on the surface in function of time. I've got the following input:

I first determine the location of the point in the local (body fixed) coordinate system for the first timestep. Then, I calculate the coordinates of that point in global coordinates for the second step, using the corresponding rotation matrix (as illustrated in this question). I manualy measured where that point should be and that doesn't match.

The example code below probably better illustrates my method and the issue. Is the methodology or the implementation wrong? And how do I approach this correctly? The code is maybe a bit lengthy but allows for execution. I'm doing this in Matlab.

%% Clean workspace and set path
% Clean up the workspace, command window, close figures
clear all
close all
clc

  
%% Object inputs
% Coordinates of 3 orthogonal axes through the center of gravity, in the
% global coordinate system. At the first timestep.
object{1}.X.startpoint  = [24.7349058855438,-11.5479658533523,1098.57662582473];
object{1}.X.endpoint    = [-0.832905885543847,1.10916585335228,1097.37317417527];

object{1}.Y.startpoint  = [5.676667643105184,-17.997569574970736,1.096883018501248e+03];
object{1}.Y.endpoint    = [18.225332356894818,7.558769574970736,1.099066781498751e+03];

object{1}.Z.startpoint  = [12.973532739613240,-4.506163733481916,1.083752143929416e+03];
object{1}.Z.endpoint    = [10.4904947081313,-6.23813055135267,1118.28956543937];

% Same information for the object at a second timestep
object{2}.X.startpoint  = [24.656923458165010,-8.251284344143922,1.104914286298065e+03];
object{2}.X.endpoint    = [0.352676541834988,3.899684344143922,1.091666913701935e+03];

object{2}.Y.startpoint  = [13.544852623105038,9.838707132571910,1.107402610954567e+03];
object{2}.Y.endpoint    = [11.464747376894960,-14.190307132571911,1.089178589045433e+03];

object{2}.Z.startpoint  = [3.577145171754987,-9.045948694099657,1.108368142901039e+03];
object{2}.Z.endpoint    = [21.432454828245014,4.694348694099658,1.088213057098961e+03];

% Coordinates of a point fixed on the (rigid) object, in the global
% coordinate system. At the first timestep.
object{1}.POI.dorsal.global = [19.231140624851460,-0.281237811310405,1.099798468105154e+03];

%% Check
% We measured manualy what the position of the same point should be at the
% second timestep. We'll use this to check the calculate. A small error is
% to be expected.
check = [16.17432165785938  3.6337839926906312  1103.4455838115914];

%% Calculate the coordinate of the origin of the coordinate system fixed to the object

for i=1:2
    M_start = [object{i}.X.startpoint;object{i}.Y.startpoint;object{i}.Z.startpoint];
    M_end   = [object{i}.X.endpoint;object{i}.Y.endpoint;object{i}.Z.endpoint];
    
    [P,dist] = lineIntersect3D(M_start,M_end);
    object{i}.COG = P;
end

% to execute the code without the lineInterSect3D function, use
% object{1}.COG = [11.951000000000000,-5.219399999999998,1.097974900000000e+03];
% object{2}.COG = [12.504799999999960,-2.175799999999953,1.098290600000000e+03];


%% Rotation matrix
% Create from the known axis in the global coordinate system a normalized
% orthogonal matrix. This is the rotation matrix. 

for i=1:2
    % x,y,z coordinates in the global frame of the original axis line end points
    M_end   = [object{i}.X.endpoint;object{i}.Y.endpoint;object{i}.Z.endpoint];
    % subtract the COG coodinates for each the origin of the object frame
    % with the global coordinate system
    M_tmp   = M_end-repmat(object{i}.COG,3,1); 
    % Normalize
    V = [M_tmp(1,:)/norm(M_tmp(1,:));M_tmp(2,:)/norm(M_tmp(2,:));M_tmp(3,:)/norm(M_tmp(3,:))]; 
    object{i}.rotMatrix = V;
end

%% POI to local coordinates for the 1st timestep
% First shift the point with the COG vector then rotate with the rotation
% matrix.
tmp = object{1}.POI.dorsal.global - object{1}.COG;
object{1}.POI.dorsal.local = object{1}.rotMatrix*tmp';

%% And back to global in the second timestep

% test if the transformation works backwards for the 1st points:
test = (object{1}.rotMatrix.' * object{1}.POI.dorsal.local)' + object{1}.COG % first rotate, then translate. This should give back the global coord.
object{1}.POI.dorsal.global
dist_1 = norm(object{1}.POI.dorsal.global-test) % that works

% now do this for the second timestep
% the coordinate of the point of interest in the local (fixed to the
% object) coordinate system has not changed
object{2}.POI.dorsal.local = object{1}.POI.dorsal.local;
% rotate with the transpose of the rotation matrix (of the second timestep)
% and translate with the vector to the object fixed coordinate system
object{2}.POI.dorsal.global = (object{2}.rotMatrix' * object{2}.POI.dorsal.local)' + object{2}.COG;

%% Check the results
% The results don't match with what we manualy measured however
object{2}.POI.dorsal.global
check
dist_2 = norm(object{2}.POI.dorsal.global-check) % that doesn't work

% Plot the result

c = object{2};
P_actual = check; % for datapoint 2 lunate dorsal global

vecX_X = [c.X.startpoint(1) c.X.endpoint(1)];vecX_Y = [c.X.startpoint(2) c.X.endpoint(2)];vecX_Z = [c.X.startpoint(3) c.X.endpoint(3)];
vecY_X = [c.Y.startpoint(1) c.Y.endpoint(1)];vecY_Y = [c.Y.startpoint(2) c.Y.endpoint(2)];vecY_Z = [c.Y.startpoint(3) c.Y.endpoint(3)];
vecZ_X = [c.Z.startpoint(1) c.Z.endpoint(1)];vecZ_Y = [c.Z.startpoint(2) c.Z.endpoint(2)];vecZ_Z = [c.Z.startpoint(3) c.Z.endpoint(3)];
POI_X = [c.COG(1) c.POI.dorsal.global(1)];POI_Y = [c.COG(2) c.POI.dorsal.global(2)];POI_Z = [c.COG(3) c.POI.dorsal.global(3)];

figure
title('Global')
hold all
plot3(vecX_X,vecX_Y,vecX_Z,'b-')
plot3(vecY_X,vecY_Y,vecY_Z,'r-')
plot3(vecZ_X,vecZ_Y,vecZ_Z,'k-')
plot3(POI_X,POI_Y,POI_Z,'mo--')
plot3(P_actual(1),P_actual(2),P_actual(3),'gd')
plot3(c.COG(1),c.COG(2),c.COG(3),'kd')
plot3(c.X.endpoint(1),c.X.endpoint(2),c.X.endpoint(3),'bo')
plot3(c.Y.endpoint(1),c.Y.endpoint(2),c.Y.endpoint(3),'ro')
plot3(c.Z.endpoint(1),c.Z.endpoint(2),c.Z.endpoint(3),'ko')



legend('X','Y','Z','POI','checkpoint','COG')
grid on

Checking the determinant of the rotation matrix, I see that it is -1. Indeed, the body-fixed axes define a left-handed system and transform to a righthanded one. To ensure this isn't the cause of the issue, I have swapped start and endpoint for the Y vector (effectively creating a righthanded coordinate system. To no avail.

Upvotes: 0

Views: 788

Answers (1)

Neil Bajaj
Neil Bajaj

Reputation: 1

  1. Find the vector (in the global frame) from the coordinate frame origin to the point you care about on the surface. Call this vector V.
  2. Compute the projection of V onto your x, y, and z axes; i.e. take the dot products: dot(V, x), dot(V, y), dot(V, z). These dot products are the vector in the local frame. This can also be computed by inv(R)*V. Since R is an orthonormal rotation matrix, inv(R)=R', so its just R'V.

Not sure why you’re rotation matrix has det = -1, but I would check that the way you’re constructing your vectors is right-handed. In fact, since you know X and Y, why not use Z=cross(X, Y)? If x and y are orthogonal, you’ll be guaranteed to get a right handed CS.

Upvotes: 0

Related Questions