Bugfix : repère résultat orienté sur le repère cible

bras_cmd.py

@@ -54,15 +54,19 @@ def commands():
     # # set_speed(0,30,30) # Super rapide
     # # set_speed(0, 1, 1)
     # # set_speed(0, 3, 3) # 3 rad/s
-    # move_q([0,0,0,0,0,0], ot=33)
+    move_q([0,0,0,0,0,0], ot=33)
 
-    traj=[]
 
-    # Approach 
-    add_pose('1', [-141.56, 29.1,  84.13, 0.32, 67.14, -141.69], 33)
-    add_pose('2', [100.41, 38.07, 89.09, 0.37, 53.22, -141.63], 33)
-    add_pose('3', [141.41, 38.07, 89.09, 0.37, 53.22, -141.63], 33)
-    add_pose('4', [-141.41, 38.07, 89.09, 0.37, 53.22, -141.63], 33)
+    p0 = [-0.13877, -0.4113, 0.62946, -2.38, -23.23, 32.21] # <-> q0 = [-10,-70,20,30,40,20]
+    p1 = [0.13877, -0.4113, 0.62946, -2.38, -23.23, 32.21]  # mirroir sur x
+    p2 = [0.0277, -0.4114, 0.6295,  -2.47, -23.18,  32.15] # Point atégnable avec RTB
+    
+    p_cible = p2
+
+    # IK Blender solver with 'near' algorithm and orientation constraint ignored
+    move_q([-10,-70,20,30,40,20])
+    success, traj = move_p(p_cible, nb_pt=10, draw=True, marker='pose', solver='blender', ctrl='near', orientation=False, error_max=0.05, verbose=True, force=False)
+    print (traj)
 
     # while True:
     #     pass

bras_lib.py

@@ -660,7 +660,7 @@ def move_p(p2, ot=None, resol=1000, nb_pt=None, draw=False, marker=None, color=N
     t = None
     speed=0.1 # Vitesse 1m/10s
     qd_max = 90 # Vitesse maxi des moteurs 1,57 rad/s ou 90 deg/s
-    dist = math.sqrt((p2[0]-p1[0])**2+(p2[1]-p1[1])**2+(p2[2]-p1[2])**2) # Distance
+    dist  = math.sqrt((p2[0]-p1[0])**2+(p2[1]-p1[1])**2+(p2[2]-p1[2])**2) # Distance
     angle = math.sqrt((p2[3]-p1[3])**2+(p2[4]-p1[4])**2+(p2[5]-p1[5])**2) # Angle à parcourir (transposition angle -> distance)
     if t is None:
         t = dist/speed
@@ -690,9 +690,10 @@ def move_p(p2, ot=None, resol=1000, nb_pt=None, draw=False, marker=None, color=N
         if error_limit or error_ctrl:
             break
         q_result.append(q)
-        p_result.append(p)
-        q_prev=q.copy()
-        p_prev=p.copy()
+        p_result_i = rtb_fkine(q, unit="deg") # RTB est plus fiable
+        p_result.append(p_result_i)
+        q_prev = q.copy()
+        p_prev = p_result_i.copy()
 
     ##
     # Affichage

twin_ik_blender.py

@@ -176,9 +176,9 @@ def ikb_ikine_slv(p, orientation=True, error_max=0.001):
     sleep(TEMPO_RENDER)
 
     # Armature
-    i=0
-    success=False
-    while success==False:
+    i = 0
+    success = False
+    while success == False:
         if orientation:
             bpy.data.objects["Armature ikb"].pose.bones["Pince ikb"].constraints["IK ikb"].enabled = False
             bpy.data.objects["Armature ikb"].pose.bones["Pince ikb"].constraints["IK-rot ikb"].enabled = True
@@ -202,7 +202,8 @@ def ikb_ikine_slv(p, orientation=True, error_max=0.001):
             success=False
 
         # Trop d'itérations
-        i+=1
+        i += 1
+        # print (i)
         if i == ITERATIONS_IK:
             break