Lab 2: Forward Kinematics

Goal

Implement forward kinematics for the right front leg of the Pupper robot using ROS2 and Python.

Here’s what your implementation should look like when complete:

Fill out the lab document as you go. Make a copy and add your responses.

Part 1: Hardware Build

  1. Follow the build instructions for lab 2

    📝 Open build instructions in new tab 📝

    You will build a front right leg for Pupper in this lab. Begin by checking to see that your kits contain all the pieces, if not, please ask a TA.

Part 2: Setup

  1. Make sure you have completed Lab 1 and are familiar with the ROS2 environment on your Raspberry Pi 5.

  2. Clone the lab 2 code repository on the Raspberry Pi:

    cd ~/
git clone https://github.com/cs123-stanford/lab_2_2024.git lab_2

  3. Open the workspace (lab_2 directory) in VSCode and examine the lab_2.py file.

  4. For the next few labs, we will tune down the kp and kd values of our motor controllers (which behave as PID controllers, similar to the PD controller we implemented in lab 1) to make the Pupper’s legs less stiff (so that you can move them around more easily)! To do that, open the ~/ros2_ws/src/pupper_v3_description/description/components.xacro file using VSCode, and change all 12 occurrences of homing_kp values to 0.5 and homing_kd values to 0.1 (you can do that quickly using ctrl+f and replace all).

DELIVERABLE: Why do you think there are 12 occurrences of these values in the xacro file? What do you think changing them from the previous value does?

Part 3: Understanding the Code Structure

Before we start implementing the TODOs, let’s understand the structure of the lab_2.py file:

  1. The code defines a ForwardKinematics class that inherits from rclpy.node.Node.

  2. It subscribes to the joint_states topic and publishes to the leg_front_r_end_effector_position topic.

  3. The forward_kinematics method is where we’ll implement the forward kinematics calculations.

  4. The code uses NumPy for matrix operations.

  5. Note that it is convention to orient the coordinate frame so that the rotation about each motor is the z axis.

Part 4: Implementing Forward Kinematics

For the following steps, you can view the Pupper CAD to help you understand the kinematic chain CAD

Step 1: Implement Rotation Matrices

  1. Open lab_2.py and locate the forward_kinematics method.

  2. Implement the rotation matrices about the x, y, and z axes. Follow the homogeneous coordinates representation as presented in lecture.

DELIVERABLE: Which axis is typically used as the default axis for rotations in robotic systems? What angles are we rotating along the default axis? Why?

Step 2: Implement Transformation Matrices

Note

In the following steps, \(\theta\) (theta) represents the motor angle.

  1. The transformation matrix from the base link to leg_front_r_1 has been implemented for you in T_0_1. This involves a translation and two rotations. We include a visualization of this transformation below to facilitate your understanding (keeping all these in mind can be tricky!). Understanding this transformation will help you complete the remainder of the transformations.

    ../../../_images/base_r1_kinematics.png

    Transformation from base link to leg_front_r_1

DELIVERABLE: Explain the reasoning behind this implementation. What does the translation and each of the rotations do in T_0_1?

  1. Implement the transformation matrix from leg_front_r_1 to leg_front_r_2 in T_1_2. Follow the same thought process as with T_0_1. Check out the figure below for visual reference.

    ../../../_images/r1_r2_kinematics.png

    Transformation from leg_front_r_1 to leg_front_r_2

  2. Implement the transformation matrix from leg_front_r_2 to leg_front_r_3 in T_2_3. Check out the figure below for visual reference.

    ../../../_images/r2_r3_kinematics.png

    Transformation from leg_front_r_2 to leg_front_r_3

  3. Implement the transformation matrix from leg_front_r_3 to the end effector in T_3_ee. Check out the figure below for visual reference.

    ../../../_images/r3_ee_kinematics.png

    Transformation from leg_front_r_3 to the end effector

  4. Compute the final transformation matrix following the described process from lecture in T_0_ee. Remember that the end effector position is not in homogeneous coordinates. Calculate end_effector_position from T_0_ee.

    Note

    The translation values may need to be adjusted based on the actual dimensions of your robot. Make sure to verify these values with your robot’s specifications.

DELIVERABLE:

  1. Write out the full equation you used to calculate the forward kinematics (in math). Please use LaTeX and take a screenshot, or use the equation functionality in Google Docs. What is the benefit of using homogeneous transformations?

  2. Why is there a 1 in the bottom-right corner of a homogeneous transformation matrix?

Part 5: Testing Your Implementation

  1. Save your changes to lab_2.py.

  2. Run the ROS2 nodes:

    ros2 launch lab_2.launch.py

  3. In another terminal, use the following command to run the main code:

    python lab_2.py

  4. Move the right front leg of your robot and observe the changes in the published positions.

To test your code in simulation to make sure that the code works as expected, you can use RVIZ. RVIZ will show the Pupper model as well as a marker that shows the output from the forward kinematics.

rviz2 -d lab_2.rviz

The above command will load the RVIZ config file. If you just run rviz, you can manually add the configuration. After running rviz, click the “Add” button, and then select a Robot Model type. Select the /robot_description topic. Next, add the marker by selecting “Add” again, and select a Marker type. Select the topic /marker.

Note

While we’ve tested this pipeline on a Pupper and it works as expected, rviz may fail on your robot due to heating in the Raspberry Pi. If this happens, reach out to a TA to check the implementation first, then turn off Pupper, wait a while to let it cool down, and try again.

DELIVERABLE: Take a video of the working implementation with you moving Pupper’s leg and the simulation mimicking the results and upload it to the Google Drive

Part 6: Analyzing the Results

  1. Record the end-effector positions for the front right leg configurations.

  2. Compare these positions with the expected positions based on the physical dimensions of your robot. (Why are the numbers printed in the terminal so small?)

  3. If there are discrepancies, try to identify the source of the errors. It could be due to:

    • Incorrect transformation matrices

    • Inaccurate joint angle readings

    • Errors in the physical measurements of the robot

DELIVERABLE:

  1. Measuring the correct physical parameters of the robot (leg lengths, motor angles, etc.) is essential to compute accurate kinematics. This process is called system identification. How would your estimate of the end effector (EEF) position change if your estimate of leg link 2 (r2) is off by 0.2 cm short from the actual distance to leg link 1 (r1)? What about 0.4 cm, or 0.8 cm? Write out the numbers you computed, and how you calculated them, for both 0 degrees rotation in each of the joints, and 45 degrees rotation in each of the joints. Qualitatively, how does error in estimated EEF position change with respect to error in leg length?

  2. How does computational complexity of FK scale with respect to degree of freedom (number of motor angles)? Please use big O notation.

Additional Challenges (Optional)

If you finish early and want to explore further:

  1. Extend your implementation to calculate forward kinematics for all four legs of the Pupper robot. Save your calculations for these other legs for lab 4, where we will need forward kinematics for all four legs.

    We provide the base link to leg_back_r1 transformation in the diagram below. The rest of the transformations are identical to the right front leg:

    ../../../_images/base_back_kinematics.png

    Base to back right leg transformation diagram

  2. During the testing of rviz, write a script that saves the sequence of your well-crafted motion, recorded as end effector positions into a file. You will have a chance to let Pupper replay this recorded motion in the next lab! You will need to use the joint_states topic to record the motor angles, and the leg_front_r_end_effector_position topic to record the end effector positions.

Friendly reminder: These are by no means any of the “optional labs” yet for this quarter. The first optional lab will be released next week, and it will be much more challenging/fun/frustrating/rewarding than these ;)

Remember, understanding forward kinematics is crucial for robot control and motion planning. Take your time to ensure you understand each step of the process.