How can I rotate the following 3d body with rotation matrix? What´s wrong if the displayed angle is correct?

HI! I have read the euler angles of an IMU and I have read them in Matlab by serial. I would like to do a simulation of a 3d model with the orientation. As you can see, I have created a GUI that shows the values of the angles and gives the possibility of reseting them and closing the serial port.

The euler angles are printed in the GUI if are chosen with the mouse click. I have veryfied that the angles shown in the GUI in real time are correct. That means that I have read the angles correctly. However, I use the same variable (called displayAngleX for x angle, for example) to rotate the 3D model. However, the 3D model doesn´t rotate accordingly to that angles (it rotates a lot). I think I haven´t applied correctly the rotation matrix to the object or the "hgtransform".

Could someone tell me where the mistake is, please?

%Clear all
clc;
close all;
clear all;
%

%% Inicializo el puerto serial donde trabajaré. Borro datos previos y declaro el puerto y la velocidad de transimision de datos
delete(instrfind({'Port'}, {'COM3'}));
arduino = serial('COM3');
arduino.Baudrate = 9600;
%puerto_serial = serial('COM3', 'BaudRate', 9600, 'Terminator', 'CR/LF');
fopen(arduino); %%Abrir el puerto serial
warning('off','MATLAB:serial:fscanf:unsuccesfulRead');
%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%CREAR GUI PARA FIGURA
%Si no existe la figura, la creamos
if (~exist('figureHandle', 'var') || ~ishandle(figureHandle))
    figureHandle = figure(1);
end

%Crear el botón de STOP
if (~exist('stopButton', 'var'))
    stopButton = uicontrol('Style', 'togglebutton', 'String', ...
        'Stop & Close Serial port', ...
        'pos', [0 0 200 25], 'parent', figureHandle);
end

%Selector de ejes
if (~exist('axisSwitchX', 'var'))
    axisSwitchX = uicontrol('Style', 'checkbox', 'String', 'X axis', ...
        'pos', [300 0 50 25], 'parent', figureHandle);
end

if (~exist('axisSwitchY', 'var'))
    axisSwitchY = uicontrol('Style', 'checkbox', 'String', 'Y axis', ...
        'pos', [350 0 50 25], 'parent', figureHandle);
end

if (~exist('axisSwitchZ', 'var'))
    axisSwitchZ = uicontrol('Style', 'checkbox', 'String', 'Z axis', ...
        'pos', [400 0 50 25], 'parent', figureHandle);
end

%Crear ejes de los angulos

if (~exist('degreelabelX', 'var'))
    degreelabelX = uicontrol('Style', 'text', 'String', 'X: 0 degrees', ...
        'pos', [450 100 100 25], 'parent', figureHandle);
end

if (~exist('degreelabelY', 'var'))
    degreelabelY = uicontrol('Style', 'text', 'String', 'Y: 0 degrees', ...
        'pos', [450 75 100 25], 'parent', figureHandle);
end

if (~exist('degreelabelZ', 'var'))
    degreelabelZ = uicontrol('Style', 'text', 'String', 'Z: 0 degrees', ...
        'pos', [450 50 100 25], 'parent', figureHandle);
end

%Crear botón de display de RESET de los angulos
if (~exist('resetRadioButton', 'var'))
    resetRadioButton = uicontrol('Style', 'radiobutton', 'String', 'Reset', ...
        'pos', [500 0 50 25], 'parent', figureHandle);
end
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%VISUALIZACIÓN

%Configurar los ejes
graphAxes = axes('XLim', [-2, 2], 'YLim', [-2, 2], 'ZLim', [-2 2]);
%Vistas 3d
view(3);

% axis off;
axis equal;

%CREAR EL MODELO 3D
% Dim_X = 4;
% Dim_Y = 3;
% Dim_Z = 1;
% 
% vertex_matrix = [0 0 0;
% 1 0 0;
% 1 1 0;
% 0 1 0;
% 0 0 1;
% 1 0 1;
% 1 1 1;
% 0 1 1];
% 
% faces_matrix = [1 2 6 5
% 2 3 7 6
% 3 4 8 7
% 4 1 5 8
% 1 2 3 4
% 5 6 7 8];
% 
% CubeCenter_coord = [Dim_X/2 Dim_Y/2 Dim_Z/2];
% %origin = CubeCenter;
% origin = [0 0 0];
% CubeParameters = [vertex_matrix(:,1)*Dim_X+origin(1),vertex_matrix(:,2)*Dim_Y+origin(2),vertex_matrix(:,3)*Dim_Z+origin(3)];
% cube = patch('Vertices',CubeParameters,'Faces',faces_matrix,'FaceColor', 'blue');


