In order to be able to control the pupper, the first thing we would need is to get the position of the legs. What we are given through the power of ROS magic as an input is the angles of each joint, and by knowing the topology of the robot we can figure out the end position of each leg through the power of linear algebra.

Recall from computer graphics that points in space are represented with vectors, and that any transformations can be represented with a matrix

• Make sure you have completed Lab 1 and are familiar with the ROS2 environment on your Raspberry Pi 5.
• Open the lab 2 code repository (lab 2 code repository) on your GitHub account.
• Open the lab 2 folder in VSCode

cd ~/lab_2_fall_2025
code .

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_1_end_effector_position and marker topics.
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 the convention to orient the coordinate frame so that the rotation about each motor is the z axis.

1. Open lab_2.py and locate the forward_kinematics method.
2. Implement the rotation matrices about the x, y, and z axes.

• The transformation matrix from the base link to leg_front_l_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.

• Implement the transformation matrix from leg_front_l_1 to leg_front_l_2 in T_1_2. Follow the same thought process as with T_0_1. Check out the figure below for visual reference

• Implement the transformation matrix from leg_front_l_2 to leg_front_l_3 in T_2_3. Check out the figure below for visual reference.

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

• 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.

1. Save your changes to lab_2.py
2. Run the ROS2 nodes:

ros2 launch lab_2.launch.py

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

python lab_2.py

1. Move the left 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 RVIZ2. RVIZ2 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 rviz2, 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.

1. Record the end-effector positions for the left front 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, innacurate joint angle readings, errors in the physical measurements of the robot

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 front leg:

1. During the testing with rviz2, 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_l_end_effector_position topic to record the end effector positions.