Commit c6a9029c authored by muskula2's avatar muskula2
Browse files

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

parent c2d60892
......@@ -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:
'''
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment