Mi programa Pick and Place no funciona

Question

Espero que todos estén bien.

Estoy tratando de terminar un proyecto final en Coursera. Me pide que lleve un objeto cubo desde una posición inicial hasta la meta. Este video muestra el movimiento.

Sitio web del proyecto: http://hades.mech.northwestern.edu/index.php/Mobile_Manipulation_Capstone

En el software de simulación, los comandos de simulación se envían como vectores de fila de 13 elementos. La información del vector se ve así:

chassis phi, chassis x, chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4, gripper state

J1 a J5 son articulaciones de brazos robóticos y W1 a W4 son ángulos de rueda móviles. El estado de la pinza es 0 si la pinza está abierta y 1 si está cerrada.

En la página del proyecto, me dice que cree 3 funciones separadas.

La primera función (NextState) toma 3 entradas:

1. Una configuración de robot que contiene doce vectores (sin estado de pinza)
2. Un vector 9 que contiene las velocidades de las ruedas y las articulaciones (1 a 5 para las velocidades de las juntas y 6 a 9 son las velocidades de las ruedas)
3. Un paso de tiempo t
4. Un valor máximo para las articulaciones del brazo y las ruedas (se puede establecer como un número muy alto si se desea)

Produce 1 entrada:

1. 12 vector que contiene la configuración del robot después del paso de tiempo t.

La segunda función (TrajectoryGenerator) toma 6 entradas:

1. Configuración inicial del efector final, Tse
2. Configuración inicial del cubo, Tsc_initial
3. Configuración de cubo deseada, Tsc_final
4. Efector final relacionado con la configuración del cubo mientras se agarra, Tce_grasp
5. Efector final relacionado con la configuración del cubo antes y después de agarrar, Tce_standoff
6. Configuraciones de referencia de trayectoria por segundo, k.

La segunda función produce 1 salida:

1 matriz de 13 por m que contiene configuraciones de robot de referencia en cada fila (la 13ª fila contiene el estado de la pinza).

La tercera función toma 5 entradas:

1. Configuración del efector final real, X o Tse
2. Configuración del efector final de referencia, Xd o Tse_d
3. Siguiente configuración del efector final de referencia, Xd_next o Tse_d_next
4. PI ganancia Kp y Ki
5. Paso de tiempo t
6. Configuración conjunta real, thetaList

La tercera función produce 4 salidas:

1. Giro de referencia de avance, Vd
2. Giro del efector final, en el marco del efector final, V.
3. Je para calcular el vector de controles
4. controla el vector, u o u_theta_dot
5. Xerr para inspeccionar el error de referencia.

Aquí están mis implementaciones de funciones:

function newConfiguration = NextState(currentConfiguration, controls, delta_t, limit)

%% Assigning the properties of the robot:
l = 0.47/2;
w = 0.30/2;
r = 0.0475;

%% Assigning current and new configurations and speeds:
currentJoints = currentConfiguration(4:8)';
currentWheels = currentConfiguration(9:12)';
armJointSpeeds = controls(1:5)';
wheelSpeeds = controls(6:9)';
newJoints = currentJoints + (armJointSpeeds .* delta_t);
newWheels = currentWheels + (wheelSpeeds .* delta_t);
Tsbk = [cos(currentConfiguration(1)), -sin(currentConfiguration(1)), 0, currentConfiguration(2); sin(currentConfiguration(1)), cos(currentConfiguration(1)), 0, currentConfiguration(3); 0, 0, 1, 0.0963; 0, 0, 0, 1];

%% Calculating deltaTheta to initiate the odometry process:
deltaTheta = newWheels - currentWheels;
thetaDot = deltaTheta/delta_t;

Vb = (r/4) * [-1 / (l + w), 1 / (l + w), 1 / (l + w), -1 / (l + w); 1, 1, 1, 1; -1, 1, -1, 1] * thetaDot;
Vb6 = [0; 0; Vb; 0];
newChassisRelative = expm(VecTose3(Vb6));
Tsbk1 = Tsbk * newChassisRelative;

phi = acos(Tsbk1(1,1));
x = Tsbk1(1, 4);
y = Tsbk1(2, 4);

chass = [phi x y];

newConfiguration = [chass'; newJoints; newWheels]';

end

................

function movement = TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_final, Tce_grasp, Tce_standoff, k)

%% Sequence:
%  1. Arm moves above the cube.
%  2. Arm reaches the cube.
%  3. Arm grasps the cube.
%  4. Arm returns to the position 1
%  5. Arm goes to final position.

