Fixed checkpoint 2, no longer hardcoded

FinalProject/Check_Point_2/check_point2.py

@@ -3,53 +3,13 @@ import time
 import numpy as np
 from numpy.linalg import multi_dot
 from scipy.linalg import expm
-from usefulFunctions import deg2rad, rad2deg, revolute, bracket_skew_4
-from usefulFunctions import skew, euler2mat, euler_to_a,euler_to_axis, exp_s_the
-from usefulFunctions import screwBracForm, rot2euler, quaternion_from_matrix, deg2rad
+from usefulFunctions import *
 import transforms3d
 
 '''
 https://matthew-brett.github.io/transforms3d/reference/transforms3d.euler.html#transforms3d.euler.euler2mat
 pip install transforms3d
 '''
-#input for user for goal pose
-
-# def main():
-# 	x_pos = float(input("Enter X translation position"))
-# 	y_pos = float(input("Enter Y translation position"))
-# 	z_pos = float(input("Enter Z translation position"))
-# 	a = int(input("Enter a rorational angle in degrees"))
-# 	b = int(input("Enter b rorational angle in degrees"))
-# 	c = int(input("Enter c rorational angle in degrees"))
-#
-# 	Goal_pose = RotationMatrixToPose(x, y, z, a, b, c)
-#
-#     return Goal_pose
-#
-#
-# def  RotationMatrixToPose(x, y, z, a, b, c):
-# 	Goal_pose = np.zeros((4,4))
-# 	Goal_pose[0,3] = x
-# 	Goal_pose[1,3] = y
-# 	Goal_pose[2,3] = z
-# 	Goal_pose[3,3] = 1
-#
-# 	Rot_x = np.array([[1, 0, 0],
-# 					  [0, math.cos(deg2rad(a)), -1*math.sin(deg2rad(a))],
-# 					  [0, math.sin(deg2rad(a)), math.cos(deg2rad(a))]])
-#
-# 	Rot_y = np.array([[math.cos(deg2rad(b)), 0, math.sin(deg2rad(b))],
-# 					  [0, 1, 0],
-# 					  [-1*math.sin(deg2rad(b)), 0, math.cos(deg2rad(b))]])
-#
-# 	Rot_z = np.array([[math.cos(deg2rad(c)), -1*math.sin(deg2rad(c)), 0],
-# 					  [math.sin(deg2rad(c)), math.cos(deg2rad(c)), 0],
-# 					  [0, 0, 1]])
-#
-#
-# 	R = multi_dot(Rot_x, Rot_y, Rot_z)
-# 	Goal_pose[0:3,0:3] = R
-# 	return Goal_pose
 
 # Close all open connections (just in case)
 vrep.simxFinish(-1)
@@ -59,16 +19,14 @@ clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
 if clientID == -1:
     raise Exception('Failed connecting to remote API server')
 
-
-
 # Make a dummy
 result, dummy_handle_0 = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
 
-# Joint:          1  2  3  4  5  6  7
+# Joint:                 1   2    3     4   5   6    7
 theta_list = np.array([0.2, 0.1, 0.5, 0.7, 0.9, 0.1])
 #theta_list = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
 
-# Get handles
+# Get joint object handles
 result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking)
 if result != vrep.simx_return_ok:
     raise Exception('could not get object handle for first joint')
@@ -93,10 +51,14 @@ result, joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6', vrep
 if result != vrep.simx_return_ok:
     raise Exception('could not get object handle for sixth joint')
 
-#Get handel for UR3
+#Get handle for UR3
 result, UR3_handle = vrep.simxGetObjectHandle(clientID, 'UR3', vrep.simx_opmode_blocking)
 if result != vrep.simx_return_ok:
     raise Exception('could not get object handle for UR3')
