Robotic Mechanics and Modeling/Kinematics/Additional Examples of Homogeneous Transforms
Example 1 (Team 5, Spring 20)
editAn example for homogeneous transformation in 3 different frames
editFrame 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 .
%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)
editAn example of a practical use of a homogeneous transform matrix
editA 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 away from the air traffic control tower. The air tower suddenly spots a drone at position 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 away.
Example 3 (Team 6, Spring 2020)
editAn example of finding coordinates of end effector of robotic arm in different frames by using homogeneous transform matrix
editThe position of end effector of a 3-joint robotic arm is relative to the frame B, and frame B relative to frame A (global frame) is in distance and 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 ]
