Posicionamiento RB-VOGUI de Robotnik
Publicado por Sacar datos de coordenadas del RB-VOGUI de Robotnik (1 intervención) el 20/07/2023 14:14:25
Hola me llamo Jorge,
He utilizado el robot RB-VOGUI de Robotnik para dar una vuelta y quiero sacar una gráfica de las coordenadas de su trayectoria en XY. El robot no proporciona directamente sus coordenadas, sino que da las transformadas entre sus sistemas de referencia. Conozco el punto de inicio del robot con respecto a una referencia del terreno.
Lo que he hecho ha sido generar en Matlab las matrices de transformación para pasar del sistema footprint a odom (A_foot_odom) y de odom a map (A_odom_map). Para después multiplicar por el vector de coordenadas iniciales (Xo).
Así es como genero las matrices:
Y este el cálculo de las coordenadas transformadas, teniendo en cuenta que las coordenadas en X e Y son las 2 primeras columnas de explorer_final_Xo:
Por otro lado, el fichero csv que obtengo me da valores de traslaciones multiplicadas por 10^16, lo cual no comprendo.
Si alguno ha trabajado ya con este robot y sabe qué transformaciones hay que realizar o si alguno ve un error en el código, os agradecería mucho vuestra ayuda.
Un saludo.
He utilizado el robot RB-VOGUI de Robotnik para dar una vuelta y quiero sacar una gráfica de las coordenadas de su trayectoria en XY. El robot no proporciona directamente sus coordenadas, sino que da las transformadas entre sus sistemas de referencia. Conozco el punto de inicio del robot con respecto a una referencia del terreno.
Lo que he hecho ha sido generar en Matlab las matrices de transformación para pasar del sistema footprint a odom (A_foot_odom) y de odom a map (A_odom_map). Para después multiplicar por el vector de coordenadas iniciales (Xo).
Así es como genero las matrices:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
quat_foot_odom=[tf_foot_odom.orientation.x(i) tf_foot_odom.orientation.y(i) tf_foot_odom.orientation.z(i)
tf_foot_odom.orientation.w(i)];
foot_odom_euler = quat2eul(quat_foot_odom,'XYZ');
rx_foot_odom=rotx(foot_odom_euler(1));
ry_foot_odom=roty(foot_odom_euler(2));
rz_foot_odom=rotz(foot_odom_euler(3));
quat_odom_map=[tf_odom_map.orientation.x(i) tf_odom_map.orientation.y(i) tf_odom_map.orientation.z(i)
tf_odom_map.orientation.w(i)];
odom_map_euler = quat2eul(quat_odom_map,'XYZ');
rx_odom_map=rotx(odom_map_euler(1));
ry_odom_map=roty(odom_map_euler(2));
rz_odom_map=rotz(odom_map_euler(3));
R_foot_odom = rx_foot_odom*ry_foot_odom*rz_foot_odom;
d_foot_odom = [tf_foot_odom.position.x(i); tf_foot_odom.position.y(i); tf_foot_odom.position.z(i)];
A_foot_odom = [R_foot_odom d_foot_odom;
0 0 0 1];R_odom_map = rx_odom_map*ry_odom_map*rz_odom_map;
d_odom_map = [tf_odom_map.position.x(i); tf_odom_map.position.y(i); tf_odom_map.position.z(i)];
A_odom_map = [R_odom_map d_odom_map;
0 0 0 1];Y este el cálculo de las coordenadas transformadas, teniendo en cuenta que las coordenadas en X e Y son las 2 primeras columnas de explorer_final_Xo:
1
2
Xo = [4.39; 0.435; 0; 1];
explorer_final_Xo(i,1:4) = (A_foot_odom*A_odom_map*Xo)';
Por otro lado, el fichero csv que obtengo me da valores de traslaciones multiplicadas por 10^16, lo cual no comprendo.
Si alguno ha trabajado ya con este robot y sabe qué transformaciones hay que realizar o si alguno ve un error en el código, os agradecería mucho vuestra ayuda.
Un saludo.
Valora esta pregunta
0