Mój program Pick and Place nie działa

Question

mam nadzieję, że wszyscy macie się dobrze.

Próbuję dokończyć zwieńczenie projektu na Coursera. Prosi mnie o przeniesienie obiektu sześcianu z początkowej pozycji do celu. Ten film pokazuje ruch.

Strona projektu: http://hades.mech.northwestern.edu/index.php/Mobile_Manipulation_Capstone

W oprogramowaniu symulacyjnym polecenia symulacji są wysyłane jako 13-elementowe wektory wierszowe. Informacje wektorowe wyglądają następująco:

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

J1 do J5 to przeguby ramion robota, a W1 do W4 to ruchome kątowniki kół. Stan chwytaka to 0, jeśli chwytak jest otwarty i 1, jeśli chwytak jest zamknięty.

Na stronie projektu mówi mi, żebym utworzył 3 oddzielne funkcje.

Pierwsza funkcja (NextState) przyjmuje 3 dane wejściowe:

1. Dwanaście wektorów zawierających konfigurację robota (bez stanu chwytaka)
2. Wektor 9 zawierający prędkości przegubów i kół (1 do 5 dla prędkości przegubów i 6 do 9 to prędkości kół)
3. Krok czasu t
4. Maksymalna wartość dla przegubów ramion i kół (w razie potrzeby można ustawić bardzo wysoką liczbę)

Daje 1 wejście:

1. 12 wektor zawierający konfigurację robota po kroku czasowym t.

Druga funkcja (TrajectoryGenerator) przyjmuje 6 wejść:

1. Początkowa konfiguracja efektora końcowego, Tse
2. Początkowa konfiguracja kostki, Tsc_initial
3. Żądana konfiguracja kostki, Tsc_final
4. Efektor końcowy związany z konfiguracją kostki podczas chwytania, Tce_grasp
5. Efekt końcowy związany z konfiguracją kostki przed i po chwytaniu, Tce_standoff
6. Konfiguracje odniesienia trajektorii na sekundę, k.

Druga funkcja generuje 1 wyjście:

1 Macierz 13 na m zawierająca konfiguracje robota odniesienia w każdym rzędzie (13-ty rząd zawiera stan chwytaka).

Trzecia funkcja zajmuje 5 wejść:

1. Rzeczywista konfiguracja efektora końcowego, X lub Tse
2. Referencyjna konfiguracja efektora końcowego, Xd lub Tse_d
3. Następna konfiguracja efektora końcowego odniesienia, Xd_next lub Tse_d_next
4. PI zysk Kp i Ki
5. Timestep t
6. Rzeczywista konfiguracja złącza, thetaList

Trzecia funkcja daje 4 wyjścia:

1. Skręt odniesienia sprzężenia do przodu, Vd
2. Skręt efektora końcowego, w ramie efektora końcowego, V.
3. Je do obliczenia wektora sterowania
4. kontroluje wektor, u lub u_theta_dot
5. Xerr, aby sprawdzić błąd odniesienia.

Oto moje implementacje funkcji:

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;

koniec

.....

Przetestowałem moje 3 funkcje w celu sprawdzenia ewentualnych błędów; jednak wyniki funkcji były dokładnie takie, jak oczekiwane na stronie internetowej projektu. Jednak napisałem ostateczny program, aby złożyć wszystko razem, ale wyniki nie są w żaden sposób bliskie rozwiązania. Uważam, że zrobiłem coś złego w programie.

To było moje podejście:

1. Podano ostateczną i początkową konfigurację Cube.

2. Podana jest początkowa konfiguracja odniesienia efektora końcowego.

3. Stworzyłem trajektorię odniesienia z funkcją Trajectory Generator

4. Wybrałem losowo konfigurację robota z 12 wektorami.

5. Obliczyłem 4x4 Tse_actual z tym wektorem 12

6. Jako Xd i Xd_next wybieram k-ty i k + 1

7. Obliczyłem skręt sprzężenia do przodu V i kontrolki z Kp i Ki = 0 z funkcją FeedbackControl

8. Obliczyłem nową konfigurację robota z aktualną rzeczywistą konfiguracją robota (12 wektorów) i sterowaniem z funkcją NextState.

9. Poszedł do kroku 4

10. Pętla tyle razy ile punktów trajektorii.

A to jest mój program do wyszukiwania konfiguracji robota, aby osiągnąć cel:

    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

Wiem, że to dużo do szukania; jednak próbuję przez prawie 2 dni i nigdy nie mogłem rozgryźć mojego błędu. Możesz mi pomóc?

Answer 1

Najlepszym sposobem debugowania aplikacji związanych z geometrią jest użycie wykresów.

Przede wszystkim wykreśl początkową konfigurację, z robotem TCP, początkową pozycją kostki, końcową pozycją kostki. Ponieważ są to wszystkie wartości wejściowe, wszystkie powinny być w porządku.

Następnie wykreśl wygenerowaną trajektorię. Czy to jest to, czego się spodziewasz? czy dokładnie łączy punkty początkowe i końcowe? Możesz użyć małych strzałek (wykres kołczanowy), aby dodać orientacje do każdego punktu, aby upewnić się, że są one zgodne z oczekiwaniami. Wizualnie łatwo jest ocenić, czy coś pójdzie nie tak.

Następnie możesz lepiej zidentyfikować, gdzie jest problem, przed wygenerowaniem trajektorii, w trakcie lub po. Jest to celem debugowania (rodzaj meta divide et impera), aby powoli zbliżyć się do problematycznej części, dzieląc i dzieląc kod oraz sprawdzając, czy działa poprawnie.

Dla każdego kroku czasowego po wygenerowaniu trajektorii możesz wykreślić samego robota. Możesz sprawdzić, czy porusza się po trajektorii, czy nie, lub gdzie schodzi z toru.