Skip to content

Instantly share code, notes, and snippets.

@ljvmiranda921
Last active Dec 21, 2021
Embed
What would you like to do?
Particle swarm optimization for inverse kinematics

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

# -*- 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()
# -*- 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
@Aishkrish18
Copy link

Aishkrish18 commented Jul 24, 2020

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment