Robotic Mechanics and Modeling/Kinematics/Additional Examples for Rotations and Coordinates

Example 1 (Spring '20 - Team 1)

An astronaut is floating in space looking down at the earth. The coordinates of the astronaut's space craft are $\langle 1.4,0.65,0.78\rangle$ km with respect to the astronaut's reference frame. the The astronaut's partner informs him of an asteroid passing by. In order to see it, the astonaut must rotate $67^{\circ }$ to the left (around the z-axis), then $54^{\circ }$ down (around the new y-axis), and then $38^{\circ }$ on his side (the new x-axis). What are the coordinates of the space craft in the astronauts new reference frame.

This whole code falls in one single block but it has been separated into chunks for ease of explanation.

The following code is the initial setup for the program. First we input, the angles in which the astronaut has to rotate by, alpha will be used for the z rotation, beta will be used for the y rotation and gamma will be used for the x rotation. Notice that the angles are converted to radians in Python.

%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

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

The following code shows the rotation matrices for the z, y and x rotations accordingly. The final part of code will provide a sequence of dot products of these matrixes that will give the Euler rotations that the astronaut experiences.

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)

This code block gives a name to the Euler sequence and presents the location of the spacecraft in the original frame (a_p). Afterwards a series of elements corresponding to the x, y z coordinates of the spacecraft are made for ease of plotting. The vectors x_a, y_a, z_a are for the inital point of the spacecraft, x_b, y_b, z_b represent the new location of the aircraft which is expressed by the dot product of the original location and the Euler sequence.

b_a_R=EulerRzyx
a_p=np.array([[1.40,0.65,0.78]])
a_p=a_p.T
b_p=np.dot(b_a_R,a_p)
b_p

x_a=np.array([[1,0,0]])
x_a=x_a.T
y_a=np.array([[0,1,0]])
y_a=y_a.T
z_a=np.array([[0,0,1]])
z_a=z_a.T

x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

The cooridnates of the spacecraft with respect to the new cooridnate system is $\langle 0.61,1.52,-0.54\rangle$ . These next two sets of code are responsible for plotting the position of the spacecraft relative to the astronaut and the new direction in which the astronaut is facing. The result is shown in the following figures.

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')
ax.text(x_a[0][0],x_a[1][0],x_a[2][0],'$x_a$',None,size=20)
ax.text(y_a[0][0],y_a[1][0],y_a[2][0],'$y_a$',None,size=20)
ax.text(z_a[0][0],z_a[1][0],z_a[2][0],'$z_a$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$Spacecraft$',None,size=15)
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.set_zlim([-2, 2])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-2,2,num=5))
plt.yticks(np.linspace(-2,2,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Ref_a_with_point.png',dpi=300,bbox_inches='tight')

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='green')
ax.text(x_b[0][0],x_b[1][0],x_b[2][0],'$x_b$',None,size=20)
ax.text(y_b[0][0],y_b[1][0],y_b[2][0],'$y_b$',None,size=20)
ax.text(z_b[0][0],z_b[1][0],z_b[2][0],'$z_b$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$Spacecraft$',None,size=15)
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.set_zlim([-2, 2])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-2,2,num=5))
plt.yticks(np.linspace(-2,2,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Rotated_Frame_b_with_point_at_origin.png',dpi=300,bbox_inches='tight')
plt.show

Example 2 (Spring '20 - Team 2)

A two scuba divers are diving in the Great Blue Hole for the first time. They are following the buddy protocol as they continue to dive down deeper to a depth of $\langle 6,4,-8\rangle$ . Suddenly, Diver 1 lost his buddy and couldn't see him anymore. In order to find his buddy Diver 1 needs to rotate 45 about the X, Y, and Z axes. What are the coordinates in Diver 1's new reference frame?

To start, the following libraries need to be imported for the script

import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D

The first step to solve the problem is to define the angles of rotation the Diver needs to rotate, and define the Euler angle rotation matrix in the X Y and Z directions . This can be done in the following way

# Angles to be rotated about
alpha = 45 * np.pi / 180  # Yaw for body-fixed rotation (z)
beta = 45 * np.pi / 180  # Pitch for body-fixed rotation (y)
gamma = 45 * np.pi / 180  # Roll for body-fixed rotation (x)

# Construction of the Euler angle rotations
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)

Once everything is defined, the method of rotation from the initial reference frame (a) to the new reference frame (b) is take the dot product of the Euler angle rotation matrix's transpose and the initial position vector. This is shown in the following code.

b_a_R = EulerRzyx # Rotation from a to b

# Initial Position Vector
a_p = np.array([ [6, 4, -8] ])
a_p = a_p.T

# Final Position Vector
a_b_R = b_a_R.T
b_p=np.dot(a_b_R,a_p)

Once the rotation is completed, the vectors can be plotted for a visual representation

# X,Y, and Z vectors
x_a = np.array([ [3, 0, 0] ])
x_a = x_a.T
y_a = np.array([ [0, 3, 0] ])
y_a = y_a.T
z_a = np.array([ [0, 0, 3] ])
z_a = z_a.T

x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

# Plotting
# Creates the Plot
fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')

# Creates arrows
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')

ax.quiver(0,0,0,x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='Yellow')
ax.quiver(0,0,0,y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='Orange')
ax.quiver(0,0,0,z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='Purple')

# Creates labels for arrows
ax.text(x_a[0][0]+1,x_a[1][0]+1,x_a[2][0]+1,'$x_a$',None,size=20)
ax.text(y_a[0][0]+1,y_a[1][0]+1,y_a[2][0]+1,'$y_a$',None,size=20)
ax.text(z_a[0][0]+1,z_a[1][0]+1,z_a[2][0]+1,'$z_a$',None,size=20)

# Creates points A and B
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.scatter(b_p[0],b_p[1],b_p[2],s=60,marker='o')

# Creates labels for points A and B
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$_a p$',None,size=20)
ax.text(b_p[0][0]+0.1,b_p[1][0],b_p[2][0],'$_b p$',None,size=20)

# Graph Ticks
ax.set_xlim([-10,10])
ax.set_ylim([-10,10])
ax.set_zlim([-10,10])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-10,10,num=5))
plt.yticks(np.linspace(-10,10,num=5))
ax.set_zticks(np.linspace(-10,10,num=5))
ax.view_init(20,-110)

