Kinetic devel

[addy1997]

I have written code for computing solutions of inverse kinematics of UR5 robotic arm. I wanted someone to go through and use it. I will be glad if for suggestions and errors. Here is the link to my repository.

https://github.com/addy1997/inv_kinematics

[gavanderhoorn]

Can you clarify where in the PR your code is?

I have the impression that your code is in the repository, and the PR is just a merge of kinetic-devel into master.

If that is the case, please close the PR.

I'm afraid we don't run a code review service here, as we're all mightily busy with other things.

[addy1997]

Can you clarify where in the PR your code is?

I have the impression that your code is in the repository, and the PR is just a merge of kinetic-devel into master.

If that is the case, please close the PR.

I'm afraid we don't run a code review service here, as we're all mightily busy with other things.

My code

!/usr/bin/python2

import numpy as np from numpy import linalg import cmath import math from math import cos as cos from math import sin as sin from math import arctan2 as arctan2 from math import arccos as arccos from math import arcsin as arcsin from math import sqrt as sqrt from math import pi

global mat mat = np.matrix

UR5 Parameters

global d1, a2, a3, a7, d4, d5, d6 d1 = 0.1273 a2 = -0.612 a3 = -0.5723 a7 = 0.075 d4 = 0.163941

DH PARAMETERS d,a and alpha for UR5

global offset_distance, link_length, alpha

offset_distance = mat([0.089159,0,0,0.10915,0.09465,0.0823]) link_length = mat([0 ,-0.425 ,-0.39225 ,0 ,0 ,0]) alpha = mat([math.pi/2, 0, 0, math.pi/2, -math.pi/2, 0 ])

def AH( n,th,c ):

T_a = mat(np.identity(4), copy=False) T_a[0,3] = a[0,n-1] T_d = mat(np.identity(4), copy=False) T_d[2,3] = d[0,n-1]

Rzt = mat([[cos(th[n-1,c]), -sin(th[n-1,c]), 0 ,0], [sin(th[n-1,c]), cos(th[n-1,c]), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],copy=False)

Rxa = mat([[1, 0, 0, 0], [0, cos(alph[0,n-1]), -sin(alph[0,n-1]), 0], [0, sin(alph[0,n-1]), cos(alph[0,n-1]), 0], [0, 0, 0, 1]],copy=False)

A_i = T_d Rzt T_a * Rxa

return A_i

def HTrans(th,c ): A_1=AH( 1,th,c ) A_2=AH( 2,th,c ) A_3=AH( 3,th,c ) A_4=AH( 4,th,c ) A_5=AH( 5,th,c ) A_6=AH( 6,th,c )

T_06=A_1A_2A_3A_4A_5*A_6

return T_06

** INVERSE KINEMATICS

def invKine(desired_pos):# T60 th = mat(np.zeros((6, 8))) P_05 = (desired_pos * mat([0,0, -d6, 1]).T-mat([0,0,0,1 ]).T)

theta1

psi = atan2(P_05[2-1,0], P_05[1-1,0]) phi = acos(d4 /sqrt(P_05[2-1,0]P_05[2-1,0] + P_05[1-1,0]P_05[1-1,0]))

The two solutions for theta1 correspond to the shoulder

being either left or right

th[0, 0:4] = pi/2 + psi + phi th[0, 4:8] = pi/2 + psi - phi th = th.real

theta5

cl = [0, 4]# wrist up or down for i in range(0,len(cl)): c = cl[i] T_10 = linalg.inv(AH(1,th,c)) T_16 = T_10 * desired_pos th[4, c:c+2] = + acos((T_16[2,3]-d4)/d6); th[4, c+2:c+4] = - acos((T_16[2,3]-d4)/d6);

th = th.real

theta6

theta6 is not well-defined when sin(theta5) = 0 or when T16(1,3), T16(2,3) = 0.

cl = [0, 2, 4, 6] for i in range(0,len(cl)): c = cl[i] T_10 = linalg.inv(AH(1,th,c)) T_16 = linalg.inv( T_10 * desired_pos ) th[5, c:c+2] = atan2((-T_16[1,2]/sin(th[4, c])),(T_16[0,2]/sin(th[4, c])))

th = th.real

theta3

cl = [0, 2, 4, 6] for i in range(0,len(cl)): c = cl[i] T_10 = linalg.inv(AH(1,th,c)) T_65 = AH( 6,th,c) T_54 = AH( 5,th,c) T_14 = ( T_10 desired_pos) linalg.inv(T_54 T_65) P_13 = T_14 mat([0, -d4, 0, 1]).T - mat([0,0,0,1]).T t3 = cmath.acos((linalg.norm(P_13)2 - a22 - a3*2 )/(2 a2 * a3)) # norm ? th[2, c] = t3.real th[2, c+1] = -t3.real

theta2 and theta 4

cl = [0, 1, 2, 3, 4, 5, 6, 7] for i in range(0,len(cl)): c = cl[i] T_10 = linalg.inv(AH( 1,th,c )) T_65 = linalg.inv(AH( 6,th,c)) T_54 = linalg.inv(AH( 5,th,c)) T_14 = (T_10 desired_pos) T_65 T_54 P_13 = T_14 mat([0, -d4, 0, 1]).T - mat([0,0,0,1]).T

      # theta 2
      th[1, c] = -atan2(P_13[1], -P_13[0]) + asin(a3* sin(th[2,c])/linalg.norm(P_13))
      # theta 4
      T_32 = linalg.inv(AH( 3,th,c))
      T_21 = linalg.inv(AH( 2,th,c))
      T_34 = T_32 * T_21 * T_14
      th[3, c] = atan2(T_34[1,0], T_34[0,0])

th = th.real

return th

[gavanderhoorn]

So your code is not part of the PR?

[gavanderhoorn]

I'm closing this, as it does not seem to contribute anything to ur_modern_driver, but is a request for code review of stand-alone code that is not part of the driver nor will it become part of it.

[addy1997]

I'm closing this, as it does not seem to contribute anything to ur_modern_driver, but is a request for code review of stand-alone code that is not part of the driver nor will it become part.

I am being too polite and on your part your attitude is way too derogatory. I havent specifically asked you for a favour to review my code. So next time dont comment.

[gavanderhoorn]

@addy1997 wrote:

I wanted someone to go through and use it. I will be glad if for suggestions and errors. [..] I havent specifically asked you for a favour to review my code.

So who did you ask then? I'm sure the other maintainers would respond in a similar way.

Furthermore: I am one of the maintainers of this repository. You open a Pull Request merging two "arbitrary" branches. Your code is part of neither. Nor is it directly intended to be merged into this repository. That makes it at best tangentially related to this repository.

Additionally, we've clarified a few times (at least in #349 and in #346) that what you request of us (reviews of your personal code that does not seem to relate to this repository) is not a service we provide here.

Please clarify why I am not allowed to comment, and to close the PR?

[addy1997]

Then Please could you suggest me where should I make a pull up request?

[gonzalocasas]

@addy1997 I believe you're mixing up some things. The UR driver does not include inverse kinematic solvers (look into the entire MoveIt stack for that). But more importantly, you are mixing up how Github works: if you send a pull request, the operation is "YOUR FORK'S BRANCH -> YOUR TARGET's REPO BRANCH", you cannot open a pull request between two existing branches of the target repo and then simply add a link to another repository because there's no automated way to merge the two things.

Creating a pull request means you need to invest substantially more time into making your code fit into the target repository you want it to be merged.