Tse_standoff = Tsc_initial * Tce_standoff;

Tse_grasp = Tsc_initial * Tce_grasp;

Tse_final = Tsc_final * [cosd(135), 0, sind(135) 0; 0, 1, 0  0; -sind(135), 0, cosd(135), 0; 0 0 0 1];

% Standing on top of the cube tracejtory:
traj1 = CartesianTrajectory(Tse_initial, Tse_standoff, 8, (8*k) / 0.01, 5)

traj2 = CartesianTrajectory(Tse_standoff, Tse_grasp, 8, (8*k) / 0.01, 5);

traj3 = CartesianTrajectory(Tse_grasp, Tse_standoff, 8, (8*k) / 0.01, 5);

traj4 = CartesianTrajectory(Tse_standoff, Tse_final, 8, (8*k) / 0.01, 5);


for i = 1 : length(traj1)
    trajectory1(i, :) = [traj1{i}(1,1) traj1{i}(1,2) traj1{i}(1,3) traj1{i}(2,1) traj1{i}(2,2) traj1{i}(2,3) traj1{i}(3,1) traj1{i}(3,2) traj1{i}(3,3) traj1{i}(1, 4) traj1{i}(2, 4) traj1{i}(3, 4) 0];
end

for i = 1 : length(traj2)
    trajectory2(i, :) = [traj2{i}(1,1) traj2{i}(1,2) traj2{i}(1,3) traj2{i}(2,1) traj2{i}(2,2) traj2{i}(2,3) traj2{i}(3,1) traj2{i}(3,2) traj2{i}(3,3) traj2{i}(1, 4) traj2{i}(2, 4) traj2{i}(3, 4) 0];
end

index = size(trajectory2);

for i = 1 : 15
    trajectory3(i, :) = [traj3{i}(1,1) traj3{i}(1,2) traj3{i}(1,3) traj3{i}(2,1) traj3{i}(2,2) traj3{i}(2,3) traj3{i}(3,1) traj3{i}(3,2) traj3{i}(3,3) traj3{i}(1, 4) traj3{i}(2, 4) traj3{i}(3, 4) 1];
end 

for i = 16 : length(traj3)
    trajectory3(i, :) = [traj3{i}(1,1) traj3{i}(1,2) traj3{i}(1,3) traj3{i}(2,1) traj3{i}(2,2) traj3{i}(2,3) traj3{i}(3,1) traj3{i}(3,2) traj3{i}(3,3) traj3{i}(1, 4) traj3{i}(2, 4) traj3{i}(3, 4) 1];
end 

trajectory2(:, 13) = 0; 
trajectory2(1:35, 13) = 0;

for i = 1 : length(traj4)
    trajectory4(i, :) = [traj4{i}(1,1) traj4{i}(1,2) traj4{i}(1,3) traj4{i}(2,1) traj4{i}(2,2) traj4{i}(2,3) traj4{i}(3,1) traj4{i}(3,2) traj4{i}(3,3) traj4{i}(1, 4) traj4{i}(2, 4) traj4{i}(3, 4) 1];
end
index = size(trajectory4);
trajectory4(index(1)-20:index(1), 13) = 0; 

%% traj2_5 is used for the arm to wait for few seconds before grasping.
for i = 1 : 10
traj2_5{i} = traj2{length(traj2)};
end

%% traj4_5 is used for the arm to wait for few seconds before releasing the cube.
for i = 1 : 50
traj4_5{i} = traj4{length(traj4)};
end

for i = 1 : length(traj4_5)
trajectory4_5(i, :) = [traj4_5{i}(1,1) traj4_5{i}(1,2) traj4_5{i}(1,3) traj4_5{i}(2,1) traj4_5{i}(2,2) traj4_5{i}(2,3) traj4_5{i}(3,1) traj4_5{i}(3,2) traj4_5{i}(3,3) traj4_5{i}(1, 4) traj4_5{i}(2, 4) traj4_5{i}(3, 4) 0];
end

for i = 1 : length(traj2_5)
trajectory2_5(i, :) = [traj2_5{i}(1,1) traj2_5{i}(1,2) traj2_5{i}(1,3) traj2_5{i}(2,1) traj2_5{i}(2,2) traj2_5{i}(2,3) traj2_5{i}(3,1) traj2_5{i}(3,2) traj2_5{i}(3,3) traj2_5{i}(1, 4) traj2_5{i}(2, 4) traj2_5{i}(3, 4) 1];
end

