Merge branch 'siyunhe' of https://gitlab.engr.illinois.edu/rmaksi2/ECE470 into siyunhe

FinalProject/Check_Point_4/UsefulFuncUpdated_0410.py

+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