import pygame import math import time import random import numpy as np from pygame.locals import* # the units are (cm or pixel), and (degree) # The coordinate of PyGame is: # ---> x # ' # ' # y # the upperleft pixel is (0,0) # Note: use function PointDisplay() to convert to PyGame coordinate # use function PointActual() to convert to the actual coordinate # The size of the display window DISP_WIDTH = 500 DISP_HEIGHT = 300 class ArmPlanning(): def __init__(self, length, angles, colors): pygame.init() self.screen = pygame.display.set_mode((DISP_WIDTH, DISP_HEIGHT)) self.screen.fill((255,255,255)) pygame.display.set_caption('Arm Planning--Joseph') self.length = length self.angles = angles #joint angles self.colors = colors #the color for each link self.dof = len(angles) #degree of freedom self.points = [self.PointDisplay(0, 0)]*(self.dof+1) # shift to the center self.obstacles = [] self.lines = [pygame.rect.Rect(0,0,0,0)] * self.dof self.obstacle_rects = [] self.hit_ground = False self.goals = [] self.goals_rects = [] self.goalPos = [] self.obsPos = [] self.collisionLink = 0 # Convert a point, (x,y) Tuple, to PyGame coordinate def PointDisplay(self, x, y): return (DISP_WIDTH/2 + x, DISP_HEIGHT - y) # Convert a point, (x,y) Tuple, to actual coordinate def PointActual(self, x, y): return (x - DISP_WIDTH/2, DISP_HEIGHT - y) # update the joint angels with the new values for joint indices in the list # e.g. updateArm([new_angle2, new_angle3], [1,2]) def updateArm(self, angles, indices): for i in range(0, len(angles)): self.angles[indices[i]] = angles[i] def draw(self): self.screen.fill ((255, 255, 255)) #clean the screen first # draw the whole arm for i in range(0, self.dof): real_point = self.PointActual(self.points[i][0], self.points[i][1]) # the angle for each joint is the sum of all the previous joints' angles self.points[i+1] = self.PointDisplay(real_point[0]+self.length[i]*math.cos(math.radians(sum(self.angles[:i+1]))), real_point[1]+self.length[i]*math.sin(math.radians(sum(self.angles[:i+1])))) self.lines[i] = pygame.draw.line(self.screen, colors[i], self.points[i], self.points[i+1], 5) #check if it collides with ground if self.points[i+1][1] > DISP_HEIGHT: self.hit_ground = True # draw the obstacles for i in range(0, len(self.obstacles)): self.obstacle_rects[i] = pygame.draw.circle(self.screen, self.obstacles[i][0], self.obsPos, self.obstacles[i][2], 0) # draw the goals for i in range(0, len(self.goals)): self.goals_rects[i] = pygame.draw.circle(self.screen, self.goals[i][0], self.goalPos, self.goals[i][2], 0) # get the mouse input for event in pygame.event.get(): if event.type == pygame.MOUSEBUTTONDOWN: if event.button == 1: # left click self.goalPos = event.pos if event.button == 3: self.obsPos = event.pos pygame.display.update() def addObstacle(self, color, loc, radius): obs = (color, self.PointDisplay(loc[0],loc[1]), radius) self.obsPos = self.PointDisplay(loc[0],loc[1]) self.obstacles.append(obs) self.obstacle_rects.append(pygame.rect.Rect(0,0,0,0)) def addGoals(self, color, loc, radius): obs = (color, self.PointDisplay(loc[0],loc[1]), radius) self.goalPos = self.PointDisplay(loc[0],loc[1]) self.goals.append(obs) self.goals_rects.append(pygame.rect.Rect(0,0,0,0)) # check each link if it collides with ground or obstacles def checkCollision(self): for i in range(0, self.dof): if (self.lines[i].collidelist(self.obstacle_rects) != -1): self.collisionLink = i+1 return True return False # Forward Kinematics of the arm system. NOTE: the current version can only be applied to 3 links. # e.g. forwardK(self, angle), return is the (x, y) coordinate of the end point. def forwardK(self, angles): # define the transformation matrix from link1 to the base # in the form of the T0_1 (from link 1 to base 0) T1_0 = np.matrix([[math.cos(math.radians(angles[0])), -math.sin(math.radians(angles[0])), 0, 0], [math.sin(math.radians(angles[0])), math.cos(math.radians(angles[0])), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T2_1 = np.matrix([[math.cos(math.radians(angles[1])), -math.sin(math.radians(angles[1])), 0, self.length[0]], [math.sin(math.radians(angles[1])), math.cos(math.radians(angles[1])), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T3_2 = np.matrix([[math.cos(math.radians(angles[2])), -math.sin(math.radians(angles[2])), 0, self.length[1]], [math.sin(math.radians(angles[2])), math.cos(math.radians(angles[2])), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T3_0 = T1_0*T2_1*T3_2 # Define the coordinate vector of the end point in link 3 frame. # in the form of [x, y, z, 1]', it's column vector pEnd =[[self.length[2]], [0], [0], [1]] pCoordinates = T3_0*pEnd return pCoordinates # display the coordinates of the end point. def drawText(self, point): font = pygame.font.Font(None, 22) text = font.render(str(point), 1, (10, 10, 10)) textpos = text.get_rect() textpos.centerx = self.points[3][0] textpos.centery = self.points[3][1] self.screen.blit(text, textpos) pygame.display.update() # calculate the jacobian matrix of the end point. # this method is the expansion of the forwardK. def jacobian(self, angles): T1_0 = np.matrix([[math.cos(math.radians(angles[0])), -math.sin(math.radians(angles[0])), 0, 0], [math.sin(math.radians(angles[0])), math.cos(math.radians(angles[0])), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T2_1 = np.matrix([[math.cos(math.radians(angles[1])), -math.sin(math.radians(angles[1])), 0, self.length[0]], [math.sin(math.radians(angles[1])), math.cos(math.radians(angles[1])), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T3_2 = np.matrix([[math.cos(math.radians(angles[2])), -math.sin(math.radians(angles[2])), 0, self.length[1]], [math.sin(math.radians(angles[2])), math.cos(math.radians(angles[2])), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T3_0 = T1_0*T2_1*T3_2 # Define the coordinate vector of the end point in link 3 frame. # in the form of [x, y, z, 1]', it's column vector pEnd =[[self.length[2]], [0], [0], [1]] pEndCoordinates = T3_0*pEnd # Define the coordinate vector of the joint 1, 2, 3 in link 1, 2, 3 frame. # and transform them to the base frame 0. pJoint1 = [[0], [0], [0], [1]] pJoint1Coordinates = T1_0*pJoint1 pJoint2 = [[0], [0], [0], [1]] pJoint2Coordinates = T1_0*T2_1*pJoint2 pJoint3 = [[0], [0], [0], [1]] pJoint3Coordinates = T1_0*T2_1*T3_2*pJoint3 # Define the positioins of the end point, joint1, joint2, and joint3 in world space # in the form of (x, y, z) # the [*][*] indice is for extracting the element from the matrix by using type cast end = [float(pEndCoordinates[0][0]), float(pEndCoordinates[1][0]), float(pEndCoordinates[2][0])] joint1 = [float(pJoint1Coordinates[0][0]), float(pJoint1Coordinates[1][0]), float(pJoint1Coordinates[2][0])] joint2 = [float(pJoint2Coordinates[0][0]), float(pJoint2Coordinates[1][0]), float(pJoint2Coordinates[2][0])] joint3 = [float(pJoint3Coordinates[0][0]), float(pJoint3Coordinates[1][0]), float(pJoint3Coordinates[2][0])] # define the unit length rotation axis in word space unitAxis = [0, 0, 1] # calculate the individual column of the Jacobian matrix jacobianColumn1 = np.cross(unitAxis, np.subtract(end, joint1)) jacobianColumn2 = np.cross(unitAxis, np.subtract(end, joint2)) jacobianColumn3 = np.cross(unitAxis, np.subtract(end, joint3)) # Note: the last row of the Jaocobian matrix should always be zero in this case # since the z-cooridnate of the end point won't change with any of the three joint. jacobian = np.transpose([jacobianColumn1, jacobianColumn2, jacobianColumn3]) return jacobian def inverseK(self, init_angles, goal): initialEndPos = arm.forwardK(init_angles) iterativeEndPos = [float(initialEndPos[0]), float(initialEndPos[1]), float(initialEndPos[2])] iterativeAngles = [init_angles[0], init_angles[1], init_angles[2]] # choose the beta from 0 to 10 # beta[0], beta[1], and beta[2] are for joint1, joint2, and joint3. # Increase the beta value can speed up the specific joint. beta = [8e-4, 2e-3, 6e-3] jacobianMatrix = self.jacobian(iterativeAngles) endPosDiff = np.subtract(goal, iterativeEndPos) epsilon = 1e-1 # Inverse Kinematic of the arm has been added into the following lines. # while (max(abs(endPosDiff)) > epsilon) and not self.checkCollision() and not self.hit_ground: while (max(abs(endPosDiff)) > epsilon): # check collision if self.checkCollision(): print "collision is happening at", self.collisionLink if (self.collisionLink == 1): beta = [-8e-4, -1e-4, -1e-4] elif (self.collisionLink == 2): beta = [-1e-4, -8e-4, -1e-4] else: beta = [-1e-4, -1e-4, -8e-4] else: print "no collision" # update the joint angles tempDeltaAngles = np.dot(np.transpose(jacobianMatrix), endPosDiff) iterativeAngles = np.add(iterativeAngles, np.multiply(beta, tempDeltaAngles)) # update the Jacobian with the new joint values: jacobianMatrix = self.jacobian(iterativeAngles) # update the position of the end effector: tempEndPos = self.forwardK([float(iterativeAngles[0]), float(iterativeAngles[1]), float(iterativeAngles[2])]) iterativeEndPos = [float(tempEndPos[0]), float(tempEndPos[1]), float(tempEndPos[2])] # update the goal point. tempGoal = self.PointActual(self.goalPos[0], self.goalPos[1]) goal = [tempGoal[0], tempGoal[1], 0] # update the distance between the goal and the current position. endPosDiff = np.subtract(goal, iterativeEndPos) ang1 = iterativeAngles[0] ang2 = iterativeAngles[1] ang3 = iterativeAngles[2] self.updateArm([ang1, ang2, ang3], [0, 1, 2]) self.draw() time.sleep(0.02) # resume the pre-set beta = [8e-5, 2e-4, 5e-4] beta = [8e-5, 2e-4, 5e-4] # the following two lines can be used to check the forward kinematics of the arm. # draw the (x, y) end point coordinates. coordinates = self.forwardK([ang1, ang2, ang3]) point = [int(coordinates[0]), int(coordinates[1])] self.drawText(point) # the following two lines can be used to check the Jacobian Matrix. jacobianMatrix = self.jacobian([ang1, ang2, ang3]) #print jacobianMatrix print "goal reached" # if self.checkCollision() or self.hit_ground: # print "Collide with Obstacles" # else: # print "goal reached" if __name__ == "__main__": length = [100, 100, 50] #the length of links init_angles = [10, -10, 30] #initial joint angles colors = [(0,0,255), (0,255,80), (255,0,0)] #the color for each link arm = ArmPlanning(length, init_angles, colors) init_goal = [0, 0, 0] init_obs = [-25, 75] arm.addGoals( (0,255,127), (init_goal[0], init_goal[1]), int(6) ) arm.addObstacle( (100,25,60), init_obs, int(12.5) ) # the following lines are for initialization of the Inverse Kinematics of the arm: # goal is in the form of [x, y, z] arm.inverseK(init_angles, init_goal) arm.draw() ### ToDo: Add the planning code obstacle avoiding code into the code.