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

5 次查看（过去 30 天）
Aitor Burdaspar
Aitor Burdaspar 2019-4-8
评论： Aitor Burdaspar 2019-4-17
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.

请先登录，再回答此问题。

回答（2 个）

Mark Sherstan
Mark Sherstan 2019-4-12
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;
Aitor Burdaspar
Aitor Burdaspar 2019-4-17
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?
  2 个评论
Mark Sherstan
Mark Sherstan 2019-4-17
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
Aitor Burdaspar 2019-4-17
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 ;)

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 Programming 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by