Skip to content

Lab 5: Tool Orientation of PincherX 100 Robot Arm Using Rotation Matrices

Jump to bottom
Madi Babaiasl edited this page Aug 19, 2024 · 4 revisions

Objectives of lab 5

  • Calculate and visualize the orientation of the end effector of the PincherX100 robot arm with respect to its base frame using rotation matrices

Required hardware and software

  • Computer running Ubuntu 22.04
  • PincherX 100 robot arm along with its RViz simulation environment
  • Python programming environment (VS code) with matplotlib
  • Pen and paper for manual drawing

Calculating the Orientation of the PincherX 100 Robot Arm Using Rotation Matrices

Step 1. Put the PincherX100 Robot Arm in the Home Pose

Note: The home pose is where all servos are centered, and the joint angles are set to zero.

pix100 in home pose

Step 2. Draw a Diagram of Coordinate Frames

  • Based on the above figure, draw a diagram illustrating the coordinate frames, including the base frame and end-effector frame, along with all the frames in between.
  • Note that the first coordinate frame coincides with the {s} frame (the world frame of the pix100 is on the robot's base).

Step 3. Identify Rotation Axes and Joint Angles:

  • Identify the rotation axes of each joint and define the rotation angles $\theta_1$ to $\theta_4$ based on RHR.

Step 4. Calculate Rotation Matrices

  • Begin from the base frame and work your way up to find the rotation matrix representing the orientation of one frame with respect to the previous frame. For example, $R_{01} = Rot(\hat{z},\theta_1)I$.
  • Use the subscript cancellation rule to express all the rotation matrices w.r.t the base frame. For instance, $R_{02} = R_{01}R_{12} = Rot(\hat{z},\theta_1)Rot(\hat{y},\theta_2)I$. Do this until you get the $R_{05}$ which represents the orientation of the tool frame w.r.t the base frame.

Note: Here you can also visualize it as successive rotations from the base frame to the tool frame. Each rotation happens w.r.t the current frame and you can post-multiply the rotation operators starting from joint 1.

Step 5. Write a Python Code to Multiply Rotation Matrices

  • Write a Python code to multiply the rotation matrices as calculated in Step 4. Complete the following code:
import numpy as np 

# angles in radians
theta_1 = 
theta_2 = 
theta_3 = 
theta_4 = 

rot_z_theta_1 = np.array([[ , , ],
                        [ , , ],
                        [ , , ]]) 

rot_y_theta_2 = 
rot_y_theta_3 = 
rot_y_theta_4 = 
eye = 

# tool frame w.r.t the base frame

rot_0_5 = 

# print the final rotation matrix
  • Calculate the rotation matrix representing the orientation of the tool frame w.r.t the base frame when $\theta_1 = 90^{0}, \theta_2 = -45^{o}, \theta_3 = 0, \theta_4 = 45^{o}$. Note that if you are seeing the result in scientific notation, and want to make it more readable, add np.set_printoptions(suppress=True) to the above code after importing numpy.
  • Draw the calculated orientation by hand on a piece of paper

Step 6. Visualize the final rotation matrix using matplotlib

  • Open a command line and install matplotlib if it is not already installed: pip install matplotlib
  • Create a 3D visualization of the calculated matrix in Step 5. Complete the following code and combine it with the code from the previous step:
import matplotlib.pyplot as plt

# Create vectors for the coordinate frame
origin = np.zeros(3)
x_axis = 
y_axis = 
z_axis = 

# Visualize the coordinate frame
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.quiver(*origin, *x_axis, color='r', label='X-axis')
ax.quiver(*origin, *y_axis, color='g', label='Y-axis')
ax.quiver(*origin, *z_axis, color='b', label='Z-axis')

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')
ax.legend()

plt.show()
  • Verify that the visualization is the same as what you have drawn by hand. Show a side-by-side comparison.

Step 7. Verify Orientation with thet pix100's joint position control

  • Use the joint position control that we learned from previous labs
  • Input the above given joint angles
  • Record the orientation of the pix100's tool frame w.r.t the world frame (this frame is stationary and on the robot's base in the simulation).
  • Verify that the orientation achieved by physically rotating the joints to those angles is the same as the orientation you draw by hand, and visualized using the matplotlib. Show a side-by-side comparison. Note: If necessary, you can selectively hide frames in RViz from the left-hand side panel to focus on specific frames or components during visualization.

Guidelines for Lab 5 report

  • Submit one report per group. Note that each member of the group should have an equal contribution to completing the lab and documenting and writing the report (as always be an ethical engineer and disclose all your collaborators including the AI ones).
  • The codes that you wrote should be uploaded to your GitHub repo for this course (you can make a folder and call it Lab 5). Include the link in the report.
  • Use a lot of descriptive figures when possible.
  • Reports are due one week after the class that they completed. You can submit the report one week later than the due date for half credit.
  • If you are short of time, it is OK to write math, etc. by hand. If you do so, make sure to take a photo and submit all of them in just one PDF.

Pat yourself on the back for completing Lab 5! You've done a fantastic job.

Clone this wiki locally