diff --git a/FinalProject/Check_Point_4/UsefulFuncUpdated_0410.py b/FinalProject/Check_Point_4/UsefulFuncUpdated_0410.py
new file mode 100644
index 0000000000000000000000000000000000000000..49be3fa24e035328681ccfa9057f4ffebad4c8bf
--- /dev/null
+++ b/FinalProject/Check_Point_4/UsefulFuncUpdated_0410.py
@@ -0,0 +1,240 @@
+import vrep
+import time
+import numpy as np
+import math
+from numpy.linalg import multi_dot
+from scipy.linalg import expm, logm
+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 createSkew(mat):
+ skew = np.array([
+ [0,-mat[2][0],mat[1][0]],
+ [mat[2][0],0,-mat[0][0]],
+ [-mat[1][0],mat[0][0],0]
+ ])
+
+ return skew
+
+def createScrew(a):
+ screw = np.array([
+ [0],
+ [0],
+ [0],
+ [a[0][0]],
+ [a[1][0]],
+ [a[2][0]]
+ ])
+ return screw
+
+def createScrewQ(a, q):
+
+ aq = -1 * np.dot(createSkew(a), q)
+
+ screw = np.array([
+ [a[0][0]],
+ [a[1][0]],
+ [a[2][0]],
+ [aq[0][0]],
+ [aq[1][0]],
+ [aq[2][0]]
+ ])
+
+ return screw
+
+def screwBracForm(mat) :
+
+ brac = np.array([
+ [0, -1 * mat[2][0], mat[1][0], mat[3][0]],
+ [mat[2][0], 0, -1 * mat[0][0], mat[4][0]],
+ [-1 * mat[1][0], mat[0][0], 0, mat[5][0]],
+ [0,0,0,0]
+ ])
+
+ return brac
+
+def createAdjoint(T):
+ """
+ Returns the adjoint transformation matrix of T
+ :param T: the pose whose 6x6 adjoint matrix to return
+ """
+ rot, pos = fromPose(T)
+ return np.block([[ rot, np.zeros((3,3)) ],
+ [ bracket(pos).dot(rot), rot ]])
+
+ return adj
+
+def invScrewBrac(mat):
+
+ twist = np.array([
+ [mat[2][1]],
+ [mat[0][2]],
+ [mat[1][0]],
+ [mat[0][3]],
+ [mat[1][3]],
+ [mat[2][3]]
+ ])
+
+ return twist
+
+def toTs(S, theta):
+ """
+ Generates a list of HCT matricies from a list of screw axes and joint variables. Not that useful for general work,
+ but used by other functions.
+ :param S: A python list of 6x1 screw axes
+ :param theta: A list/numpy array of joint vars. Should have the same number of elements as S
+ :returns: A python list of 4x4 HCT matricies representing a transformation by each of the screw axes
+ """
+ if isinstance(S, np.ndarray):
+ S = np.hsplit(S, S.shape[1])
+ return [expm(bracket(s) * t) for s, t in zip(S, theta)]
+
+
+def getSpacialJacobian(S, theta):
+ """
+ Finds the space jacobian of a robot with given screw axes at a given joint positions:
+ TODO: Improve efficeny by removing the need to recompute the transformation for each screw
+ :param S: a python list of 6x1 screw axes
+ :param theta: a python list/numpy array of joint vars. Should be same number of elements as S
+ :returns: A 6xN matrix representing the space Jacobian of the robot with the given screw axes at the given joint vars
+ """
+ if isinstance(S, np.ndarray):
+ S = np.hsplit(S, S.shape[1])
+ T = sequential_Ts(S, theta)
+ J = [S[0]]
+ for t, s in zip(T, S[1:]):
+ J.append(adj_T(t).dot(s))
+ return np.hstack(J)
+
+
+def getT_1in0(M, S, theta):
+ """
+ Basically Forward Kinematics
+ Finds the end position of a robot based on space screw axes, joint vars and the space 'zero HCT'
+ Note that numpy arrays of screw axes are not supported, only python lists of screw axes.
+ Use np.hsplit(S, N) to generate a list of screw axes given a numpy array S where N is the number of joints (cols in the matrix)
+ :param S: A python list of 6x1 screw axes from the base to the manipulator
+ :param theta: A python list/numpy array of joint vars in the same order as S.
+ :param M: A 4x4 HCT transformation matrix representing the pose of the end effector when theta = 0 for all joint vars
+ :returns: A numpy 4x4 HCT transformation matrix representing the pose of the end effector at the given joint vars
+ """
+ M=np.identity(4)
+ ret = np.identity(4)
+ for t in toTs(S, theta):
+ ret = ret.dot(t)
+ return ret.dot(M)
+
+#######################################################################################################################
+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
+
+
+
+
+
+# Compute the predicted pose for the tool frame given a set of joint variables.
+def forwardKinematics(M, S, thetas):
+
+ product = 1
+ for s in range(len(thetas)):
+ product = np.dot(product, expm(bracket(S[:,s])*degToRad(thetas[s])))
+
+ T = np.dot(product, M)
+ return T
+
+# Find a set of joint variables to reach the goal pose
+def inverseKinematics(goal, M, S):
+ #initial gase of thetas
+ theta = np.random.rand(S.shape[1])
+ V_error = 7
+ theta_error = 7
+
+ count = 0
+ while V_error > 0.1 or theta_error > 0.01:
+ if count > 100:
+ return theta
+
+ #Goal Pose
+ T = forwardKinematics(M, S, theta)
+ V_bracket = logm(np.dot(goal, np.linalg.inv(T)))
+
+ #Compute Required spatial twist to achieve this
+ V = twist(V_bracket)
+
+ #calculate the space jacobian
+ J = spaceJacobian(S, theta)
+
+ # Theta = Theta + [ (JT * J + 0.00001*I)^-1 * (JT * V) ] - [ (I - J#J) * Theta ]
+ theta_dot = np.dot(np.linalg.inv(np.dot(np.transpose(J), J) + 0.1*np.identity(10)),
+ np.dot(np.transpose(J), V))
+ - np.dot(np.identity(10) - np.dot(np.linalg.pinv(J), J), theta)
+ # theta_dot = np.dot(J, V)
+ theta = theta + theta_dot
+ V_error = np.linalg.norm(V)
+ theta_error = np.linalg.norm(theta_dot)
+ count += 1
+
+ return theta
+
+
+# Update sphere centers using forward kinematics
+def updateCenterSphere(clientID, centers, S, thetas):
+ new_centers = []
+ thetas = thetas.copy()
+
+ joints_to_add = [0,1,2,3,4,5]
+ for i in range(5):
+ old_position = np.block([centers[i], 1])
+ new_position = forwardKinematics(old_position, S[:,:joints_to_add[i]+1], thetas[:joints_to_add[i]+1])
+ new_centers.append(new_position[0:3])
+ return new_centers
+
+
+# Check for collision
+def checkCollision(joint_centers, body_centers, link_centers):
+
+ # Check for link collision
+ for i in range(len(joint_names)):
+ center = joint_centers[i]
+
+ for j in range(len(link_names)):
+ link = link_centers[j]
+
+ if np.linalg.norm(center - link) < joint_diam[i]/2 + link_diam/2:
+ return True
+
+ # Check for self-collision
+ self_centers = body_centers.copy()
+ self_centers.extend(joint_centers)
+ total = len(joint_names) + len(body_names)
+ for i in range(total):
+ for j in range(total-1-i):
+ if np.linalg.norm(self_centers[i] - self_centers[j+i+1]) < self_diam[i]/2 + self_diam[j+i+1]/2:
+ return True
+
+ return False
diff --git a/FinalProject/Check_Point_4/check_point4.py b/FinalProject/Check_Point_4/check_point4.py
index 7c88aba475ba96af6b490e08d04ca449ba4cf465..70b6afc02864a5999f7810d68619c8717428d4af 100644
--- a/FinalProject/Check_Point_4/check_point4.py
+++ b/FinalProject/Check_Point_4/check_point4.py
@@ -2,462 +2,229 @@ import vrep
import time
import numpy as np
import math
-from numpy.linalg import multi_dot
+from numpy.linalg import multi_dot, norm, inv
from scipy.linalg import expm, logm
-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 ece470_lib import *
+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 createSkew(mat):
- skew = np.array([
- [0,-mat[2][0],mat[1][0]],
- [mat[2][0],0,-mat[0][0]],
- [-mat[1][0],mat[0][0],0]
- ])
-
- return skew
-
-def createScrew(a):
- screw = np.array([
- [0],
- [0],
- [0],
- [a[0][0]],
- [a[1][0]],
- [a[2][0]]
- ])
- return screw
-
-def createScrewQ(a, q):
-
- aq = -1 * np.dot(createSkew(a), q)
-
- screw = np.array([
- [a[0][0]],
- [a[1][0]],
- [a[2][0]],
- [aq[0][0]],
- [aq[1][0]],
- [aq[2][0]]
- ])
-
- return screw
-
-def screwBracForm(mat) :
-
- brac = np.array([
- [0, -1 * mat[2][0], mat[1][0], mat[3][0]],
- [mat[2][0], 0, -1 * mat[0][0], mat[4][0]],
- [-1 * mat[1][0], mat[0][0], 0, mat[5][0]],
- [0,0,0,0]
- ])
-
- return brac
-
-def createAdjoint(mat):
-
- R = np.array([
- [mat[0][0],mat[0][1],mat[0][2]],
- [mat[1][0],mat[1][1],mat[1][2]],
- [mat[2][0],mat[2][1],mat[2][2]]
- ])
- p = np.array([
- [mat[0][3]],
- [mat[1][3]],
- [mat[2][3]]
- ])
-
- bottomLeft = np.dot(createSkew(p), R)
-
- adj = np.array([
- [R[0][0],R[0][1],R[0][2],0,0,0],
- [R[1][0],R[1][1],R[1][2],0,0,0],
- [R[2][0],R[2][1],R[2][2],0,0,0],
- [bottomLeft[0][0],bottomLeft[0][1],bottomLeft[0][2], R[0][0],R[0][1],R[0][2]],
- [bottomLeft[1][0],bottomLeft[1][1],bottomLeft[1][2], R[1][0],R[1][1],R[1][2]],
- [bottomLeft[2][0],bottomLeft[2][1],bottomLeft[2][2], R[2][0],R[2][1],R[2][2]]
- ])
-
-
- return adj
-
-def invScrewBrac(mat):
-
- twist = np.array([
- [mat[2][1]],
- [mat[0][2]],
- [mat[1][0]],
- [mat[0][3]],
- [mat[1][3]],
- [mat[2][3]]
- ])
-
- return twist
-
-# def spaceJacobian(S, theta):
-# J = np.zeros((6,len(theta)))
-
-# for i in range(len(theta)):
-# if i == 0:
-# J[:,i] = S[:,i]
-# else:
-# product = 1
-# for j in range(i):
-# product = np.dot(product, expm(bracket(S[:,j])*theta[j]))
-
-# J[:,i] = np.dot(adjoint(product), S[:,i])
-
-# return J
-
-def getSpacialJacobian(S, theta):
-
- mat1 = np.array([
- [S[0][0]],
- [S[1][0]],
- [S[2][0]],
- [S[3][0]],
- [S[4][0]],
- [S[5][0]]
- ])
- brac1 = screwBracForm(mat1) * theta[0]
- mat1adj = createAdjoint(expm(brac1))
-
- mat2 = np.array([
- [S[0][1]],
- [S[1][1]],
- [S[2][1]],
- [S[3][1]],
- [S[4][1]],
- [S[5][1]]
- ])
- brac2 = screwBracForm(mat2) * theta[1]
- mat2adj = createAdjoint(expm(brac2))
- mat2 = np.dot(mat1adj, mat2)
-
- mat3 = np.array([
- [S[0][2]],
- [S[1][2]],
- [S[2][2]],
- [S[3][2]],
- [S[4][2]],
- [S[5][2]]
- ])
- brac3 = screwBracForm(mat3) * theta[2]
- mat3adj = createAdjoint(expm(brac3))
- mat3 = np.linalg.multi_dot([mat1adj, mat2adj, mat3])
-
- mat4 = np.array([
- [S[0][3]],
- [S[1][3]],
- [S[2][3]],
- [S[3][3]],
- [S[4][3]],
- [S[5][3]]
- ])
- brac4 = screwBracForm(mat4) * theta[3]
- mat4adj = createAdjoint(expm(brac4))
- mat4 = np.linalg.multi_dot([mat1adj, mat2adj, mat3adj, mat4])
-
-
- mat5 = np.array([
- [S[0][4]],
- [S[1][4]],
- [S[2][4]],
- [S[3][4]],
- [S[4][4]],
- [S[5][4]]
- ])
- brac5 = screwBracForm(mat5) * theta[4]
- mat5adj = createAdjoint(expm(brac5))
- mat5 = np.linalg.multi_dot([mat1adj, mat2adj, mat3adj, mat4adj, mat5])
-
- mat6 = np.array([
- [S[0][5]],
- [S[1][5]],
- [S[2][5]],
- [S[3][5]],
- [S[4][5]],
- [S[5][5]]
- ])
- brac6 = screwBracForm(mat6) * theta[5]
- mat6adj = createAdjoint(expm(brac6))
- mat6 = np.linalg.multi_dot([mat1adj, mat2adj, mat3adj, mat4adj, mat5adj, mat6])
-
-
- j = np.array([
- [mat1[0][0],mat2[0][0], mat3[0][0], mat4[0][0], mat5[0][0], mat6[0][0]],
- [mat1[1][0],mat2[1][0], mat3[1][0], mat4[1][0], mat5[1][0], mat6[1][0]],
- [mat1[2][0],mat2[2][0], mat3[2][0], mat4[2][0], mat5[2][0], mat6[2][0]],
- [mat1[3][0],mat2[3][0], mat3[3][0], mat4[3][0], mat5[3][0], mat6[3][0]],
- [mat1[4][0],mat2[4][0], mat3[4][0], mat4[4][0], mat5[4][0], mat6[4][0]],
- [mat1[5][0],mat2[5][0], mat3[5][0], mat4[5][0], mat5[5][0], mat6[5][0]]
- ])
-
- return j
-
-def getT_1in0(M, S, theta):
-
- mat1 = np.array([
- [S[0][0]],
- [S[1][0]],
- [S[2][0]],
- [S[3][0]],
- [S[4][0]],
- [S[5][0]]
- ])
- brac1 = screwBracForm(mat1) * theta[0]
-
- mat2 = np.array([
- [S[0][1]],
- [S[1][1]],
- [S[2][1]],
- [S[3][1]],
- [S[4][1]],
- [S[5][1]]
- ])
- brac2 = screwBracForm(mat2) * theta[1]
-
- mat3 = np.array([
- [S[0][2]],
- [S[1][2]],
- [S[2][2]],
- [S[3][2]],
- [S[4][2]],
- [S[5][2]]
- ])
- brac3 = screwBracForm(mat3) * theta[2]
-
- mat4 = np.array([
- [S[0][3]],
- [S[1][3]],
- [S[2][3]],
- [S[3][3]],
- [S[4][3]],
- [S[5][3]]
- ])
- brac4 = screwBracForm(mat4) * theta[3]
-
- mat5 = np.array([
- [S[0][4]],
- [S[1][4]],
- [S[2][4]],
- [S[3][4]],
- [S[4][4]],
- [S[5][4]]
- ])
- brac5 = screwBracForm(mat5) * theta[4]
-
- mat6 = np.array([
- [S[0][5]],
- [S[1][5]],
- [S[2][5]],
- [S[3][5]],
- [S[4][5]],
- [S[5][5]]
- ])
- brac6 = screwBracForm(mat6) * theta[5]
-
- T = multi_dot([expm(brac1), expm(brac2), expm(brac3), expm(brac4), expm(brac5), expm(brac6), M])
-
- return T
-
-#######################################################################################################################
-
-# Compute the predicted pose for the tool frame given a set of joint variables.
-def forwardKinematics(M, S, thetas):
-
- product = 1
- for s in range(len(thetas)):
- product = np.dot(product, expm(bracket(S[:,s])*degToRad(thetas[s])))
-
- T = np.dot(product, M)
- return T
-
-# Find a set of joint variables to reach the goal pose
-def inverseKinematics(goal, M, S):
- #initial gase of thetas
- theta = np.random.rand(S.shape[1])
- V_error = 7
- theta_error = 7
-
- count = 0
- while V_error > 0.1 or theta_error > 0.01:
- if count > 100:
- return theta
-
- #Goal Pose
- T = forwardKinematics(M, S, theta)
- V_bracket = logm(np.dot(goal, np.linalg.inv(T)))
-
- #Compute Required spatial twist to achieve this
- V = twist(V_bracket)
-
- #calculate the space jacobian
- J = spaceJacobian(S, theta)
-
- # Theta = Theta + [ (JT * J + 0.00001*I)^-1 * (JT * V) ] - [ (I - J#J) * Theta ]
- theta_dot = np.dot(np.linalg.inv(np.dot(np.transpose(J), J) + 0.1*np.identity(10)),
- np.dot(np.transpose(J), V))
- - np.dot(np.identity(10) - np.dot(np.linalg.pinv(J), J), theta)
- # theta_dot = np.dot(J, V)
- theta = theta + theta_dot
- V_error = np.linalg.norm(V)
- theta_error = np.linalg.norm(theta_dot)
- count += 1
-
- return theta
-
-
-# Update sphere centers using forward kinematics
-def updateCenterSphere(clientID, centers, S, thetas):
- new_centers = []
- thetas = thetas.copy()
-
- joints_to_add = [0,1,2,3,4,5]
- for i in range(5):
- old_position = np.block([centers[i], 1])
- new_position = forwardKinematics(old_position, S[:,:joints_to_add[i]+1], thetas[:joints_to_add[i]+1])
- new_centers.append(new_position[0:3])
- return new_centers
-
-# Check for collision
-def checkCollision(joint_centers, body_centers, link_centers):
-
- # Check for link collision
- for i in range(len(joint_names)):
- center = joint_centers[i]
-
- for j in range(len(link_names)):
- link = link_centers[j]
-
- if np.linalg.norm(center - link) < joint_diam[i]/2 + link_diam/2:
- return True
- # Check for self-collision
- self_centers = body_centers.copy()
- self_centers.extend(joint_centers)
- total = len(joint_names) + len(body_names)
- for i in range(total):
- for j in range(total-1-i):
- if np.linalg.norm(self_centers[i] - self_centers[j+i+1]) < self_diam[i]/2 + self_diam[j+i+1]/2:
- return True
-
- return False
-
-
-def Collision_main():
-
- #Base dummy object
- body_names = ["Dummy_body_base"]
- body_diam = [0.4]
-
- #link is between two joints
- link_names = ["Dummy_link_joint1", "Dummy_link_joint2", "Dummy_link_joint3", "Dummy_link_joint4", "Dummy_link_joint5", "Dummy_link_joint6", "Dummy_link_joint7"]
-
- #dummy objects for joints
- joint_names = ["Dummy_joint1", "Dummy_joint2", "Dummy_joint3", "Dummy_joint4", "Dummy_joint5", "Dummy_joint6"]
- joint_diam = [0.3, 0.3, 0.2, 0.2, 0.15, 0.15]
-
- #ADD Base dummy to whole joints dummy.
- self_diam = body_diam.copy()
- self_diam.extend(joint_diam)
- link_centers = []
- body_centers = []
- joint_centers = []
+# Close all open connections (just in case)
+vrep.simxFinish(-1)
- for i in range(len(link_names)):
- result, dummy_handle = vrep.simxGetObjectHandle(clientID, link_names[i], vrep.simx_opmode_blocking)
- if result != vrep.simx_return_ok:
- raise Exception("Could not get object handle for the Dummy object")
+# Connect to V-REP (raise exception on failure)
+clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
+if clientID == -1:
+ raise Exception('Failed connecting to remote API server')
- status, position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
- link_centers.append(np.array(position))
+# Dummies for ConcretBlock1
+result, dummy_0 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
+result, dummy_2 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_3 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_1 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_4 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_5 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+
+# Dummies for ConcretBlock2
+result, dummy_6 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
+result, dummy_7 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_8 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_9 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_10 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+result, dummy_11 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
+
+# Get handle for the ConcretBlock1
+result, ConcretBlock1_handle = vrep.simxGetObjectHandle(clientID, 'ConcretBlock1', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for the ConcretBlock1')
- for j in range(len(body_names)):
- result, dummy_handle = vrep.simxGetObjectHandle(clientID, body_names[j], vrep.simx_opmode_blocking)
- if result != vrep.simx_return_ok:
- raise Exception("Could not get object handle for the Dummy object")
+# Get handle for the ConcretBlock2
+result, ConcretBlock2_handle = vrep.simxGetObjectHandle(clientID, 'ConcretBlock2', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for the ConcretBlock2')
- status, position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
- body_centers.append(np.array(position))
+# Get position of the ConcretBlock1
+result, ConcretBlock1_position = vrep.simxGetObjectPosition(clientID, ConcretBlock1_handle, -1, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('Could not get object position for the ConcretBlock1')
+ConcretBlock1_position = np.reshape(ConcretBlock1_position,(3,1))
- for k in range(len(joint_names)):
- result, dummy_handle = vrep.simxGetObjectHandle(clientID, joint_names[k], vrep.simx_opmode_blocking)
- if result != vrep.simx_return_ok:
- raise Exception("Could not get object handle for the Dummy object")
+# Get position of the ConcretBlock2
+result, ConcretBlock2_position = vrep.simxGetObjectPosition(clientID, ConcretBlock2_handle, -1, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('Could not get object position for the ConcretBlock2')
+ConcretBlock2_position = np.reshape(ConcretBlock2_position,(3,1))
+
+# Draw the dummies for the ConcretBlock1
+vrep.simxSetObjectPosition(clientID, dummy_0, -1, ConcretBlock1_position, vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_1, dummy_0, [0,0,-0.1], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_2, dummy_0, [0,0,-0.2], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_3, dummy_0, [0,0,-0.3], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_4, dummy_0, [0,0,0.1], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_5, dummy_0, [0,0,0.2], vrep.simx_opmode_oneshot_wait)
+
+# Draw the dummies for the ConcretBlock2
+vrep.simxSetObjectPosition(clientID, dummy_6, -1, ConcretBlock2_position, vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_7, dummy_6, [0,0,-0.1], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_8, dummy_6, [0,0,-0.2], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_9, dummy_6, [0,0,-0.3], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_10, dummy_6, [0,0,0.1], vrep.simx_opmode_oneshot_wait)
+vrep.simxSetObjectPosition(clientID, dummy_11, dummy_6, [0,0,0.2], vrep.simx_opmode_oneshot_wait)
+
+# Create p_obstacle array for collision checking and path planner
+
+# Lists for final p_obstacle
+entry_one = list()
+entry_two = list()
+entry_three = list()
+
+# First, we handle first dummy (string combination), then we handle the rest in a loop
+result, dummy_handle = vrep.simxGetObjectHandle(clientID, 'Dummy', vrep.simx_opmode_oneshot_wait)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for Dummy')
- status, position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
- joint_centers.append(np.array(position))
+result, dummy_position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_oneshot_wait)
+if result != vrep.simx_return_ok:
+ raise Exception('Could not get object position for Dummy')
+entry_one.append(dummy_position[0])
+entry_two.append(dummy_position[1])
+entry_three.append(dummy_position[2])
- dummy_list = []
- print(link_centers)
- print(joint_centers)
- print(body_centers)
+# Loop for the rest
+for i in range(11):
+ dummy_handle_name = 'Dummy' + str(i)
+ result, dummy_handle = vrep.simxGetObjectHandle(clientID, dummy_handle_name, vrep.simx_opmode_blocking)
+ if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for ', dummy_handle_name)
+ dummy_name = 'dummy_' + str(i+1)
+ result, dummy_position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
+ if result != vrep.simx_return_ok:
+ raise Exception('Could not get object position for ', dummy_name)
+ entry_one.append(dummy_position[0])
+ entry_two.append(dummy_position[1])
+ entry_three.append(dummy_position[2])
+entry_one = np.asarray(entry_one)
+entry_two = np.asarray(entry_two)
+entry_three = np.asarray(entry_three)
+#################### p_obstacle ################################################
+p_obstacle = np.block([
+[np.asarray(entry_one)], [np.asarray(entry_two)], [np.asarray(entry_three)]
+])
-#######################################################################################################################
-def main():
- x = float(input("Enter X translation position"))
- y = float(input("Enter Y translation position"))
- z = float(input("Enter Z translation position"))
- a = float(input("Enter a rorational angle in degrees"))
- b = float(input("Enter b rorational angle in degrees"))
- c = float(input("Enter c rorational angle in degrees"))
+################################################################################
- Goal_pose = RotationMatrixToPose(x, y, z, a, b, c)
- return Goal_pose
+# Get joint object handles
+result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link1_visible', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for first joint')
+#First dummy1-Red
+result, handle = vrep.simxCreateDummy(clientID, 0.125, [255,0,0] , vrep.simx_opmode_blocking)
+#use the handle to set dummy object of the joint handle
+vrep.simxSetObjectPosition(clientID, handle, -1, (0,0,0), vrep.simx_opmode_oneshot)
-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))]])
+#Get second joint object handles
+result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link2_visible', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for first joint')
+#Get position of joint 2 handle
+result, p = vrep.simxGetObjectPosition(clientID, joint_two_handle, handle, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object current position for UR3')
+P_2 = np.reshape(p,(3,1))
+print ("P_2", P_2)
- 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))]])
+#Set dummy2
+result, handle_2 = vrep.simxCreateDummy(clientID, 0.125, [0,0,255] , vrep.simx_opmode_blocking)
+# vrep.simxSetObjectPosition(clientID, handle, -1, P_2, vrep.simx_opmode_oneshot)
+vrep.simxSetObjectPosition(clientID, handle_2, -1, P_2, vrep.simx_opmode_oneshot)
- 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]])
+#Joint 3
+result, joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link3_visible', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for first joint')
+result, p_3 = vrep.simxGetObjectPosition(clientID, joint_three_handle, handle, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object current position for UR3')
+P_3 = np.reshape(p_3,(3,1))
+print ("P_3", P_3)
+#Dummy 3
+result, handle_3 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
+vrep.simxSetObjectPosition(clientID, handle_3, -1, P_3, vrep.simx_opmode_oneshot)
- R = multi_dot([Rot_x, Rot_y, Rot_z])
- Goal_pose[0:3,0:3] = R
- return Goal_pose
+#Joint4
+result, joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for first joint')
+result, p_4 = vrep.simxGetObjectPosition(clientID, joint_four_handle, handle, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object current position for UR3')
+P_4 = np.reshape(p_4,(3,1))
+print ("P_4", P_4)
+#Dummy 3
+result, handle_4 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
+vrep.simxSetObjectPosition(clientID, handle_4, -1, P_4, vrep.simx_opmode_oneshot)
+
+#Joint5
+result, joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link4_visible', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for first joint')
+result, p_5 = vrep.simxGetObjectPosition(clientID, joint_five_handle, handle, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object current position for UR3')
+P_5 = np.reshape(p_5,(3,1))
+print ("P_5", P_5)
+#Dummy 3
+result, handle_5 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
+vrep.simxSetObjectPosition(clientID, handle_5, -1, P_5, vrep.simx_opmode_oneshot)
+
+#Joint6
+result, joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for first joint')
+result, p_6 = vrep.simxGetObjectPosition(clientID, joint_six_handle, handle, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object current position for UR3')
+P_6 = np.reshape(p_6,(3,1))
+print ("P_6", P_6)
+#Dummy 3
+result, handle_6 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
+vrep.simxSetObjectPosition(clientID, handle_6, -1, P_6, vrep.simx_opmode_oneshot)
-# Close all open connections (just in case)
-vrep.simxFinish(-1)
-# Connect to V-REP (raise exception on failure)
-clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
-if clientID == -1:
- raise Exception('Failed connecting to remote API server')
+#Joint7
+result, joint_seven_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link7', vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object handle for first joint')
+result, p_7 = vrep.simxGetObjectPosition(clientID, joint_seven_handle, handle, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+ raise Exception('could not get object current position for UR3')
+P_7 = np.reshape(p_7,(3,1))
+print ("P_7", P_7)
+#Dummy 3
+result, handle_7 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
+vrep.simxSetObjectPosition(clientID, handle_7, -1, P_7, vrep.simx_opmode_oneshot)
+# vrep.simxSetObjectPosition(clientID, handle, joint_one_handle, (0,0,0), vrep.simx_opmode_oneshot_wait)
+print("################################################")
-# Make a dummy
-result, dummy_handle_0 = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
+##################################################################################
-# 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')
@@ -486,36 +253,29 @@ if result != vrep.simx_return_ok:
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 handel 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)
-
-# Dummy handle draw at the joint_six_handle
-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)
+#Start simulation
+vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# 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([[0,0,-1, -0.1940], [0,1,0,0], [1,0,0,0.6511], [0,0,0,1]])
-
-print ("M", M, "\n")
+M = np.array([[0,0,-1, P_initial[0][0]], [0,1,0, P_initial[1][0]], [1,0,0, P_initial[2][0]], [0,0,0,1]])
a1 = np.array([[0],[0],[1]])
q1 = np.array([[0], [0], [0.1045]])
@@ -541,113 +301,286 @@ a6 = np.array([[-1],[0],[0]])
q6 = np.array([[0.1115], [0], [0.6511]])
S6 = revolute(a6,q6)
-pos_moved_to = 0
-
-while pos_moved_to < 3:
- T_2in0 = main()
- # T_2in0 = RotationMatrixToPose(0.2, 0.2, 0.2, 0.1, 0.1, 0.1)
- print(T_2in0)
- S = np.array([
- [S1[0][0], S2[0][0], S3[0][0], S4[0][0], S5[0][0], S6[0][0]],
- [S1[1][0], S2[1][0], S3[1][0], S4[1][0], S5[1][0], S6[1][0]],
- [S1[2][0], S2[2][0], S3[2][0], S4[2][0], S5[2][0], S6[2][0]],
- [S1[3][0], S2[3][0], S3[3][0], S4[3][0], S5[3][0], S6[3][0]],
- [S1[4][0], S2[4][0], S3[4][0], S4[4][0], S5[4][0], S6[4][0]],
- [S1[5][0], S2[5][0], S3[5][0], S4[5][0], S5[5][0], S6[5][0]]
- ])
-
- ############### Get the values for joint variables #############################
-
- result1, theta1 = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking)
- if result1 != vrep.simx_return_ok:
- raise Exception('could not get first joint variable')
- print('current value of first joint variable on UR3: theta = {:f}'.format(theta1))
-
- 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))
-
- theta = np.array([[theta1],[theta2],[theta3],[theta4],[theta5],[theta6]])
- k = 1
-
- T_1in0 = getT_1in0(M, S, theta)
- screwMat = logm(np.dot(T_2in0, np.linalg.inv(T_1in0)))
- twist = invScrewBrac(screwMat)
+S = np.block([[S1, S2, S3, S4, S5, S6]])
+
+
+############### Get the values for joint variables #############################
+
+result1, theta1 = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking)
+if result1 != vrep.simx_return_ok:
+ raise Exception('could not get first joint variable')
+# print('current value of first joint variable on UR3: theta = {:f}'.format(theta1))
+
+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))
+
+#Inital thetas from the simulator
+initial_thetas = [theta1, theta2, theta3, theta4, theta5, theta6]
+
+# Make a dummy for end_effector_pose
+result, end_effector_pose = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
+
+############################################
+# This is totally not working
+# #Change from Dummy11 if more dummies added before
+# result, end_effector_handle = vrep.simxGetObjectHandle(clientID, "Dummy11", vrep.simx_opmode_blocking)
+# if result != vrep.simx_return_ok:
+# raise Exception('could not get object handle for end_effector_pose')
+# result, end_effector_position = vrep.simxGetObjectPosition(clientID, end_effector_handle, -1, vrep.simx_opmode_blocking)
+# if result != vrep.simx_return_ok:
+# raise Exception('Could not get object position for end_effector_pose')
+#
+# # Draw the dummy
+# vrep.simxSetObjectPosition(clientID, end_effector_handle, -1, [1,1,1], vrep.simx_opmode_oneshot_wait)
+#
+# # vrep.simxSetObjectPosition(clientID, dummy_1, dummy_0, [0,0,-0.1], vrep.simx_opmode_oneshot_wait)
+#
+# print("Move dummy object at the end effector to the position you want to move to")
+#
+# while(1):
+# result, pos_check = vrep.simxGetObjectPosition(clientID, end_effector_handle, -1, vrep.simx_opmode_blocking)
+# if result != vrep.simx_return_ok:
+# raise Exception('Could not get object position for end_effector_pose')
+#
+# if(pos_check != end_effector_position):
+# break
+##########################################
+
+i = 0
+while i < 3:
+ if(input("Press any key to start: ") == "quit"):
+ break
+
+ goal_pose = user_input()
+ # goal_pose = RotationMatrixToPose(0.3, 0.4, 0.3, 30, 30, 30)
+
+ # end_effector_position = np.reshape(end_effector_position,(3,1))
+ # goal_pose = RotationMatrixToPose(end_effector_position[0][0], end_effector_position[1][0], end_effector_position[2][0], 30, 30, 30)
+
+ quaternion = quaternion_from_matrix(goal_pose)
+ temp = quaternion[0]
+ quaternion[0] = quaternion[1]
+ quaternion[1] = quaternion[2]
+ quaternion[2] = quaternion[3]
+ quaternion[3] = temp
+
+ P_final = goal_pose[0:3,3]
+
+ # Draw the dummy
+ vrep.simxSetObjectPosition(clientID, end_effector_pose, -1, P_final, vrep.simx_opmode_oneshot_wait)
+
+ 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]
+
+ 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]]])
+ step_size = 0.01
+
+ start_pos = np.array([[0],[0],[0]])
+ end_pos = np.array([[M[0][3]],[M[1][3]],[M[2][3]]])
+ p_robot = np.block([[start_pos, q1, q2, q3, q4, q5, q6, end_pos]])
+
+ r_robot = np.array([[0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]])
+ # r_robot = np.array([[0.12 , 0.18, 0.18, 0.18, 0.18, 0.18, 0.18,0.18 ]])
+ # p_obstacle = np.array([[1], [1], [1]])
+ r_obstacle = np.array([[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]])
+ # print(p_obstacle)
+
+ start_root = Node()
+ start_root.theta = theta_start
+ end_root = Node()
+ end_root.theta = theta_goal
+ start_curr = start_root
+ end_curr = end_root
counter = 0
- final_pose_flag = 1
+ bound = 50
+ #
+ # 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:
+ '''
+ 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))
+ rand1 = random.uniform(-np.pi,np.pi)
+ rand2 = random.uniform(-np.pi,np.pi)
+ rand3 = random.uniform(-np.pi,np.pi)
+ rand4 = random.uniform(-np.pi,np.pi)
+ rand5 = random.uniform(-np.pi,np.pi)
+ rand6 = random.uniform(-np.pi,np.pi)
+
+
+ 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
- while (np.linalg.norm(twist) > 0.01):
+ counter += 1
+
+ if counter == bound:
+ print("NO PATH POSSIBLE")
+ print("Start list:")
+ print_linked_list(start_root)
+ print("Goal list")
+ print_linked_list(end_root)
+ else:
+ print("Found path")
+
+ for theta in q:
+ # Set the desired value of the first joint variable
+ vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta[0][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta[1][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta[2][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta[3][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta[4][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta[5][0], vrep.simx_opmode_oneshot)
+ time.sleep(1)
+
+ q.reverse()
+
+ for theta in q:
+ # Set the desired value of the first joint variable
+ vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta[0][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta[1][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta[2][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta[3][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta[4][0], vrep.simx_opmode_oneshot)
+ time.sleep(0.1)
+ vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta[5][0], vrep.simx_opmode_oneshot)
+ time.sleep(1)
+
+ i += 1
+
+# Remove end_effector_pose dummy
+vrep.simxRemoveObject(clientID, end_effector_pose, vrep.simx_opmode_oneshot_wait)
+
+# Remove dummy handles for ConcretBlock1
+vrep.simxRemoveObject(clientID, dummy_0, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_1, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_2, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_3, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_4, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_5, vrep.simx_opmode_oneshot_wait)
+
+# Remove dummy handles for ConcretBlock2
+vrep.simxRemoveObject(clientID, dummy_6, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_7, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_8, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_9, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_10, vrep.simx_opmode_oneshot_wait)
+vrep.simxRemoveObject(clientID, dummy_11, vrep.simx_opmode_oneshot_wait)
- if(counter >= 100):
- print("Desired Pose not reachable")
- final_pose_flag = 0
- break
- J = getSpacialJacobian(S, theta)
- thetadot = np.dot(np.linalg.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), theta)
- theta = theta + (thetadot * k)
-
- T_1in0 = getT_1in0(M, S, theta)
- screwMat = logm(np.dot(T_2in0, np.linalg.inv(T_1in0)))
- twist = invScrewBrac(screwMat)
-
- counter = counter + 1
-
- if(final_pose_flag):
- pos_moved_to = pos_moved_to + 1
-
- quaternion = quaternion_from_matrix(T_2in0)
- temp = quaternion[0]
- quaternion[0] = quaternion[1]
- quaternion[1] = quaternion[2]
- quaternion[2] = quaternion[3]
- quaternion[3] = temp
-
- P_final = T_2in0[0:3,3]
- print ("P_final", P_final)
- print("Theta:", theta)
- print("Calculated quaternion:", quaternion)
-
- 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)
-
-
- # Set the desired value of the first joint variable
- vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta[0], vrep.simx_opmode_oneshot_wait)
- vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta[1], vrep.simx_opmode_oneshot_wait)
- vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta[2], vrep.simx_opmode_oneshot_wait)
- vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta[3], vrep.simx_opmode_oneshot_wait)
- vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta[4], vrep.simx_opmode_oneshot_wait)
- vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta[5], vrep.simx_opmode_oneshot_wait)
- # Wait two seconds
- time.sleep(10)
-
- #vrep.simxRemoveObject(clientID,dummy_handle_1,vrep.simx_opmode_oneshot_wait)
-vrep.simxRemoveObject(clientID,dummy_handle_0,vrep.simx_opmode_oneshot_wait)
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)