Particle swarm optimization for inverse kinematics

README.md

Inverse Kinematics using PSO

Solution for the inverse kinematics problem using Particle Swarm Optimization (PSO)
Blog Post: https://ljvmiranda921.github.io/notebook/2017/02/04/inverse-kinematics-pso/

Inverse Kinematics (IK) is one of the most challenging problems in robotics. The problem involves finding an optimal pose for a manipulator given the position of the end-tip effector. This can be seen in contrast with forward kinematics, where the end-tip position is sought given the pose or joint configuration. Normally, this position is expressed as a point in a coordinate system (e.g., in a Cartesian system, it is a point comprising of x, y, and z coordinates.). On the other hand, the manipulator's pose can be expressed as the collection of joint variables that can describe the angle of bending or twist (in revolute joints) or length of extension (in prismatic joints).

IK is particularly difficult because multiple solutions can arise. Intuitively, a robotic arm can have multiple ways of reaching through a certain point. Moreover, calculation can prove to be very difficult. Simple solutions can be found for 3-DOF manipulators, but a 6-DOF or higher can be very challenging to solve algebraically.

License

All public gists https://gist.github.com/ljvmiranda921
Copyright 2017, Lester James V. Miranda
MIT License, http://www.opensource.org/licenses/mit-license.php

particleSwarmOptimization.py

# -*- coding: utf-8 -*-
"""
Module: particleSwarmOptimization.py
Author: L. J. Miranda
"""

import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt


class ParticleSwarmOptimization(object):
    
    def __init__(self):
        pass
    
    def runOptimizer(self, objectiveFcn, target, params, iterations, options, threshold, animate):
        """
        Runs the particle swarm optimization (PSO) algorithm in order to minimize
        the objective function given different parameters.
        
        Inputs:
            - objectiveFcn: the function to be optimized (type -> method).
            - target: target coordinate used as basis.
            - params: np array of the initial parameters to be optimized.
            - iterations: number of iterations that PSO will run. (default = 1000)
            - options: a list containing the parameters for PSO behavior, 
                      in this case, options = [swarmSize c1 c2 w epsilon v_initial]
                      [0] swarmSize - the number of particles or swarm size
                      [1] c1 - cognitive parameter
                      [2] c2 - social parameter
                      [3] w  - inertia weight
                      [4] epsilon - the spread of the particles.
                      [5] v_initial - initial velocity
            - threshold: set the threshold when to exit the optimizer. 
            - animate: trigger to show animation of particles, input 1 to animate. (default = 0)
            
        Returns:
            - final_params: returns an np-array of the final parameters after the given number of iterations.
            - cost: returns the final cost. [TODO]
            - cost_hist: returns the cost history given the number of iterations. [TODO]
        """
        
        ################################################################################
        #                             INITIALIZE MATRICES                              #
        ################################################################################
        size_params = params.size
        
        # Current Positions of the Swarm
        currentPos = np.random.uniform(0,1,[options[0], size_params]) * options[4] + params.T
        
        # Initialize Personal Best Position
        pBestPos = currentPos;

        # Initialize Global Best Position
        J = []
        for i in range(options[0]):
            temp = objectiveFcn(currentPos[i,:], target)
            J.append(temp[1])
        
        minIndex = np.argmin(J)
        gBestPos = currentPos[minIndex,:]
            
        # Initialize Velocity Matrix
        velocityMatrix = options[5] * np.ones([options[0], size_params])
        
        # Initialize Record Vectors
        J_hist = []
        pBestRecord = []
        cBestRecord = []

        
        
        ################################################################################
        #                             PSO ALGORITHM                                    #   
        ################################################################################
        
        for i in range(iterations):
            # Update the pbest and gbest
            for particle in range(options[0]):
                # Set the personal best option
                costParticle = objectiveFcn(currentPos[particle,:],target)
                costpBest = objectiveFcn(pBestPos[particle,:],target)
                cBestRecord.append(costParticle[1])
                if costParticle[1] < costpBest[1]:
                    pBestPos[particle,:] = currentPos[particle,:]
                costpBest2 = objectiveFcn(pBestPos[particle,:],target)
                pBestRecord.append(costpBest2[1])
                # Set the global best option
                costgBest = objectiveFcn(gBestPos,target)
                if costParticle[1] < costgBest[1]:
                    gBestPos = currentPos[particle,:]
            


            #------------------- HELPER FUNCTIONS --------------------------#            
            # Helper functions for display
            cost = objectiveFcn(gBestPos,target)
            self.consoleDisplay(i,cost[1])    
            
            temp = [i, cost[1], np.mean(pBestRecord)]
            J_hist.append(temp)
            
            # Helper functions for animation
            if animate==1:
                swarmPos = []
                for n in range(options[0]):
                    pos = objectiveFcn(currentPos[n,:], target)
                    swarmPos.append(pos[0])
                
                swarmPos = np.asarray(swarmPos)
                xVec = swarmPos[:,0]
                yVec = swarmPos[:,1]
                zVec = swarmPos[:,2]
                swarmPos = swarmPos.tolist()
                fig = plt.figure()
                self.plotParticles3D(fig,xVec,yVec,zVec,i)
            
            # If the cost is greater than the threshold
            if cost[1] > threshold:
                # Update the velocities and position.
                for particle in range(options[0]):
                    # Update velocity
                    cognitive = (options[1] * np.random.uniform(0,1,[1,6])) * (pBestPos[particle,:] - currentPos[particle,:])
                    social = (options[2] * np.random.uniform(0,1,[1,6])) * (gBestPos - currentPos[particle,:])
                    velocityMatrix[particle,:] = (options[3] * velocityMatrix[particle,:]) + cognitive + social
                    # Update position
                    currentPos[particle,:] = currentPos[particle,:] + velocityMatrix[particle,:]

            else:
                # Return the parameters if the cost is less than the threshold
                final_params = gBestPos
                return final_params, J_hist
                
        # Return the parameters if the iterations finished and the cost didn't go below threshold            
        final_params = gBestPos
        return final_params, J_hist
            

    def consoleDisplay(self, i, cost):      
        statement = 'Iteration: ' + str(i) + ' | Cost: ' + str(cost)
        print(statement)
    
    def plotParticles3D(self,fig,x,y,z,i):
        fig.clear()
        fig.suptitle('Particle End-Tip Position', fontsize=20)
        ax = fig.add_subplot(111, projection='3d')
        ax.scatter(x, y, z, c='r', marker='o')
        ax.set_xlabel('X axis')
        ax.set_ylabel('Y axis')
        ax.set_zlabel('Z axis')
        ax.set_xlim([-5, 5])
        ax.set_ylim([-5, 5])
        ax.set_zlim([0, 5])
        save_format = 'png'
        plt.savefig('figure_' + str(i) + '.' + save_format, dpi=300)
        plt.show()

