Linux - Rotacion de objeto 3D a tiempo real MATLAB

Rotacion de objeto 3D a tiempo real MATLAB

Publicado por Aitor (1 intervención) el 09/04/2019 09:22:43

Hola a todos!

He leído los datos de una IMU en arduino y los he recibido por puerto serial en Matlab. Me gustaría hacer una simulación 3D con la orientación de los angulos de euler recibidos. Como podeís ver, he creado una GUI que permite ver cual es el valor de los angulos de euler.
Estos angulos son printados en la GUI si se clickan con el mouse primero. IHe comprobado que en el display de la GUI que muestra los valores de los angulos, los valores son correctos. Eso significa que los he leído bien en la IMU. Sin embargo, la misma variable que uso para printar el valor actual de cada angulo ("displayAngleX" para el angulo x, por ejemplo) para rotar el modelo 3D. Sin embargo, el modelo 3D no rota acorde a ese ángulo. Creo que no he aplicado correctamente la matriz de rotación al modelo 3D

Lo he intentado con dos modelos diferentes, y me pasa exactamente lo mismo en ambos. El ángulo del display está correctamente calculado, pero la figura no responde a esos ángulos. Un modelo 3D (creado con hgtransform) y sus rotaciones están comentados, pero no funciona (la animación 3D rota excesivamente).

Otro de los modelos 3D (cubo creado con patch) tanto sus matrices de transformación están sin comentar. Lo curioso de esto es que si creo el mismo cubo y aplico las mismas transformadas en otro script que no tiene GUI, me funciona correctamente. En cambio, con la GUI no me funciona.

¿Alguna idea de como corregir (al menos uno de los dos casos)? El código es el siguiente:

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

%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();.

Valora esta pregunta

Me gusta: Está pregunta es útil y esta clara

No me gusta: Está pregunta no esta clara o no es útil

Responder