How to use

[masterofshapes]

Trying to test out your invKine function but dont understand what position and worldspace its expecting? Thanks

[workproduct]

also curious!

[mhorstAK]

Hi hi,

I don't know if you're still looking for answers but maybe below can help?

Forward Kinematics

For the Forward Kinematic function "HTrans(th,c )" the "th" variable contains the joint angles of the robotic arm, starting from joints 1 through 6. Just to be safe "th" is best formatted as a Numpy matrix, even if it contains only a single column of information. I say column because it's assuming that your joints are listed as a single column where each row is a single joint angle.

The reason for the "th" being a 6 rows x 8 columns is that "th" can hold information for more than one apparent arm at a single time. The reasoning for this approach becomes more apparent in the Inverse Kinematic step.

Sample Code using Forward Kinematics

"""Joint Angle (in degrees) reading from encoders.""" theta1 = np.radians(0.0) theta2 = np.radians(170.0) theta3 = np.radians(90.0) theta4 = np.radians(40.0) theta5 = np.radians(90.0) theta6 = np.radians(0.0)

th = np.matrix([[theta1], [theta2], [theta3], [theta4], [theta5], [theta6]]) c = [0] location = HTrans(th,c ) print(location)

End of Code

The location printout will be in the form common with positional frames in kinematics (see equation 1 or 2 for detail: https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_kin_tech_report_1.pdf). Where P = [px, py, pz] is the Position of the end effector relative to the base location of [0, 0, 0]. The orientation of the 'end effector' at position P is given by three coordinate vectors N, O, and A.

N = [nx, ny, nz] and similarly for O and A.

Inverse Kinematics

For the Inverse Kinematics it's a similar process as Forward Kinematics only in reverse. The desired position (desired_pos) input is in the same format as the output from the Forward Kinematics solution, as discussed above. The largest difference being the output joint angles given from the 'invKine' function. With the mechanics of inverse kinematics there are multiple solutions to a single end effector position and orientation.

Sample Code using Inverse Kinematics

desired_pos = np.matrix([[ 3.06161700e-17, 8.66025404e-01, -5.00000000e-01, 3.63537488e-01], [-1.00000000e+00, 0.00000000e+00, -5.55111512e-17, -1.09150000e-01], [-5.55111512e-17, 5.00000000e-01, 8.66025404e-01, 4.25598256e-01], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])

th = invKine(desired_pos) print(th)

End of Code

This approach gives you 8 possible joint angle configurations that the Arm can have that the specified reach the end effector position and orientation. This is why the output "th" is 6 row x 8 column matrix. Really it is a 6 Joint Angle with 8 possible configurations Matrix. This is where "c" from the Forward Kinematics above comes in. C is just specifying which column of the joint configure matrix "th" you would like to use. Because our example for Forward Kinematics only had joints for a single arm and therefore one column, I set c = [0]. But "c" can equal anything from 0-7 depending on which joint configuration works best for you, as they all have the same end effector location.

Hope this helps!

[robin-gdwl]

Thank you very much @mhorstAK, I have one question, please forgive if it is obvoius but I dont have much experience with matrices and kinematics: Why is there a fourth row in the matrix for the desired pose? As I understand it, the first row has the x value of the position and the vectors the next two rows have the y and z values. Is the fourth row (0,0,0,1) simply the coordinate system ?

desired_pos = np.matrix([[ 3.06161700e-17, 8.66025404e-01, -5.00000000e-01, 3.63537488e-01], [-1.00000000e+00, 0.00000000e+00, -5.55111512e-17, -1.09150000e-01], [-5.55111512e-17, 5.00000000e-01, 8.66025404e-01, 4.25598256e-01], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])

[mhorstAK]

Great question! And my answer is... I don't know. I believe the bottom row is an artifact of mathematics side of things and doesn't give too much meaning to kinematics side, in this particular use case. With that said, I'm hoping someone much smarter than me can answer this completely!

I would start by looking in to what are called 'Identity Matrices'. Kinematics seems to use linear algebra as a vessel for easy and quick calculations of Position and Orientation. In that it also has to deal with idea that mathematics systems won't always serve the needs of the kinematic systems completely. This identity matrix is the vessel that the Position and Orientation information placed into. The reason being (I believe) is identity matrices deal well with matrix multiplication and matrix transposition (used by @mc-capolei in the kinematics code). But by the nature of the mathematics, identity matrices are forced to be an 'n x n' sized matrix. Our kinematics doesn't need all that extra space at the bottom of the identity matrix but the linear algebra does.

I've always had a question about what additional information could possibly placed on that fourth row. I'm sure it's been done. Maybe the position of the base of an arm relative to another arm's base could be placed on that fourth row for calculating positions and orientations of two arms working next to each other... Like a global position... but alas, work for another day.

Hope this helps a little! I'd encourage you to dig into it a bit. I really enjoy @mc-capolei 's implementation. Very good python.

[OfficialVreesie]

I need help with this, I am also not familiar with inverse kinematics, but I am trying to understand. However, In my code I have a xyz position and the xyz rotation of the robotic arm head. Now I need to convert this to valid joint locations, without exceeding the joint limits. How do I go from my position to a input matrix for the inverse_kinematics method and how do I go from that output response to valid joint locations. I am using the URX package, so I can call moveJ to move to the give joints and movel to move to a given location. Please help me implement this code into my project!!

[robin-gdwl]

Hello @OfficialVreesie I would suggest using the math3d library to convert between axis angle representation (x,y,z,rx,ry,rz) and matrices. This way you dont have to worry about matrices that much.

If you have a target position you create a math3D.Transform from it. This way you can simply get the matrix by accessing the .matrix attribute.

import math3d 
import math
from universal_robot_kinematics import invKine

myposition = [0, -0.4, 0.5, 0, 0, math.pi/2 ]  # target position as axis-angle representation in meters
targetposition = math3d.Transform(myposition)

joints = invKine(targetposition.matrix)
print("joints: ", joints)

Result:

joints:  [
[ 1.14850398  1.14850398  1.14850398  1.14850398 -1.14850398 -1.14850398 -1.14850398 -1.14850398]
 [-1.99404108  0.30504539 -1.54802586  0.49134013 -3.63293278 -1.59356679 -3.44663805 -1.14755157]
 [ 2.50026891 -2.50026891  2.1654839  -2.1654839   2.1654839  -2.1654839   2.50026891 -2.50026891]
 [-2.07702416  0.62442719  0.95333829 -3.03824521 -0.10334744  2.18825437   2.51716546 -1.06456849]
 [ 1.57079633  1.57079633 -1.57079633 -1.57079633  1.57079633  1.57079633  -1.57079633 -1.57079633]
 [ 1.99308868  1.99308868 -1.14850398 -1.14850398 -1.99308868 -1.99308868   1.14850398  1.14850398]
]

The result are 8 valid joint position combinations. To chose one simply chose the column of the array you want.

jointconfiguration = joints[ :, 0 ]  # the first column of the result array
print(jointconfiguration)

Result:

[[ 1.14850398]
 [-1.99404108]
 [ 2.50026891]
 [-2.07702416]
 [ 1.57079633]
 [ 1.99308868]]

The resulting column of the array shows the joint angles in radians starting at the base. Hope this helps