Robotic Mechanics and Modeling/Kinematics/Additional Examples of Homogeneous Transforms

Example 1 (Team 5, Spring 20)

An example for homogeneous transformation in 3 different frames

Frame b is rotated and translated with respect to frame a in angle:(-20,-20,-20), distance:(0.5,0.5,0.5); Frame c is rotated and translated with respect to frame b in angle:(20,20,20), distance:(0.8,0.2,0.3). There is a point (1,2,3) in frame a, we can calculate the position in frame c by $_{a}^{c}T=\,_{a}^{b}T\,_{b}^{c}T$.

%reset -f
import numpy as np

a_P = np.array([[1,2,3]]).T             #location of the point in frame a is (1,2,3)
a_P_ab = np.array([[0.5,0.5,0.5]]).T    #translation between frame a and b is (0.5,0.5,0.5)
b_P_bc = np.array([[0.8,0.2,0.3]]).T    #translation between frame b and c is (0.8,0.2,0.3)

#define 4x4 homogeneous transfrom matrix
def transformmatrix(alpha,beta,gamma,distance):
    
    EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
    EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
    EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

    EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)  #rotation matrix
    T = np.append(np.append(EulerRzyx,distance,axis=1),[[0,0,0,1]],axis=0)
    
    return T

b_a_T = transformmatrix(-20,-20,-20,a_P_ab)   #calculate transform matrix frome b to a with angle(-20,-20,-20)
c_b_T = transformmatrix(20,20,20,b_P_bc)      #calculate transform matrix frome c to b with angle(20,20,20)
c_a_T = np.dot(b_a_T,c_b_T)                   #c_a_T = b_a_T * c_b_T
a_c_T = np.linalg.inv(c_a_T)

c_P = np.dot(a_c_T,np.append(a_P,[[1]],axis=0)) 
c_P = np.delete(c_P,3,axis=0)                 #from 4*1 vector to 3*1 vector

print('Homogenous transform from b to a:')
print(b_a_T)
print('Homogenous transform from c to b:')
print(c_b_T)
print('Homogenous transform from c to a')
print(c_a_T)
print('The position in frame c is') 
print(c_P)

Output: Homogenous transform from b to a:

[ 0.16653097  0.71268034  0.68143537  0.5       ]
[-0.37255658 -0.59438062  0.71268034  0.5       ]
[ 0.91294525 -0.37255658  0.16653097  0.5       ]
[ 0.          0.          0.          1.        ]

Homogenous transform from c to b:

[ 0.16653097 -0.03243282  0.98550269  0.8       ]
[ 0.37255658  0.92744256 -0.03243282  0.2       ]
[-0.91294525  0.37255658  0.16653097  0.3       ]
[ 0.          0.          0.          1.        ]

Homogenous transform from c to a

[-0.32886687  0.90944224  0.25448258  0.98019146]
[-0.93412075 -0.27365708 -0.22919472  0.29688271]
[-0.13879841 -0.31309201  0.93952562  1.20580418]
[ 0.          0.          0.          1.        ]

The position in frame c is

[-1.8464631 ]
[-1.00980375]
[ 1.30038838]

Example 2 (Team 4, Spring 2020)

An example of a practical use of a homogeneous transform matrix

A plane is performing tricks at an air show. Its current heading is 15 degrees yaw (rotation about z axis), 25 degrees pitch (y axis) and 15 degrees roll (x axis) and its current position is $\langle 0,500,1000\rangle \ {\mathsf {meters}}$ away from the air traffic control tower. The air tower suddenly spots a drone at position $\langle 5,550,1020\rangle \ {\mathsf {meters}}$ from the tower. What are the coordinates of the drone relative to the pilot?

%reset -f
import numpy as np

#Location of the drone in reference frame a
a_p=np.array([[5,550,1020]])
a_p=a_p.T

#location of the plane, used as origin for reference frame b
a_p_ab=np.array([[0, 500, 1000]])
a_p_ab=a_p_ab.T

alpha=15*np.pi/180 #Yaw for body-fixed rotation (z)
beta=25*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=15*np.pi/180 #Roll for body-fixed rotation (x)

#Rotation matrix calculation
EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)
b_a_R=EulerRzyx

#Pose matrix of coordinate b to a
b_a_T=np.append(b_a_R,np.zeros([1,3]),axis=0)
b_a_T=np.append(b_a_T,np.append(a_p_ab,[[1]],axis=0),axis=1)
#Find inverse of matrix to find b_p
a_b_T=np.linalg.inv(b_a_T)

#Finding b_p
a_p_1=np.append(a_p,[[1]],axis=0)
b_p_1=np.dot(a_b_T,a_p_1)
b_p=np.delete(b_p_1,3,axis=0)

print("Resulting b_p")
print(np.round(b_p,2))

The output is

[[ 7.65]
 [52.04]
 [12.6 ]]

This means that the pilot of the plane sees the drone at a relative $\langle 7.65,52.04,12.6\rangle \ {\mathsf {meters}}$ away.

Example 3 (Team 6, Spring 2020)

An example of finding coordinates of end effector of robotic arm in different frames by using homogeneous transform matrix

The position of end effector of a 3-joint robotic arm is $\langle 0.1,0.2,0.3\rangle \ {\mathsf {meters}}$ relative to the frame B, and frame B relative to frame A (global frame) is $\langle 0.5,0.6,0.7\rangle \ {\mathsf {meters}}$ in distance and $\langle 30,40,50\rangle \ {\mathsf {degree}}$ in angle, then what is the position of end effector in frame A?

%reset -f
import numpy as np

b_P = np.array([[0.1,0.2,0.3]]).T         #End Effector Location in frame B
a_P_ab = np.array([[0.5,0.6,0.7]]).T      #Translation vector of frame B relative to frame A

# yaw, pitch and roll of the rotation between frame B and frame A
alpha=30*np.pi/180;
beta=40*np.pi/180;
gamma=50*np.pi/180;

# rotation matrix for z, y, and x axis
EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx) 
b_a_T = np.append(np.append(EulerRzyx,a_P_ab,axis=1),[[0,0,0,1]],axis=0)

print('Homogenous Transform Matrix from B to A:')
print(b_a_T)

# finding a_P
b_P_1=np.append(b_P,[[1]],axis=0)
a_p_1=np.dot(b_a_T,b_P_1)
a_P=np.delete(a_p_1,3,axis=0)
print(np.round(a_P,2))

Output: Homogeneous transform matrix from frame B to frame A is:

[ 0.66341395  0.10504046  0.74084306  0.5       ]
[ 0.38302222  0.80287234 -0.45682599  0.6       ]
[-0.64278761  0.58682409  0.49240388  0.7       ]
[ 0.          0.          0.          1.        ]

And the position of end effector in frame A is:

[0.81]
[0.66]
[0.9 ] ${\mathsf {meters}}$