Checkpoint 5 works in terms of code, need to set up visuals and put in obstacle data

FinalProject/Check_Point_5/ece470_lib.py

 import numpy as np
+import math
 from numpy.linalg import inv
 from scipy.linalg import expm, logm
 
 """
 ece470_lib.py
-
 Created for ECE 470 CBTF Exams
-
 Portions of this library will be included with the exams.
 Import the library into Jupyter Notebook by placing this file in the same directory as your notebook.
 Then run:
 import ece470_lib as ece470
 and then you can use the functions as
 ece470.bracket(np.ones(6,1))
-
 Hosted at https://github.com/namanjindal/ECE-470-Library
 If you would like to contribute, reply to @289 on Piazza with your github username
-
 ------------
 Created by Naman Jindal
-March 2018
-
+With assistance of Kyle Jensen
+For any help/questions/fixes either comment on the piazza post above or email me at kejense2@illinois.edu!
+April 2018 Edition
 """
+def skew4(V_b):
+    return np.array([[0,-1*V_b[2],V_b[1],V_b[3]],[V_b[2],0,-1*V_b[0],V_b[4]],[-1*V_b[1],V_b[0],0,V_b[5]],[0,0,0,0]])
+
+
 def bracket(v):
     """
     Returns the 'bracket' operator of a 3x1 vector or 6x1 twist
@@ -117,24 +119,7 @@ def toTs(S, theta):
     :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
     """
-    # mats = list()
-    # # for s, t in zip(S, theta):
-    # #     mat = bracket(s)
-    # #     print(s)
-    # #     print(t)
-    # #     print(mat)
-    # #     brac = expm(mat * t)
-    # #     mats.append(brac)
-    # for i in range(len(theta)):
-    #     print(S[i])
-    #     print(theta[i][0])
-    #     brac = bracket(S[i])
-    #     print(brac)
-    #     brac = expm(brac*theta[i][0])
-    #     mats.append(brac)
-    #
-    # return mats
-    return [expm(bracket(s) * t) for s, t in zip(S, theta)]
+    return [expm(skew4(S[:,i]) * theta[i]) for i in range(S.shape[1])]
 
 def evalT(S, theta, M):
     """
@@ -163,65 +148,111 @@ def evalJ(S, theta):
     :returns: A 6xN matrix representing the space Jacobian of the robot with the given screw axes at the given joint vars
     """
     T = toTs(S, theta)
-    J = [S[0]]
-    for i in range(1, len(S)):
+    J = S[:,[0]]
+    for i in range(1, S.shape[1]):
         col = T[0]
         for j in range(1, i):
             col = col.dot(T[j])