+#Get handle for UR3_connection
+result, UR3_connection = vrep.simxGetObjectHandle(clientID, 'UR3_connection', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object handle for UR3UR3_connection')
 
 # Start simulation
 vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
@@ -105,128 +67,71 @@ vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
 vrep.simxSetObjectPosition(clientID, dummy_handle_0, joint_six_handle, (0,0,0), vrep.simx_opmode_oneshot_wait)
 vrep.simxSetObjectOrientation(clientID, dummy_handle_0, joint_six_handle, (0,0,0), vrep.simx_opmode_oneshot_wait)
 
-result, pos = vrep.simxGetObjectPosition(clientID, joint_six_handle, -1, vrep.simx_opmode_oneshot_wait)
-print("Initial end pos:", pos)
-
-
-# Forward Kinematics Implementation
-# Set 6 joint angles of UR3
-# joint_1_angle = np.pi/6
-# joint_2_angle = np.pi/6
-# joint_3_angle = np.pi/6
-# joint_4_angle = np.pi/6
-# joint_5_angle = np.pi/6
-# joint_6_angle = np.pi/6
-# joint_angles = np.array([joint_1_angle,joint_2_angle,joint_3_angle,joint_4_angle,joint_5_angle,joint_6_angle])
-
-
 # Get the orientation from base to  world frame
-result, orientation = vrep.simxGetObjectOrientation(clientID, joint_six_handle, UR3_handle, vrep.simx_opmode_blocking)
+result, orientation = vrep.simxGetObjectOrientation(clientID, UR3_connection, UR3_handle, vrep.simx_opmode_blocking)
 if result != vrep.simx_return_ok:
     raise Exception('could not get object orientation angles for UR3')
-print ("orientation", orientation)
+
+
 # Get the position from base to world frame
-result, p = vrep.simxGetObjectPosition(clientID, joint_six_handle, UR3_handle, vrep.simx_opmode_blocking)
+result, p = vrep.simxGetObjectPosition(clientID, UR3_connection, UR3_handle, vrep.simx_opmode_blocking)
 if result != vrep.simx_return_ok:
     raise Exception('could not get object current position for UR3')
 P_initial = np.reshape(p,(3,1))
 print ("P_initial", P_initial)
-# Return matrix for rotations around z, y and x axes
 R_initial = transforms3d.euler.euler2mat(orientation[0], orientation[1], orientation[2])
 print ("R_itinial", R_initial)
 
-# M = np.array([
-# [R_initial[0][0], R_initial[0][1], R_initial[0][2], P_initial[0][0]],
-# [R_initial[1][0], R_initial[1][1], R_initial[1][2], P_initial[1][0]],
-# [R_initial[2][0], R_initial[2][1], R_initial[2][2], P_initial[2][0]],
-# [0,0,0,1]
-# ])
-# M = np.array([
-# [0.0, 0.0, -1.0, P_initial[0][0]],
-# [0.0, 1.0, 0.0, P_initial[1][0]],
-# [1.0, 0.0, 0.0, P_initial[2][0]],
-# [0, 0, 0, 1]
-# ])
-M = np.array([[0,0,-1, -0.1940], [0,1,0,0], [1,0,0,0.6511], [0,0,0,1]])
-
+M = np.block([
+[R_initial[0,:], P_initial[0,:]],
+[R_initial[1,:], P_initial[1,:]],
+[R_initial[2,:], P_initial[2,:]],
+[0,0,0,1] ])
 print ("M", M, "\n")
 
-# result, orientation = vrep.simxGetObjectOrientation(clientID, joint_one_handle, UR3_handle, vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get object orientation angles for UR3')
-# print ("orientation", orientation)
-# a1 = transforms3d.euler.euler2mat(orientation[0], orientation[1], orientation[2])
-# #a1 = np.reshape(a1,(3,1))
-# print("a1: ", a1, "\n")
-
-#Set up scew axis with repesct to base frame
-# a1 = np.array([[0], [0], [1]])
-# result, q1 = vrep.simxGetObjectPosition(clientID,joint_one_handle, UR3_handle,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get object current position for UR3')
-# q1 = np.reshape(q1,(3,1))
-# # print("q1: ",q1)
+# Set up scew axis with respect to base frame
+result, q1 = vrep.simxGetObjectPosition(clientID,joint_one_handle, UR3_handle,vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object current position for UR3')
+q1 = np.reshape(q1,(3,1))
 a1 = np.array([[0],[0],[1]])
-q1 = np.array([[0], [0], [0.1045]])
 S1 = revolute(a1, q1)
-print("S1: ", S1, "\n")
-
-# a2 = np.array([[-1], [0], [0]])
-# # a2 = transforms3d.euler.euler2mat(a2_arr)
-# result, q2 = vrep.simxGetObjectPosition(clientID,joint_two_handle, UR3_handle,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get object current position for UR3')
-# q2 = np.reshape(q2,(3,1))
+
+result, q2 = vrep.simxGetObjectPosition(clientID,joint_two_handle, UR3_handle,vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object current position for UR3')
+q2 = np.reshape(q2,(3,1))
 a2 = np.array([[-1],[0],[0]])
-q2 = np.array([[-0.1115], [0], [0.1089]])
-S2 = revolute(a2,q2)
-#print("q2: ",q2)
-print("S2: ", S2, "\n")
-
-# a3 = np.array([[1], [0], [0]])
-# # a3 = transforms3d.euler.euler2mat(a3_arr)
-# result, q3 = vrep.simxGetObjectPosition(clientID,joint_three_handle, UR3_handle,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get object current position for UR3')
-# q3 = np.reshape(q3,(3,1))
+S2 = revolute(a2, q2)
+
+result, q3 = vrep.simxGetObjectPosition(clientID,joint_three_handle, UR3_handle,vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object current position for UR3')
+q3 = np.reshape(q3,(3,1))
 a3 = np.array([[-1],[0],[0]])
-q3 = np.array([[-0.1115], [0], [0.3525]])
-S3 = revolute(a3,q3)
-
-# a4 = np.array([[-1], [0], [0]])
-# # a4 = transforms3d.euler.euler2mat(a4_arr)
-# result, q4 = vrep.simxGetObjectPosition(clientID,joint_four_handle, UR3_handle,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get object current position for UR3')
-# q4 = np.reshape(q4,(3,1))
+S3 = revolute(a3, q3)
+
+result, q4 = vrep.simxGetObjectPosition(clientID,joint_four_handle, UR3_handle,vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object current position for UR3')
+q4 = np.reshape(q4,(3,1))
 a4 = np.array([[-1],[0],[0]])
-q4 = np.array([[-0.1115], [0], [0.5658]])
-S4 = revolute(a4,q4)
-
-# a5 = np.array([[0], [0], [1]])
-# # a5 = transforms3d.euler.euler2mat(a5_arr)
-# result, q5 = vrep.simxGetObjectPosition(clientID,joint_five_handle, UR3_handle,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get object current position for UR3')
-# q5 = np.reshape(q5,(3,1))
+S4 = revolute(a4, q4)
+
+result, q5 = vrep.simxGetObjectPosition(clientID,joint_five_handle, UR3_handle,vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object current position for UR3')
+q5 = np.reshape(q5,(3,1))
 a5 = np.array([[0],[0],[1]])
-q5 = np.array([[-0.1122], [0], [0.65]])
-S5 = revolute(a5,q5)
-
-# a6 = np.array([[-1], [0], [0]])
-# # a6 = transforms3d.euler.euler2mat(a6_arr)
-# result, q6 = vrep.simxGetObjectPosition(clientID,joint_six_handle, UR3_handle,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get object current position for UR3')
-# q6 = np.reshape(q6,(3,1))
+S5 = revolute(a5, q5)
+
+result, q6 = vrep.simxGetObjectPosition(clientID,joint_six_handle, UR3_handle,vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object current position for UR3')
+q6 = np.reshape(q6,(3,1))
 a6 = np.array([[-1],[0],[0]])
-q6 = np.array([[0.1115], [0], [0.6511]])
-S6 = revolute(a6,q6)
+S6 = revolute(a6, q6)
 
-# Also get the end of UR3 sixth joint handle
-# result, joint_six_handle= vrep.simxGetObjectHandle(clientID, 'UR3_joint6', vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-# 	raise Exception('could not get UR3_joint6 handle')
 
 #Use T = e^([s]*theta)* M_inital to get final pose
 #The basic Forward kinematics equation
@@ -238,7 +143,7 @@ j5 = screwBracForm(S5) * theta_list[4]
 j6 = screwBracForm(S6) * theta_list[5]
 
 T_final = multi_dot([expm(j1), expm(j2), expm(j3), expm(j4), expm(j5), expm(j6), M])
-print("T_final", T_final)
+# print("T_final", T_final)
 
 quaternion = quaternion_from_matrix(T_final)
 temp = quaternion[0]
@@ -248,125 +153,28 @@ quaternion[2] = quaternion[3]
 quaternion[3] = temp
 
 P_final = T_final[0:3,3]
-print ("P_final", P_final)
-# position = (pose[0, 3], pose[1, 3], pose[2, 3] - 0.193424112)
-print("Theta:", theta_list)
-print("Calculated quaternion:", quaternion)
-
-#P_final[3][0] = P_final[3][0] +
-
-# R_final = T_final[0:3, 0:3]
-# print ("R_final", R_final)
-
-#
-# final_a,final_b,final_g = transforms3d.euler.mat2euler(R_final)
-# final_euler = [final_a,final_b,final_g]
-# print ("final_euler", final_euler)
-
-
-
-# result, dummy_ball_handle = vrep.simxGetObjectHandle(clientID, 'ReferenceFrame1', vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-# 	raise Exception('could not get reference frame 1 handle')
-#
-# time.sleep(1)
-#
-# result = vrep.simxSetObjectOrientation(clientID,dummy_ball_handle,-1,final_euler,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-# 	raise Exception('could not set dummy ball orientation')
-#
-# time.sleep(1)
-#
-# result = vrep.simxSetObjectPosition(clientID,dummy_ball_handle,-1,P_final,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-# 	raise Exception('could not set dummy ball position')
-
-
-
+# print ("P_final", P_final)
 
 # Show predicted position
-#result, dummy_handle_0 = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
-#if result != vrep.simx_return_ok:
-#    raise Exception('could not get dummy_handle_1')
 vrep.simxSetObjectPosition(clientID, dummy_handle_0, -1, P_final, vrep.simx_opmode_oneshot_wait)
 vrep.simxSetObjectQuaternion(clientID, dummy_handle_0, -1, quaternion, vrep.simx_opmode_oneshot_wait)
-time.sleep(3)
-
-# quaternion = quaternion_from_matrix(T_final)
-# temp = quaternion[0]
-# quaternion[0] = quaternion[1]
-# quaternion[1] = quaternion[2]
-# quaternion[2] = quaternion[3]
-# quaternion[3] = temp
-
-P_final = T_final[0:3,3]
-print ("P_final", P_final)
-print("Theta:", theta_list)
-# print("Calculated quaternion:", quaternion)
+time.sleep(1)
 
 R_final = T_final[0:3, 0:3]
-print ("R_final", R_final)
+# print ("R_final", R_final)
 
 final_a,final_b,final_g = transforms3d.euler.mat2euler(R_final)
 final_euler = [final_a, final_b, final_g]
-print ("final_euler", final_euler)
-
-# result, dummy_ball_handle = vrep.simxGetObjectHandle(clientID, 'ReferenceFrame1', vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-# 	raise Exception('could not get reference frame 1 handle')
-#
-# time.sleep(1)
-#
-# result = vrep.simxSetObjectOrientation(clientID,dummy_ball_handle,-1,final_euler,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-# 	raise Exception('could not set dummy ball orientation')
-#
-# time.sleep(1)
-#
-# result = vrep.simxSetObjectPosition(clientID,dummy_ball_handle,-1,P_final,vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-# 	raise Exception('could not set dummy ball position')
+# print ("final_euler", final_euler)
 
 # Show predicted position
 result, dummy_handle_1 = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
 if result != vrep.simx_return_ok:
     raise Exception('could not get dummy_handle_1')
+
 vrep.simxSetObjectPosition(clientID, dummy_handle_1, UR3_handle, P_final, vrep.simx_opmode_oneshot_wait)
 vrep.simxSetObjectOrientation(clientID, dummy_handle_1, UR3_handle, final_euler, vrep.simx_opmode_oneshot_wait)
 
-
-# Get the current value of the first joint variable
-# result, theta = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking)
-# if result != vrep.simx_return_ok:
-#     raise Exception('could not get first joint variable')
-# print('current value of first joint variable: theta = {:f}'.format(theta))
-#
-# result2, theta2 = vrep.simxGetJointPosition(clientID, joint_two_handle, vrep.simx_opmode_blocking)
-# if result2 != vrep.simx_return_ok:
-#     raise Exception('could not get second joint variable')
-# print('current value of second joint variable on UR3: theta = {:f}'.format(theta2))
-#
-# result3, theta3 = vrep.simxGetJointPosition(clientID, joint_three_handle, vrep.simx_opmode_blocking)
-# if result3 != vrep.simx_return_ok:
-#     raise Exception('could not get third joint variable')
-# print('current value of third joint variable on UR3: theta = {:f}'.format(theta3))
-#
-# result4, theta4 = vrep.simxGetJointPosition(clientID, joint_four_handle, vrep.simx_opmode_blocking)
-# if result4 != vrep.simx_return_ok:
-#     raise Exception('could not get fourth joint variable')
-# print('current value of fourth joint variable on UR3: theta = {:f}'.format(theta4))
-#
-# result5, theta5 = vrep.simxGetJointPosition(clientID, joint_five_handle, vrep.simx_opmode_blocking)
-# if result5 != vrep.simx_return_ok:
-#     raise Exception('could not get fifth joint variable')
-# print('current value of fifth joint variable on UR3: theta = {:f}'.format(theta5))
-#
-# result6, theta6 = vrep.simxGetJointPosition(clientID, joint_six_handle, vrep.simx_opmode_blocking)
-# if result6 != vrep.simx_return_ok:
-#     raise Exception('could not get sixth joint variable')
-# print('current value of sixth joint variable on UR3: theta = {:f}'.format(theta6))
-
-
 # Set the desired value of the first joint variable
 vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta_list[0], vrep.simx_opmode_oneshot_wait)
 vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta_list[1], vrep.simx_opmode_oneshot_wait)
@@ -374,12 +182,10 @@ vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta_list[2], vre
 vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta_list[3], vrep.simx_opmode_oneshot_wait)
 vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta_list[4], vrep.simx_opmode_oneshot_wait)
 vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta_list[5], vrep.simx_opmode_oneshot_wait)
-# Wait two seconds
-time.sleep(10)
-
+# time.sleep(2)
 
 vrep.simxRemoveObject(clientID,dummy_handle_0,vrep.simx_opmode_oneshot_wait)
-#vrep.simxRemoveObject(clientID,dummy_handle_1,vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID,dummy_handle_1,vrep.simx_opmode_oneshot_wait)
 
 # Stop simulation
 vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)