Kinematics Functions for Robotics

[hamorabi]

Hello,

I am relatively new to the prime. I am sharing some functions I created that can be pretty useful for someone studying Robotics, particularly Robot kinematics.

Here's a list of the functions and brief explanation of what they do:

1. atan_2(x,y): This is the tan inverse function that takes into account the quadrant. For example, the angle returned for atan_2(3/4) is different from the angle returned for atan_2(-3/-4). This is necessary in Robotics as we want to avoid any ambiguity regarding the angle of rotation of a joint.

The matrices that we deal with in Robotics are usually 4x4 (known as the homogeneous matrix). The upper left 3x3 matrix defines the orientation of body in space. The upper right 3x1 matrix defines the relative position of the body from some location.

2. Trans(x,y,z): This function returns a 4x4 translation matrix x units in the x direction, y units in the y direction, and z units in the z direction.

3. Rot(ang,ax): This function returns a 4x4 rotation matrix for a body rotated by an angle "ang" about the axis "ax". To make things simple, if the rotation is about the x axis, use Rot(ang,1). If the rotation is about the y axis, use Rot(ang,2), and Rot(ang,3) for rotation about the z axis.

Here's an example of how the two functions above can prove useful.
Suppose we have a body that is currently at the origin. Suppose the body undergoes the following transformations:
1. Rotation of 90° about the z-axis,
2. Followed by a rotation of 90° about the new y-axis,
3. Followed by a translation of [4,-3,7] with respect to the fixed axis.

What is the orientation and location of the body relative to the origin?
Take a guess about the x,y,z location of the body. You will likely not get the correct answer because of all the rotations and change of direction of the unit axes as the body rotates.

We can calculate the orientation and location by compounding the transformation matrices as shown below. Transformations with respect to the new axis are multiplied on the left while transformations with respect to the fixed axis are multiplied on the right:

PHP Code:

Rot(90,2)*Rot(90,3)*Trans(4,-3,7) 


3. dh(v): This is a function that calculates the transformation matrix that will align one joint with the next one. We use it as follows:

PHP Code:

dh(θ,d,α,a) 


4. EulerMatrix(Φ,θ,ψ,ax): This functions does gives us the orientation of a body that has undergone three successive rotations given by Φ, θ, and ψ about three axis. For example EulerMatrix(90,60,30,213) means rotate the body by 90 degrees about the fixed y axis, then rotate by 60 degrees about the fixed x axis, then rotate by 30 degrees about the fixed z axis.


5. EulerAngle(m,ax): Suppose we have a body that has undergone some rotations. How can we know the angles relative to the axes that we need to rotate the body so that its orientation is just as it was before any rotation? This function takes a 3x3 orientation matrix and tells gives you the angles you can rotate the body by to return it to its default orientation. For instance, EulerAngle(matrix,212) means tell me the three angles with which I need to rotate the body first with respect to the y axis, then with respect to the x axis, and finally with respect to the y axis again.

PHP Code:

#cas

def atan_2(x,y):
    hypo = sqrt(x**2 + y**2)
    if (hypo < 10**(-6)):
        return 0
    elif (y >= 0):
        return acos(x/hypo)
    else:
        if HAngle == 0:
            return 2*pi - acos(x/hypo)
        elif HAngle == 1:
            return 360 - acos(x/hypo)
        else:
            return 400 - acos(x/hypo)

def Trans(x,y,z):
    return [[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z],[0, 0, 0, 1]]

def Rot(ang,ax):
    if (ax == 1):
        return [[1, 0, 0, 0], [0, cos(ang), -sin(ang), 0], [0, sin(ang), cos(ang), 0],[0, 0, 0, 1]]
    elif (ax == 2):
        return [[cos(ang), 0, sin(ang), 0], [0, 1, 0, 0], [-sin(ang), 0, cos(ang), 0],[0, 0, 0, 1]]
    elif (ax == 3):
        return [[cos(ang), -sin(ang), 0, 0], [sin(ang), cos(ang), 0, 0], [0, 0, 1, 0],[0, 0, 0, 1]]  