%CREATE 3d MODEL
%%cilindro
[cylX, cylY, cylZ] = cylinder(0.5);
%cono
[conX, conY, conZ] = cylinder([1,0]);

hgTransformArray(1) = surface(cylX, cylY, cylZ);
hgTransformArray(2) = surface(conX, conY, conZ +1);

hgTransform = hgtransform('Parent', graphAxes);

set(hgTransformArray, 'Parent', hgTransform);
drawnow;
pause(0.1);
%%

%%Bucle de visualización
%Variables paa mostrar el angulo en pantalla
displayAngleX = 0;
displayAngleY = 0;
displayAngleZ = 0;

%Variables que guardan el angulo actual
angleX = 0;
angleY = 0;
angleZ = 0;

%Mientras que el boón de stop no se pulse...
while (get(stopButton, 'Value') == 0)
    %Leer angulos actuales
    newAngleX = fscanf(arduino, '%f');
    newAngleY = fscanf(arduino, '%f');
    newAngleZ = fscanf(arduino, '%f');
    

    %Si clickan en el eje X
    if get(axisSwitchX, 'Value') == 1
        %Obtener la diferencia de angulo respecto a la medida anterior
        delta_AngleX = angleX-newAngleX;
        %Calcular el nuevo angulo actual
        displayAngleX = displayAngleX + delta_AngleX;
        %Guardar el valor
        angleX = newAngleX;
        %Actualizar el texto del display
        set(degreelabelX, 'String', ['X: ' num2str(round(displayAngleX)) 'degrees'])
    end
    
    %Si clickan en el eje Y
    if get(axisSwitchY, 'Value') == 1
        %Obtener la diferencia de angulo respecto a la medida anterior
        delta_AngleY = angleY-newAngleY;
        %Calcular el nuevo angulo actual
        displayAngleY = displayAngleY + delta_AngleY;
        %Guardar el valor
        angleY = newAngleY;
        %Actualizar el texto del display
        set(degreelabelY, 'String', ['Y: ' num2str(round(displayAngleY)) 'degrees'])
    end
    
    %Si clickan en el eje Z
    if get(axisSwitchZ, 'Value') == 1
        %Obtener la diferencia de angulo respecto a la medida anterior
        delta_AngleZ = angleZ-(-newAngleZ);
        %Calcular el nuevo angulo actual
        displayAngleZ = displayAngleZ + delta_AngleZ;
        %Guardar el valor
        angleZ = -newAngleZ;
        %Actualizar el texto del display
        set(degreelabelZ, 'String', ['Z: ' num2str(round(displayAngleZ)) 'degrees'])
    end
    
        %Formar matriz de rotación = ROTATION FOR CUBE
%     rotate(cube, [1,0,0], newAngleX);
%     rotate(cube,[0,1,0], newAngleY);
%     rotate(cube, [0, 0, 1], -newAngleZ);
%     view(3);
    %Transformaciones
    
%     drawnow;

    %ROTATION FOR FIGURE OF CYLINDER & CONE
    R = makehgtform('xrotate', (round(displayAngleX)), 'yrotate', (round(displayAngleY)), 'zrotate', (round(displayAngleZ)));
    set(hgTransform, 'Matrix', R)
    
        %Si se pulsa el botón de RESET entonces...
    if (get(resetRadioButton, 'Value') == 1)
        %Resetear los angulos del display
        displayAngleX = 0;
        displayAngleY = 0;
        displayAngleZ = 0;
        
        %Resetera el botón de RESET como no apretado
        set(resetRadioButton, 'Value', 0);
    end
    xlabel('X');ylabel('Y');zlabel('Z');
    drawnow;
end
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CloseSerialPort();

As you can see, I tried creating a 3D cube with patch function, but the "rotate" function (which works in another script that doesn´t have a GUI) doesn´t work too.

There is no any syntax error but something must be wrong.

Answer 1

[Mark Sherstan]