roboticArm.py

# -*- coding: utf-8 -*-
"""
Module: roboticArm.py
Author: L. J. Miranda
"""

import numpy as np
import math

#import particleSwarmOptimization

class RoboticArm(object):
    
    def __init__(self, d1, d2, d4, d6):
        self.d1 = d1
        self.d2 = d2
        self.d4 = d4
        self.d6 = d6
    
    def forwardKinematics(self, params, target):
        """
        Computes for the end-tip position of the Robotic Arm with respect to the joint parameters
        
        Inputs:
            - params: Joint variables that can be found in the manipulator (all angles should be in radians):
                [0]: Theta1    [3]: Theta4
                [1]: Theta2    [4]: Theta5
                [2]: D3        [5]: Theta6
                
        Returns:
            - pos: end-tip position of the manipulator (x,y,z coordinates).
            - cost: the cost of these params end-tip position with respect to the target.
        """
        t_00 = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
        t_01 = self.getTransformMatrix(params[0], self.d2, 0, -math.pi/2)
        t_12 = self.getTransformMatrix(params[1], self.d2,0,-math.pi/2)
        t_23 = self.getTransformMatrix(0,params[2], 0, -math.pi/2)
        t_34 = self.getTransformMatrix(params[3], self.d4, 0, -math.pi/2)
        t_45 = self.getTransformMatrix(params[4], 0, 0, math.pi/2)
        t_56 = self.getTransformMatrix(params[5], self.d6,0 ,0)
        
        Etip = t_00.dot(t_01).dot(t_12).dot(t_23).dot(t_34).dot(t_45).dot(t_56)
        pos = np.array([Etip[0,3],Etip[1,3],Etip[2,3]])
        cost = self.L2Distance(pos, target)
        returnMessage = [pos,cost]
        return returnMessage
        
        
    def getTransformMatrix(self, theta, d, a, alpha):
        """
        Gets the transformation matrix using the Denavit-Hartenberg method.
        
        Inputs:
            - theta: joint angle
            - d: link offset
            - a: link length
            - alpha: link twist
        
        Returns:
            - T: a numpy array transformation matrix
        """
        T = np.array([[math.cos(theta), -math.sin(theta)*math.cos(alpha), math.sin(theta)*math.sin(alpha), a * math.cos(theta)],
                     [math.sin(theta), math.cos(theta)*math.cos(alpha), -math.cos(theta)*math.sin(alpha), a*math.sin(theta)],
                     [0, math.sin(alpha), math.cos(alpha), d],
                     [0,0,0,1]])
        
        return T
                
        
    def L2Distance(self,query,target):
        x_dist = (target[0] - query[0])**2
        y_dist = (target[1] - query[1])**2
        z_dist = (target[2] - query[2])**2
        d = math.sqrt(x_dist + y_dist + z_dist)
        return d

may i know the main function code which runs this? do we just pass the object to the pyswarm class?