def dh(v):
    return [[cos(v[0]),-sin(v[0])*cos(v[2]),sin(v[0])*sin(v[2]),v[3]*cos(v[0])],[sin(v[0]),cos(v[0])*cos(v[2]),-cos(v[0])*sin(v[2]),v[3]*sin(v[0])],[0,sin(v[2]),cos(v[2]),v[1]],[0,0,0,1]]


def EulerAngle(m,ax):
    if (ax == 123):
        ψ = atan_2(m[0][0],-m[0][1])
        Φ = atan_2(m[2][2],-m[1][2])
        θ = atan_2(sqrt(m[0][0]**2+m[0][1]**2),m[0][2])
    elif (ax == 132):
        ψ = atan_2(m[0][0],m[0][2])
        Φ = atan_2(m[1][1],m[2][1])
        θ = atan_2(sqrt(m[0][0]**2+m[0][2]**2),-m[0][1])
    elif (ax == 213):
        ψ = atan_2(m[1][1],m[1][0])
        Φ = atan_2(m[2][2],m[0][2])
        θ = atan_2(sqrt(m[1][0]**2+m[1][1]**2),-m[1][2])
    elif (ax == 231):
        ψ = atan_2(m[1][1],-m[1][2])
        Φ = atan_2(m[0][0],-m[2][0])
        θ = atan_2(sqrt(m[1][1]**2+m[1][2]**2),m[1][0])
    elif (ax == 312):
        ψ = atan_2(m[2][2],-m[2][0])
        Φ = atan_2(m[1][1],-m[0][1])
        θ = atan_2(sqrt(m[2][0]**2+m[2][2]**2),m[2][1])
    elif (ax == 321):
        ψ = atan_2(m[2][2],m[2][1])
        Φ = atan_2(m[0][0],m[1][0])
        θ = atan_2(sqrt(m[2][1]**2+m[2][2]**2),-m[2][0])
    elif (ax == 121):
        ψ = atan_2(m[0][2],m[0][1])
        Φ = atan_2(-m[2][0],m[1][0])
        θ = atan_2(m[0][0],sqrt(m[0][1]**2+m[0][2]**2))
    elif (ax == 131):
        ψ = atan_2(-m[0][1],m[0][2])
        Φ = atan_2(m[1][0],m[2][0])
        θ = atan_2(m[0][0],sqrt(m[0][1]**2+m[0][2]**2))
    elif ax == 212:
        ψ = atan_2(-m[1][2],m[1][0])
        Φ = atan_2(m[2][1],m[0][1])
        θ = atan_2(m[1][1],sqrt(m[1][0]**2+m[1][2]**2))
    elif (ax == 232):
        ψ = atan_2(m[1][0],m[1][2])
        Φ = atan_2(-m[0][1],m[2][1])
        θ = atan_2(m[1][1],sqrt(m[1][0]**2+m[1][2]**2))
    elif (ax == 313):
        ψ = atan_2(m[2][1],m[2][0])
        Φ = atan_2(-m[1][2],m[0][2])
        θ = atan_2(m[2][2],sqrt(m[2][0]**2+m[2][1]**2))
    elif (ax == 323):
        ψ = atan_2(-m[2][0],m[2][1])
        Φ = atan_2(m[0][2],m[1][2])
        θ = atan_2(m[2][2],sqrt(m[2][0]**2+m[2][1]**2))
    else:
        return "The given system does not exist."

    if HAngle == 0:
        fac = 360/(2*pi)
    elif HAngle == 2:
        fac = 9/10
    else:
        fac = 1
        
    return [Φ*fac, θ*fac, ψ*fac]

def EulerMatrix(Φ,θ,ψ,ax):
    n1 = ax//100
    n2 = (ax-n1*100)//10
    n3 = (ax-n1*100-n2*10)
    
    return (Rot(Φ,n1)*Rot(θ,n2)*Rot(ψ,n3))[0:3,0:3]

#end