check3 created

FinalProject/Check_Point_2/check_point2.py

@@ -14,42 +14,42 @@ 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
+# 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)

FinalProject/Check_Point_3/README.md

+## Checkpoint 3 (due 3/27/18)
+
+### Inverse Kinematics of UR3
+<!-- We implemented the basic forward kinematics of UR3 with V-REP at this checkpoint.
+
+First, we find the initial position and orientation of UR3 with respect to world frame.
+Then we find joint angles and implemented the matrix exponentials after which we
+calculated the final pose.
+
+Using the matrix logarithm of rotational matrix of the final pose, we found the angular
+velocity. With the angular velocity we can find a and theta, which we then use to find
+the quaternions.
+
+We referenced this Piazza post by Professor Bretl to find the equations needed:
+https://piazza.com/class/jchxn1s6tkg20r?cid=257
+
+We also displayed the dummy objects in the form reference frames as indication of successful
+implementation of the forward kinematics. -->