# python code for closed form inverse kinematics solution

I solved the inverse kinematics of a 4DOF robotic arm (which is not planar) and I wrote this code bellow in python. The coordinates for each joint is:

I considered the first three joints as a wrist and solved my inverse problem. For solving my inverse problem I used the DH parameters and by multiplying matrix I found the matrix that describes the end-effector(e) in the base frame(B)(forward kinematics). Also, I considered the first three joints as a wrist. My inverse kinematic method is similar to this onehttp://www.golems.org/papers/OFlaherty13-hubo-kinematics-techreport.pdf But the allocation of coordinates in my solution is different from that as you see in my picture. This is my python code for inverse kinematics:

import numpy as np
import math
import cmath

# length parameters are in cm
La1=1
La2=1
La3=1
x=0
i=0
with open( "endefector.txt") as f:
matrix=np.zeros((4,4))
data=[[]]
for line in f:
matrix[i]=(list(map(float,line.split(',') [:4])))
i=i+1
print(matrix)


# here i need the inverse of the above matrix for solving inverse kinematics so i reversed it. R= matrix[0:3,0:3] RT=np.zeros((3,3)) for i in range( len(R)): for j in range(len(R[0])): RT[j][i]=R[i][j]

RT=-1*RT

pos=matrix[0:3,3:4]
Tpos=np.dot(RT,pos)
Px=Tpos[0] # this is "the input x position" of endeffector for my inverse
kinematic solution
print(Px)
Py=Tpos[1]
print(Py)
Pz=Tpos[2]
print(Pz)

sintetha2=(La2*La2+La1*La1-Pz*Pz-Py*Py-(Px+La3)*(Px+La3))/(2*La1*La2)
tetha2= math.atan2(sintetha2,np.sqrt(1-sintetha2*sintetha2))
print(tetha2)


this is just a part of my code for solving tetha2 then I wanted to validate the solution for theta2.so I assume the other joint variables(theta)are equal to zero and in a loop I increased the amount of theta2 and put it in forward kinematics and achieved the desired matrix (the matrix that describes the end-effector in base frame) and used it for inverse solution as it used in the hubo kinematics too. Then I wanted to compare the input tetha2 (it is mentioned in the second part of code as th2) and the amount of tetha2 that comes from the inverse solution (it is mentioned as tetha2). I printed the difference between these two values at the end of my python code. Here is the python code for testing th2 in a given trajectory:

import numpy as np
import cmath
import math

La1=1
La2=1
La3=1

pi=math.pi #3.141592653589793238

#initial theta
th1=0.0000
th2=0.0000
th3=0.0000
th4=0.0000
while th2 >= -1*pi:

TBE=[[np.sin(th4)*(np.cos(th1)*np.sin(th3)-np.sin(th1)*np.cos(th2)*np.cos(th3))+np.sin(th1)*np.sin(th2)*np.sin(th4),np.cos(th4)*(np.cos(th1)*np.cos(th3)-np.sin(th1)*np.sin(th2)*np.sin(th3))-np.sin(th1)*np.cos(th2)*np.sin(th4),np.sin(th1)*np.sin(th2)*np.cos(th3)+np.cos(th1)*np.sin(th3),La3*np.sin(th4)*(np.cos(th1)*np.sin(th3)-np.sin(th1)*np.cos(th2)*np.cos(th3))+np.sin(th1)*np.sin(th2)*np.sin(th4) +La2*np.sin(th1)*np.cos(th2)],[np.sin(th4)*np.cos(th2)*np.sin(th3)+np.sin(th2)*np.cos(th4),np.cos(th2)*np.sin(th3)*np.cos(th4)-np.sin(th2)*np.sin(th4),-1*np.cos(th2)*np.cos(th3),La3*np.sin(th4)*np.cos(th2)*np.sin(th3)+np.sin(th2)*np.cos(th4)+La2*np.sin(th2)-La1],[np.sin(th4)*(np.cos(th1)*np.sin(th2)*np.sin(th3)+np.sin(th1)*np.cos(th3))-np.cos(th1)*np.cos(th2)*np.cos(th4),np.cos(th4)*(np.cos(th1)*np.sin(th2)*np.sin(th3)+np.sin(th1)*np.cos(th3))+np.cos(th1)*np.cos(th2)*np.sin(th4),np.sin(th1)*np.sin(th3)-np.cos(th1)*np.sin(th2)*np.cos(th3),La3*np.sin(th4)*(np.cos(th1)*np.sin(th2)*np.sin(th3)+np.sin(th1)*np.cos(th3))-np.cos(th1)*np.cos(th2)*np.cos(th4)-La2*np.cos(th1)*np.cos(th2)],[0,0,0,1]]
T=np.reshape(TBE,(4,4))

R=T[0:3,0:3]
RT=np.zeros((3,3))
for i in range(len(R)):
for j in range(len(R[0])):
RT[j][i]=R[i][j]

orientation=-1*RT
pos=T[0:3,3:4]

Tpos=np.dot(orientation,pos)
Px=Tpos[0]
Py=Tpos[1]
Pz=Tpos[2]

sintetha2=(La2*La2+La1*La1-Pz*Pz-Py*Py-(Px+La3)*(Px+La3))/(2*La1*La2)
tetha2= math.atan2(sintetha2,np.sqrt(1-sintetha2*sintetha2))
tetha22=-1*math.atan2(sintetha2,-1*np.sqrt(1-sintetha2*sintetha2))
print('th2',th2)
print('tetha2',tetha2)
print('tetha22',tetha22)

print('th2-tetha2=',th2-tetha2)
print('th2-tetha22=',th2-tetha22)
print(th2)

th2 =round(th2-0.1,2)


but as I ran run this code, the difference of the values of th2 and tetha2 as th2 gets near of 1.57, increase accurately and I don't know why. I need the difference between these two to be very small in the whole joint range. I will be so appreciated if someone helps me to find the problem and fix it.

• Welcome to Robotics, forough zamani. As it stands, it's not clear what exactly you're asking. You said, the difference of the values of tetha2 as it gets near of 1.57, increase accurately and that you need the difference of these two to be very [small]. The difference between tetha2 and what? What is the difference you're getting? What method are you trying to implement with your Python code? – Chuck Nov 27 '18 at 15:43
• In the top (and bottom) code, it looks like you're maybe trying to find sin(theta2) with... law of sines? You haven't shown what Px Py Pz are in your diagram, though, and there's no square root anywhere, so I'm not seeing how you're finding the length of a "hypotenuse." In the bottom code block, you have a while loop that seems like you're trying to iterate through values of th2, but it's in Python and you don't have any white space to group all of your code into that while loop. – Chuck Nov 27 '18 at 16:05
• Please edit your question to include more information on the methods you're trying to implement and how you arrived at the formulas you're using. For example, I suspect your TEB equation might be incorrect because I think you don't get the correct transform from E to B if all theta values are zero. So again, please document how you arrived at the code you're using and we can help troubleshoot if this is a formulation error or an implementation error. – Chuck Nov 27 '18 at 16:10
• Thanks for spending your time on my problem.I edited the question and explained more but i dot know why i cant upload the right picture of my robotic arm. – forough zamani Nov 27 '18 at 20:50