[IK] Korrektur zu 4f31000 (Bedingung für Berechnung des Positionsfehl…

…ers)
SchapplM · Sep 11, 2023 · ac157e2 · ac157e2
1 parent 2357c25
commit ac157e2
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 7 deletions.
diff --git a/kinematics/pkm_invkin_traj.m.template b/kinematics/pkm_invkin_traj.m.template
@@ -605,7 +605,7 @@ vmax_rel_lastvel = 1; % Skalierung der Geschwindigkeitsgrenzen
 dt = 0; % Initialisierung, wird überschrieben.
 Stats = struct('file', 'pkm_invkin_traj', 'iter', 0, 'h', NaN(nt,1+idx_ik_length.hntraj), ...
   'h_instspc_thresh', NaN, 'condJ', NaN(nt,2), 'h_coll_thresh', NaN, ...
-  'mode', uint32(zeros(nt,1)), 'errorcode', 0, 'version', 10);
+  'mode', uint32(zeros(nt,1)), 'errorcode', 0, 'version', 11);
 h = zeros(idx_ik_length.hntraj,1);
 
 for k = 1:nt
@@ -895,7 +895,8 @@ for k = 1:nt
     % Benutze nur für die Jacobi-Matrix die red. Koordinaten (2T1R, 3T0R)
     qD_test = J_x_inv * xD_test(Rob_I_EE); % Gelenkänderung der Nullraumbewegung
     % Berechne die Jacobi-Matrix (aufwändig wegen Invertierung)
-    if condJ < thresh_ns_qa && sum(I_qa) == sum(Rob_I_EE) || wn(idx_wnP.poserr_ee) ~= 0
+    if condJ < thresh_ns_qa && sum(I_qa) == sum(Rob_I_EE) || ...
+        wn(idx_wnP.poserr_ee) ~= 0 || ~isnan(s.abort_thresh_h(idx_hn.poserr_ee)) % Jacobi-Matrix für Berechnung des Positionsfehlers benötigt
       J_ax = inv(Jinv_ax);
     end
     % Führe Nullraumbewegung in Antriebskoordinaten durch. Geht nur, wenn
@@ -1334,7 +1335,7 @@ for k = 1:nt
         Stats.mode(k) = bitset(Stats.mode(k),9);
         h(idx_hn.jac_cond) = invkin_optimcrit_condsplineact(condJ, ...
               1.5*s.cond_thresh_jac, s.cond_thresh_jac);
-        if wn(idx_wnP.poserr_ee) ~= 0
+        if wn(idx_wnP.poserr_ee) ~= 0 || ~isnan(s.abort_thresh_h(idx_hn.poserr_ee))
           dx_poserr = abs(J_ax) * s.q_poserr(I_qa);
           h(idx_hn.poserr_ee) = norm(dx_poserr(Rob_I_EE(1:3)));
         end

diff --git a/kinematics/robot_invkin_traj.m.template b/kinematics/robot_invkin_traj.m.template
@@ -386,7 +386,7 @@ dt = 0; % Initialisierung, wird überschrieben.
 J_x_alt = zeros(6,NQJ);
 Stats = struct('file', 'robot_invkin_traj', 'iter', 0, 'h', NaN(nt,1+idx_ik_length.hntraj), ...
   'h_instspc_thresh', NaN, 'condJ', NaN(nt,2), 'h_coll_thresh', NaN, ...
-  'errorcode', 0, 'version', 9);
+  'errorcode', 0, 'version', 10);
 h = zeros(idx_ik_length.hntraj,1);
 %% Iterative Berechnung der gesamten Trajektorie
 for k = 1:nt
@@ -571,7 +571,7 @@ for k = 1:nt
     %% Singularitätsvermeidung IK-Jacobi-Matrix (Aufgaben-FG) und geom. Jacobi-Matrix (Roboter-FG)
     if (wn(idx_wnP.ikjac_cond) ~= 0 || wn(idx_wnD.ikjac_cond) ~= 0) && condJik > s.cond_thresh_ikjac || ... % IK-Jacobi-Matrix (Aufgaben-FG)
        (wn(idx_wnP.jac_cond) ~= 0 || wn(idx_wnD.jac_cond) ~= 0) && condJ > s.cond_thresh_jac || ...% geom. Jacobi-Matrix (Roboter-FG)
-        wn(idx_wnP.poserr_ee) ~= 0 % Positionsfehler ist ähnlich
+        wn(idx_wnP.poserr_ee) ~= 0 || ~isnan(s.abort_thresh_h(idx_hn.poserr_ee)) % Positionsfehler ist ähnlich
       h(idx_hn.ikjac_cond) = invkin_optimcrit_condsplineact(condJik, ... %  Konditionszahl
           1.5*s.cond_thresh_jac, s.cond_thresh_ikjac); % mit Spline-Aktivierung
       h(idx_hn.jac_cond) = invkin_optimcrit_condsplineact(condJ, ... %  Konditionszahl

diff --git a/matlab_class/@ParRob/invkin_traj.m b/matlab_class/@ParRob/invkin_traj.m
@@ -696,7 +696,8 @@
     % Benutze nur für die Jacobi-Matrix die red. Koordinaten (2T1R, 3T0R)
     qD_test = J_x_inv * xD_test(Rob_I_EE); % Gelenkänderung der Nullraumbewegung
     % Berechne die Jacobi-Matrix (aufwändig wegen Invertierung)
-    if condJ < thresh_ns_qa && sum(I_qa) == sum(Rob_I_EE) || wn(idx_wnP.poserr_ee) ~= 0
+    if condJ < thresh_ns_qa && sum(I_qa) == sum(Rob_I_EE) || ...
+        wn(idx_wnP.poserr_ee) ~= 0 || ~isnan(s.abort_thresh_h(idx_hn.poserr_ee)) % Jacobi-Matrix für Berechnung des Positionsfehlers benötigt
       J_ax = inv(Jinv_ax);
     end
     % Führe Nullraumbewegung in Antriebskoordinaten durch. Geht nur, wenn
@@ -1135,7 +1136,7 @@
         Stats.mode(k) = bitset(Stats.mode(k),9);
         h(idx_hn.jac_cond) = invkin_optimcrit_condsplineact(condJ, ...
               1.5*s.cond_thresh_jac, s.cond_thresh_jac);
-        if wn(idx_wnP.poserr_ee) ~= 0
+        if wn(idx_wnP.poserr_ee) ~= 0 || ~isnan(s.abort_thresh_h(idx_hn.poserr_ee))
           dx_poserr = abs(J_ax) * s.q_poserr(I_qa);
           h(idx_hn.poserr_ee) = norm(dx_poserr(Rob_I_EE(1:3)));
         end

diff --git a/matlab_class/@RobBase/dynprog_taskred_ik.m b/matlab_class/@RobBase/dynprog_taskred_ik.m
@@ -1030,6 +1030,10 @@
         end
         if isinf(s_Traj_l.abort_thresh_h(R.idx_iktraj_hn.poserr_ee)) && ... % s.debug && 
             any(Stats.h(1:Stats.iter,1+R.idx_iktraj_hn.poserr_ee)==0) % Zum Debuggen bei Unstimmigkeiten
+          if ~isempty(s.debug_dir)
+            save(fullfile(s.debug_dir, sprintf(['dp_stage%d_state%d_', ...
+              'to%d_error_positionerrorzero.mat'], i-1, k, l)));
+          end
           error('Positionsfehler soll berechnet werden, ist aber Null. Darf nicht sein.')
         end
       end