movement = [trajectory1; trajectory2; trajectory2_5; trajectory3; trajectory4; trajectory4_5];  

.........

function [Vd, V, Je, u_theta_dot, Xerr] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList)

%% Arm properties.
Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
l = 0.47/2;
w = 0.30/2;
r = 0.0475;
F = (r/4) * [-1/(l + w), 1/(l + w), 1/(l + w), -1/(l + w); 1 1 1 1; -1 1 -1 1];
sizee = size(F);
m = sizee(2);
zeross = zeros(1, m);
F6 = [zeross; zeross; F; zeross];

Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M0e = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
T0e = FKinBody(M0e, Blist, thetaList);
Tbe = Tb0 * T0e;
Teb = inv(Tbe);
Jbase = Adjoint(Teb) * F6;
Jarm = JacobianBody(Blist, thetaList);
Je = [Jbase, Jarm];

psInv = pinv(Je, 1e-02);

Xerr_bracket = MatrixLog6(inv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);
Vd_bracket = (1/delta_t) * MatrixLog6(inv(Xd) * Xd_next);
Vd = se3ToVec(Vd_bracket);

V = (Adjoint(inv(X) * Xd) * Vd) + (Kp * Xerr) + (Ki*(Xerr + Xerr * delta_t));

u_theta_dot = psInv * V;

fin

.....

Probé mis 3 funciones para verificar posibles errores; sin embargo, los resultados de las funciones fueron exactamente iguales a los esperados en el sitio web del proyecto. Sin embargo, escribí un programa final para armar todo, pero los resultados no se acercan a la solución. Creo que hice algo mal en el programa.

Este fue mi enfoque:

1. Se dan las configuraciones inicial y final del cubo.

2. Se proporciona la configuración de referencia inicial del efector final.

3. Creé una trayectoria de referencia con la función Generador de trayectoria

4. Elegí una configuración de robot de 12 vectores al azar.

5. Calculé el 4x4 Tse_actual con ese vector 12

6. Elijo los pasos kth y k + 1th como Xd y Xd_next

7. Calculé el giro de alimentación hacia adelante V y los controles con Kp y Ki = 0 con la función FeedbackControl

8. Calculé la nueva configuración del robot con la configuración actual del robot (12 vectores) y los controles con la función NextState.

9. Fue al paso 4

10. Rodee tantas veces el número de puntos de trayectoria.

Y este es mi programa para encontrar las configuraciones del robot para lograr el objetivo:

    clc
clear

Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M0e = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];

l = 0.47/2;
w = 0.30/2;
r = 0.0475;
F = (r/4) * [-1/(l + w), 1/(l + w), 1/(l + w), -1/(l + w); 1 1 1 1; -1 1 -1 1];

sizee = size(F);
m = sizee(2);
zeross = zeros(1, m);
F6 = [zeross; zeross; F; zeross];

Kp = 0.005;
Ki = 0.005 * 0.1;
delta_t = 0.1;

%%
Tse_initial = [0 0 1 0; 0 1 0 0; -1 0 0 0.25; 0 0 0 1];
Tsc_initial = [1 0 0 1; 0 1 0 0; 0 0 1 0.025; 0 0 0 1];
Tsc_final = [0 1 0 0; -1 0 0 -1; 0 0 1 0.025; 0 0 0 1];
Tce_standoff = [1 0 0 0; 0 1 0 0; 0 0 1 0.1; 0 0 0 1] * [cosd(135), 0, sind(135) 0; 0, 1, 0  0; -sind(135), 0, cosd(135), 0; 0 0 0 1];
Tce_grasp = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1] * [cosd(135), 0, sind(135) 0; 0, 1, 0  0; -sind(135), 0, cosd(135), 0; 0 0 0 1];

movement = TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_final, Tce_grasp, Tce_standoff, 1);

