How can I rotate the following 3d body with rotation matrix? What´s wrong if the displayed angle is correct?
7 visualizaciones (últimos 30 días)
Mostrar comentarios más antiguos
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.
0 comentarios
Respuestas (2)
Mark Sherstan
el 12 de Abr. de 2019
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;
0 comentarios
Aitor Burdaspar
el 17 de Abr. de 2019
2 comentarios
Mark Sherstan
el 17 de Abr. de 2019
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.
Ver también
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!