Example 3 (Spring '20 - Team 3)

A bus driver is training a new hire. To test his knowledge of the routes, he asks him to point out where the next stop is. Confused by his recent lecture on Robotics, the new hire points to the stop, but the trainer realizes his trainee was looking for the stop in the wrong reference frame. The new hire claims that the new stop is half a mile in the x-direction, half a mile in the y-direction and half a mile in the z-direction. The trainer says this is true, but only when the x-coordinate system is rotated by 50 degree, the y-coordinate system is rotated by 100 degrees, and the z-coordinate system is rotated by 70 degrees. The trainer sets out a coordinate axis to help the new hire understand this rotation better.

The first thing that must be done to solve this problem is to import the proper libraries and to define the three angles, denoted as "alpha", "beta", and "gamma". An Euler angle rotation matrix must be defined.with these angles, and so must the original location of the point. Remember that the new hire believed that the stop was at a vector of $\langle 5,5,5\rangle$ in miles away from where they were. That array must be transposed while the rotation matrix must be inverted. Once the dot product is taken between them, the new coordinates can be figured out and the axis and points can be graphed.

%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

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

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
a_p=np.array([[0.5,0.5,0.5]])
a_p=a_p.T

a_b_R= np.linalg.inv(b_a_R) 

b_p=np.dot(a_b_R,a_p)
print('The resulting b_p is')
print(np.round(b_p,2))

x_a=np.array([[1,0,0]])
x_a=x_a.T
y_a=np.array([[0,1,0]])
y_a=y_a.T
z_a=np.array([[0,0,1]])
z_a=z_a.T

x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')
ax.text(x_a[0][0],x_a[1][0],x_a[2][0],'$x_a$',None,size=20)
ax.text(y_a[0][0],y_a[1][0],y_a[2][0],'$y_a$',None,size=20)
ax.text(z_a[0][0],z_a[1][0],z_a[2][0],'$z_a$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$_a p$',None,size=20)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-1,1,num=5))
plt.yticks(np.linspace(-1,1,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Ref_a_with_point.png',dpi=300,bbox_inches='tight')

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='green')
ax.text(x_b[0][0],x_b[1][0],x_b[2][0],'$x_b$',None,size=20)
ax.text(y_b[0][0],y_b[1][0],y_b[2][0],'$y_b$',None,size=20)
ax.text(z_b[0][0],z_b[1][0],z_b[2][0],'$z_b$',None,size=20)
ax.scatter(b_p[0],b_p[1],b_p[2],s=60,marker='x')
ax.text(b_p[0][0]+0.1,b_p[1][0],b_p[2][0],'$_b p$',None,size=20)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-1,1,num=5))
plt.yticks(np.linspace(-1,1,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Rotated_Frame_b_with_point.png',dpi=300,bbox_inches='tight')

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')
ax.text(x_a[0][0],x_a[1][0],x_a[2][0],'$x_a$',None,size=20)
ax.text(y_a[0][0],y_a[1][0],y_a[2][0],'$y_a$',None,size=20)
ax.text(z_a[0][0],z_a[1][0],z_a[2][0],'$z_a$',None,size=20)
ax.quiver(0,0,0,x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='green')
ax.text(x_b[0][0],x_b[1][0],x_b[2][0],'$x_b$',None,size=20)
ax.text(y_b[0][0],y_b[1][0],y_b[2][0],'$y_b$',None,size=20)
ax.text(z_b[0][0],z_b[1][0],z_b[2][0],'$z_b$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$_a p$',None,size=20)
ax.scatter(b_p[0],b_p[1],b_p[2],s=60,marker='x')
ax.text(b_p[0][0]+0.1,b_p[1][0],b_p[2][0],'$_b p$',None,size=20)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-1,1,num=5))
plt.yticks(np.linspace(-1,1,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Both_axis_and_points',dpi=300,bbox_inches='tight')

With these parameters we see that the resulting bP is at ta vector $\langle -0.61,0.55,0.27\rangle$ in the original reference frame. Now the trainer wants to show the new hire just how much of an impact his mistake could have been so he asks the new hire to calculate the distance between the axis endpoints and more importantly, the total distance between points aP and bP. We know the values of the vectors x_a, x_b, y_a, y_b, z_a, z_b (These are the components of the end points for the vector axis lines) and so if we take the difference between each of those components for the x, y, and z vectors respectively, square them and then take the square root of the sum, we can find the distance between the end points of the quivers which define _a, x_b, y_a, y_b, z_a, z_b. You can also take the distance between the point a_p and b_p through a similar process where you break them down and use the distance formula between all of the components.

dx_x = (x_a[0][0] - x_b[0][0])**2
dx_y = (x_a[1][0] - x_b[1][0])**2
dx_z = (x_a[2][0] - x_b[2][0])**2
dx = np.sqrt(dx_x + dx_y + dx_z)
print("The distance between the two end of the x axis vectors is:")
print(dx)

dy_x = (y_a[0][0] - y_b[0][0])**2
dy_y = (y_a[1][0] - y_b[1][0])**2
dy_z = (y_a[2][0] - y_b[2][0])**2
dy = np.sqrt(dy_x + dy_y + dy_z)
print("The distance between the two end of the y axis vectors is:")
print(dy)

dz_x = (z_a[0][0] - z_b[0][0])**2
dz_y = (z_a[1][0] - z_b[1][0])**2
dz_z = (z_a[2][0] - z_b[2][0])**2
dz = np.sqrt(dz_x + dz_y + dz_z)
print("The distance between the two end of the z axis vectors is:")
print(dz)

dp_x = (a_p[0][0] - b_p[0][0])**2
dp_y = (a_p[1][0] - b_p[1][0])**2
dp_z = (a_p[2][0] - b_p[2][0])**2
dp = np.sqrt(dp_x + dp_y + dp_z)
print("The distance between the two points, p, is:")
print(dp)

The distance between the two end of the x axis vectors is:1.49 miles. The distance between the two end of the y axis vectors is: 0.36 miles. The distance between the two end of the z axis vectors is: 1.46 miles. the distance between the two points, p, is: 1.14 miles.