conf1(1, :) = [0.5, -0.9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
o = conf1(1);
x = conf1(2);
y = conf1(3);
z = 0.0963;
Tsb = [cos(o), -sin(o), 0 x; sin(o), cos(o), 0, y; 0, 0, 1, z; 0, 0, 0, 1];
Tb0 = [1, 0, 0, 0.1662; 0, 1, 0, 0; 0, 0, 1, 0.0026; 0, 0, 0, 1];
thetalist = conf1(4 : 8);
T0e = FKinBody(M0e, Blist, thetalist);
%%
X = Tsb * Tb0 * T0e;
%%
refconf1 = movement(1,:);
refconf2 = movement(1,:);

oconf1 = refconf1(1);
xconf1 = refconf1(2);
yconf1 = refconf1(3);

oconf2 = refconf2(1);
xconf2 = refconf2(2);
yconf2 = refconf2(3);

Tsb1 = [cos(oconf1), -sin(oconf1), 0 xconf1; sin(oconf1), cos(oconf1), 0, yconf1; 0, 0, 1, z; 0, 0, 0, 1];
thetalist1 = refconf1(4 : 8);
T0e1 = FKinBody(M0e, Blist, thetalist1);
Xd1 = Tsb1 * Tb0 * T0e1;

Tsb2 = [cos(oconf2), -sin(oconf2), 0 xconf2; sin(oconf2), cos(oconf2), 0, yconf2; 0, 0, 1, z; 0, 0, 0, 1];
thetalist2 = refconf2(4 : 8);
T0e2 = FKinBody(M0e, Blist, thetalist2);
Xd2 = Tsb2 * Tb0 * T0e2;
[Vd, V, Je, controls, Xerr] = FeedbackControl(X, Xd1, Xd2, Kp, Ki, delta_t, thetalist);

%%
for i = 2 : length(movement) -1
conf1(i, :) = NextState(conf1(i-1, :), controls', delta_t, 5000);
o = conf1(1);
x = conf1(2);
y = conf1(3);
z = 0.0963;
Tsb = [cos(o), -sin(o), 0 x; sin(o), cos(o), 0, y; 0, 0, 1, z; 0, 0, 0, 1];
Tb0 = [1, 0, 0, 0.1662; 0, 1, 0, 0; 0, 0, 1, 0.0026; 0, 0, 0, 1];
thetalist = conf1(4 : 8);
T0e = FKinBody(M0e, Blist, thetalist);
X = Tsb * Tb0 * T0e;
%
refconf1 = movement(i,:);
refconf2 = movement(i+1,:);

oconf1 = refconf1(1);
xconf1 = refconf1(2);
yconf1 = refconf1(3);

oconf2 = refconf2(1);
xconf2 = refconf2(2);
yconf2 = refconf2(3);

Tsb1 = [cos(oconf1), -sin(oconf1), 0 xconf1; sin(oconf1), cos(oconf1), 0, yconf1; 0, 0, 1, z; 0, 0, 0, 1];
thetalist1 = refconf1(4 : 8);
T0e1 = FKinBody(M0e, Blist, thetalist1);
Xd1 = Tsb1 * Tb0 * T0e1;

Tsb2 = [cos(oconf2), -sin(oconf2), 0 xconf2; sin(oconf2), cos(oconf2), 0, yconf2; 0, 0, 1, z; 0, 0, 0, 1];
thetalist2 = refconf2(4 : 8);
T0e2 = FKinBody(M0e, Blist, thetalist2);
Xd2 = Tsb2 * Tb0 * T0e2;
[Vd, V, Je, controls, Xerr] = FeedbackControl(X, Xd1, Xd2, Kp, Ki, delta_t, thetalist);
%
end
sizee = size(conf1);

for i = 1 : sizee(1)
    conf1(i, 13) = movement(i, 13);
end

Sé que esto es mucho para buscar; Sin embargo, lo intento durante casi 2 días y nunca pude entender mi error. ¿Me puedes ayudar?

Answer 1

La mejor forma de depurar aplicaciones relacionadas con la geometría es mediante gráficos.

En primer lugar, trace la configuración inicial, con el TCP del robot, la posición inicial del cubo, la posición final del cubo. Como estos son todos los valores de entrada, todos deberían estar bien.

Después de esto, traza la trayectoria generada. ¿Es esto lo que esperabas que fuera? ¿Vincula exactamente los puntos de inicio y fin? Puede usar flechas pequeñas (diagrama de carcaj) para agregar orientaciones a cada punto para asegurarse de que también sean las esperadas. Visualmente es fácil juzgar si algo sale mal.

Después de esto, puede identificar mejor dónde está el problema, antes de la generación de la trayectoria, durante o después. Este es el objetivo de la depuración (una especie de meta divide et impera) para acercarse lentamente a la parte problemática dividiendo y subdividiendo el código y verificando si funciona bien.

Para cada paso de tiempo después de la generación de la trayectoria, puede trazar el robot en sí. Puedes comprobar si sigue la trayectoria o no, o dónde se desvía.