using our algorithm for IK in motion planning, instead of the one from the cbtf library

FinalProject/Check_Point_5/check_point5.py

@@ -284,8 +284,39 @@ while i < 3:
     thetas_for_ik = np.array([[theta1], [theta2], [theta3], [theta4], [theta5], [theta6]])
     S_mat = [S1, S2, S3, S4, S5, S6]
 
-    thetas_from_ik = findIK(goal_pose, S, M, thetas_for_ik)
-    thetas_from_ik = thetas_from_ik[0]
+    # thetas_from_ik = findIK(goal_pose, S, M, thetas_for_ik)
+    k = 1
+
+    # T_1in0 = getT_1in0(M, S, thetas_for_ik)
+    T_1in0 = evalT(S, thetas_for_ik, M)
+    screwMat = logm(np.dot(goal_pose, inv(T_1in0)))
+    twist = inv_bracket(screwMat)
+
+    counter = 0
+    final_pose_flag = 1
+
+    while (np.linalg.norm(twist) > 0.01):
+
+        if(counter >= 100):
+            print("Desired Pose not reachable")
+            final_pose_flag = 0
+            break
+
+        J = evalJ(S, thetas_for_ik)
+        thetas_for_ikdot = np.dot(inv(np.dot(np.transpose(J), J) + 0.1*np.identity(6)),
+        np.dot(np.transpose(J), twist)) - np.dot(np.identity(6) - np.dot(np.linalg.pinv(J), J), thetas_for_ik)
+        thetas_for_ik = thetas_for_ik + (thetas_for_ikdot * k)
+
+        T_1in0 = evalT(S, thetas_for_ik, M)
+        screwMat = logm(np.dot(goal_pose, inv(T_1in0)))
+        twist = inv_bracket(screwMat)
+
+    if(final_pose_flag == 0):
+        continue
+
+
+    # thetas_from_ik = thetas_from_ik[0]
+    thetas_from_ik = thetas_for_ik    
 
     theta_start = np.array([[initial_thetas[0]], [initial_thetas[1]], [initial_thetas[2]], [initial_thetas[3]], [initial_thetas[4]], [initial_thetas[5]]])
     theta_goal = np.array([[thetas_from_ik[0][0]], [thetas_from_ik[1][0]], [thetas_from_ik[2][0]], [thetas_from_ik[3][0]], [thetas_from_ik[4][0]], [thetas_from_ik[5][0]]])
@@ -310,53 +341,6 @@ while i < 3:
 
     counter = 0
     bound = 10
-    #
-    # if(point2point_collision_detector(theta_start, theta_goal, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
-    #     q = [theta_start, theta_goal]
-    # else:
-    #     while counter < bound:
-    #         '''
-    #         Joint Bounds from Lab 3 Doc in degrees:
-    #         theta1 = (60,315)
-    #         theta2 = (-185,5)
-    #         theta3 = (-10,150)
-    #         theta4 = (-190,-80)
-    #         theta5 = (-120,80)
-    #         theta6 = (-180,180)
-    #         '''
-    #         rand1 = random.uniform(deg2rad(60),deg2rad(315))
-    #         rand2 = random.uniform(deg2rad(-185),deg2rad(5))
-    #         rand3 = random.uniform(deg2rad(-10),deg2rad(150))
-    #         rand4 = random.uniform(deg2rad(-190),deg2rad(-80))
-    #         rand5 = random.uniform(deg2rad(-120),deg2rad(80))
-    #         rand6 = random.uniform(deg2rad(-180),deg2rad(180))
-    #
-    #         theta_target = np.array([[rand1], [rand2], [rand3], [rand4], [rand5], [rand6]])
-    #
-    #         theta_a = start_curr.theta
-    #         theta_b = theta_target
-    #
-    #         if(point2point_collision_detector(theta_a, theta_b, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
-    #             start_curr.next = Node()
-    #             start_curr = start_curr.next
-    #             start_curr.theta = theta_target
-    #
-    #         theta_a = end_root.theta
-    #         theta_b = theta_target
-    #
-    #         if(point2point_collision_detector(theta_a, theta_b, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
-    #             temp = Node()
-    #             temp.next = end_root
-    #             temp.theta = theta_target
-    #             end_root = temp
-    #
-    #         q = get_complete_path(start_root, end_root)
-    #
-    #         if(q is not None):
-    #             break
-    #
-    #         counter += 1
-
 
     while counter < bound:
         '''