-        J.append(adj_T(col).dot(S[i]))
-    return np.hstack(J)
-'''
-Collision Detection Function
-S -- list of 6x1 screw matricies for N joints
-initial_points -- 3xN matrix of q values where N is the number of joints
-points -- 3xN matrix of q values after joints have moved
-r -- radius of each sphere
-theta -- The position the joints will move to in radians
-'''
-def collisionChecker(S, initial_points, points, r, theta):
-    for i in range(len(theta)):
-        p_initial = np.array([
-        [initial_points[0][i]],
-        [initial_points[1][i]],
-        [initial_points[2][i]],
-        [1]
-        ])
-
-        temp = np.identity(4)
-
-        for j in range(i+1):
-            temp = np.dot(temp, expm(bracket(S[j]) * theta[j]))
-
-        p_final = np.dot(temp, p_initial)
-
-        points[0].append(p_final[0][0])
-        points[1].append(p_final[1][0])
-        points[2].append(p_final[2][0])
-
-    p = np.block([
-    [np.reshape(np.asarray(points[0]),(1,len(theta)+2))],
-    [np.reshape(np.asarray(points[1]),(1,len(theta)+2))],
-    [np.reshape(np.asarray(points[2]),(1,len(theta)+2))]
-    ])
-
-    for i in range(len(theta) + 2):
-        point1 = np.array([
-        [p[0][i]],
-        [p[1][i]],
-        [p[2][i]]
-        ])
-
-        for j in range(len(theta) + 2 - i):
-            if(i == i+j):
-                continue
-
-            point2 = np.array([
-            [p[0][j+i]],
-            [p[1][j+i]],
-            [p[2][j+i]]
-            ])
-
-            if(norm(point1 - point2) <= r*2):
-                return 1
-
-    return 0
+        newterm = adj_T(col).dot(S[:,[i]])
+        J = np.concatenate((J,newterm),axis=1)
+    return J
+
+##
+## The following code will (probably) not be included with Exam 4
+##
+
+def findIK(endT, S, M, theta=None, max_iter=100, max_err = 0.001, mu=0.05):
+    """
+    Basically Inverse Kinematics
+    Uses Newton's method to find joint vars to reach a given pose for a given robot. Returns joint positions and
+    the error. endT, S, and M should be provided in the space frame. Stop condiditons are when the final pose is less than a given
+    twist norm from the desired end pose or a maximum number of iterations are reached.
+    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)
+    TODO: Improve internal type flexibilty of input types
+    :param endT: the desired end pose of the end effector
+    :param S: a python list of 6x1 screw axes in the space frame
+    :param M: the pose of the end effector when the robot is at the zero position
+    :param theta: Optional - An initial guess of theta. If not provided, zeros are used. Should be a Nx1 numpy matrix
+    :param max_iter: Optional - The maximum number of iterations of newtons method for error to fall below max_err. Default is 10
+    :param max_err: Optional - The maximum error to determine the end of iterations before max_iter is reached. Default is 0.001 and should be good for PL/quizes
+    :param mu: The normalizing coefficient (?) when computing the pseudo-inverse of the jacobian. Default is 0.05
+    :returns: A tuple where the first element is an Nx1 numpy array of joint variables where the algorithm ended. Second
+              element is the norm of the twist required to take the found pose to the desired pose. Essentially the error that PL checks against.
+    """
+    if  theta is None:
+        theta = np.zeros((S.shape[1],1))
+    V = np.ones((6,1))
+    while np.linalg.norm(V) > max_err and max_iter > 0:
+        curr_pose = evalT(S, theta, M)
+        V = inv_bracket(logm(endT.dot(inv(curr_pose))))
+        J = evalJ(S, theta)
+        pinv = inv(J.transpose().dot(J) + mu*np.identity(S.shape[1])).dot(J.transpose())
+        thetadot = pinv.dot(V)
+        theta = theta + thetadot
+        max_iter -= 1;
+    return (theta, np.linalg.norm(V))
+
+
+
+
+def Dist3D(p1, p2):
+    """Euclidean distance function for three dimensions. Assumes that p1 and p2 are
+    column matrices in the order of px, py, pz respectively.
+    """
+    return math.sqrt((p1[0]-p2[0])**2 + (p1[1]-p2[1])**2 + (p1[2]-p2[2])**2)
+
+def finalpos(S, theta, start):
+    """ This function finds the final position of all joints. Solution to 5.1.3.
+    Parameters:
+    S: the 6x6 matrix of the spatial screw axes for all joints.
+    theta: a 6x1 matrix representing a certain configuration of the robot.
+    start: a 3x8 where the i'th column represents the initial position of the ith joint in terms of frame 0. """
+
+    position = start[:,:2]
+
+    for i in range(2,len(start[0])):
+        M = np.identity(4)
+        M[0,3] = start[0, i]
+        M[1,3] = start[1, i]
+        M[2,3] = start[2, i]
+        T = evalT(S[:, 0:i-1], theta[0:i-1], M)
+        position = np.concatenate((position, T[:3, 3:4]),axis=1)
+
+    return position
+
+def checkcollision(p, r, q, s):
+    """checkcollision is the solution to 5.1.2.
+    Parameters:
+    p: a 3xn matrix which represents the positions of all spheres.
+    r: a 1xn matrix representing the radii of every sphere.
+    q: a 3x1 matrix representing the position of the final sphere.
+    s: the radius of the final sphere"""
+
+    c = np.zeros(r.shape[1])
+    for i in range(p.shape[1]):
+        dis = Dist3D(q, p[:,[i]])
+        if(dis < (r[:,i]+s)):
+            c[i]+=1
+    return c
+
+
+def checkselfcollision(S, theta, start, r):
+    """checkselfcollisions checks whether a certain series of configurations causes a self collision. Solution to 5.1.5
+    S: a 6x6 matrix of the spatial screw axes for all joints.
+    theta: a 6xn matrix of configurations. Notice that the ith column of this matrix is a 6x1 theta matrix representing a certain configuration of the robot
+    start: a 3x8 matrix of joint starting positions in frame 0.
+    r: a given radius for surrounding spheres"""
+
+    c = np.zeros(theta.shape[1])
+    for i in range(theta.shape[1]):
+        t = theta[:,[i]]
+        jointpos = finalpos(S,t,start)
+        for j in range(jointpos.shape[1]):
+            joint2check = jointpos[:,[j]]
+            for k in range(jointpos.shape[1]):
+                if(k != j):
+                    if(Dist3D(joint2check, jointpos[:,[k]])< (r+r)):
+                        c[i] = 1
+
+    return c