I did a similar project in the past. I have attachted my working code below without the Arduino connection so it should run as is. My rotation matrix might be refernced different than yours (look for more information here at the bottom of the page) but hopefully this will get you up and running.

MATLAB is extremly slow on the serial connection and depending if you are using real time data or not you may run into some issues. Good luck!

clear all
close all

figureHandle = figure(1);
StopButton = uicontrol('Style','pushbutton','String','Stop & Close Serial Port','pos',[0, 0, 200, 25],'Callback','delete(gcbo)');
degreeLabelX = uicontrol('Style','text','String','X:  0 Degrees','pos',[450, 50, 100, 20],'parent',figureHandle);
degreeLabelY = uicontrol('Style','text','String','Y:  0 Degrees','pos',[450, 30, 100, 20],'parent',figureHandle);
degreeLabelZ = uicontrol('Style','text','String','Z:  0 Degrees','pos',[450, 10, 100, 20],'parent',figureHandle);
set(gcf,'Color','black');

x = 0;
y = 0;
z = 0;
i = 1;

while ishandle(StopButton)

	[vert, face] = NewCoords(x,y,z);
	view([1, 0, 0]);

	h = patch('Vertices',vert,'Faces',face,'FaceVertexCData',3,'FaceColor','flat');

	set(degreeLabelX,'String', ['X:  ' num2str(round(x)) ' degrees']);
	set(degreeLabelY,'String', ['Y:  ' num2str(round(y)) ' degrees']);
	set(degreeLabelZ,'String', ['Z:  ' num2str(round(z)) ' degrees']);

	axis off;
	axis([-1.1,1.1,-1.1,1.1,-1.1,1.1]);

i = i + 0.5;

	x = i;
	x = updateAngle(x);

	y = i;
	y = updateAngle(y);

	z = i;
	z = updateAngle(z);

	pause(0.0001);

	drawnow;
	delete(h);

end

close all

function [angle] = updateAngle(angle)

if (angle < 0)
	angle = angle + 360;
elseif (angle >= 360)
	angle = angle - 360;
else
	return
end

angle = updateAngle(angle);

function [A, Face] = NewCoords(roll, pitch, yaw)

% Set up the cube
initial = [0 0 0; 1 0 0; 1 1 0; 0 1 0; 0 0 1; 1 0 1; 1 1 1; 0 1 1] - 0.5;
Face = [1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8; 1 2 3 4; 5 6 7 8];

% Create individual trasformation matrices
yawMatrix   = [1  0           0;
                0  cosd(roll)  -sind(roll);
                0  sind(roll)   cosd(roll)];

pitchMatrix = [cosd(pitch)   0  sind(pitch);
                0             1  0;
                -sind(pitch)  0  cosd(pitch)];

rollMatrix  = [cosd(yaw)  -sind(yaw) 0;
                sind(yaw)  cosd(yaw)  0;
                0          0          1];

% Calculate the final transformation matrix
rotationMatrix = yawMatrix*pitchMatrix*rollMatrix;
A = initial * rotationMatrix;

Answer 2

[Aitor Burdaspar]

Thank you very much for your answer Mark,

I was abroad and I couldnt answer you before. I will try using some ideas of your code in order to fix my problem. Anyway, I have a question. For example, in my case, i read the euler angles from the arduino with different ranges. I mean:

The "x" angle value is between 0º and 360º

The "y" angle value is within -90º and 90º range.

The "z" value is within -180º and 180º range.

Therefore, I think that maybe I should scale that values in order to have each angle between 0º and 360º. In that way, the rotation matrix might work better... what do you think?

[Mark Sherstan]

What sensor are you using to get your angles and how are you calculating them? I prefer to work with angles between 0 and 360 but it all depends on your application. Either way you still may run into some singularities.

[Aitor Burdaspar]

I am using the sensor BNO055. I´m using its library in order to read the values. It measures:

The "x" value between 0º and 360º

The "y" value between -90º and 90º range.

The "z" value between -180º and 180º range

The "x" is related with the YAW

The "y" with the PITCH

The "z" with the ROLL

Yes, I have some singularities in my program and that´s the reason of why I asked if scalling these values in Matlab is a good idea.

On the other hand, I am able to read the quaternions in Matlab (and solve the problem of singularities), and I can convert the quaternion to a Rotation Matrix. However, I couldn´t achieve applying that rotation matrix to a 3D model yet.

Thanks anyway, Mark ;)