FinalProject/Check_Point_5/usefulFunctions.py

 import numpy as np
-from scipy.linalg import expm
 import math
+import random
+from scipy.linalg import expm, logm
+from numpy.linalg import inv, multi_dot, norm
+from ece470_lib import inv_bracket, bracket, findIK, checkselfcollision, finalpos
 # def bracket_4x4(v):
 # return np.block([ [bracket_3x3(v[0:3, :]), v[3:6, :]],[ np.zeros((1,4))] ])
 
 
+class Node(object):
+    def __init__(self):
+        self.theta = None
+        self.next = None
+
+#-1 returned means the node arguement passed in was NULL
+def find_closest_node(node, theta_target):
+    distance = -1
+    closest_node = node
+
+    while (node is not None):
+        if(norm(node.theta - theta_target) < distance or distance is -1):
+            distance = norm(node.theta - theta_target)
+            closest_node = node
+
+        node = node.next
+
+    return closest_node
+
+#None means no path exists
+def get_complete_path(start, end):
+
+    intersecting_theta = None
+    curr_node_s = start
+
+    while curr_node_s is not None:
+        curr_node_e = end
+        flag = False
+        while curr_node_e is not None:
+            if(norm(curr_node_s.theta - curr_node_e.theta) == 0):
+                flag = True
+                intersecting_theta = curr_node_s.theta
+                break
+            curr_node_e = curr_node_e.next
+
+        if(flag):
+            break
+
+        curr_node_s = curr_node_s.next
+
+    #No shared theta so return None
+    if(intersecting_theta is None):
+        return intersecting_theta
+
+    print("Hi")
+
+    q = list()
+    backtrack = dict()
+
+    #puts thetas from start till the shared theta, including the shared theta
+    curr_node_s = start
+    while curr_node_s is not None:
+        q.append(curr_node_s.theta)
+
+        if(norm(curr_node_s.theta - intersecting_theta) == 0):
+            break
+
+        curr_node_s = curr_node_s.next
+
+    #puts the thetas from the shared theta till the end, not including the shared data
+    curr_node_e = end
+    flag = False
+    while curr_node_e is not None:
+
+        if(flag):
+            q.append(curr_node_e.theta)
+
+        if(norm(curr_node_e.theta - intersecting_theta) == 0):
+            flag = True
+
+        curr_node_e = curr_node_e.next
+
+    return q
+
+def self_collision_detector(p, r):
+
+    for i in range(len(p[0])):
+        point1 = np.array([
+        [p[0][i]],
+        [p[1][i]],
+        [p[2][i]]
+        ])
+
+        for j in range((len(p[0])) - i):
+            if(i == i+j):
+                continue
+
+            point2 = np.array([
+            [p[0][j+i]],
+            [p[1][j+i]],
+            [p[2][j+i]]
+            ])
+            # print(i,j)
+            if(norm(point1 - point2) <= r[i]+r[i+j]):
+                return 1
+
+    return 0
+
+def other_collision_detector(robot, others, r_robot, r_others):
+
+    for i in range(len(robot[0])):
+        point1 = np.array([
+        [robot[0][i]],
+        [robot[1][i]],
+        [robot[2][i]]
+        ])
+
+        for j in range(len(others[0])):
+
+            point2 = np.array([
+            [others[0][j]],
+            [others[1][j]],
+            [others[2][j]]
+            ])
+
+            # print(norm(point1 - point2), r_robot[i]+r_others[j])
+            if(norm(point1 - point2) <= r_robot[i]+r_others[j]):
+                return 1
+
+    return 0
+
+def point2point_collision_detector(theta_a, theta_b, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size):
+
+    s = step_size
+
+    while s <= 1:
+
+        theta = (1-s)*theta_a + s*theta_b
+
+        final_pos = finalpos(S, theta, p_robot)
+        self_colision_flag = self_collision_detector(final_pos, r_robot[0])
+        other_colision_flag = other_collision_detector(final_pos, p_obstacle, r_robot[0], r_obstacle[0])
+
+        if(self_colision_flag or other_colision_flag):
+            return s
+
+        s = s + step_size
+
+    return 0
 #euler to scew_axis
 def euler_to_a(a,b,g):
     R = euler2mat(a,b,g)