We want to build upon last time's lab, but this time instead of finding out what is the final position as a function of motor values, this time we will be making (or trying at least) a function that takes as input the position and outputs the motor values. We will also play with this function to make the pupper's leg to move in a triangular shape

• Open the lab 3 code repository (lab 3 code repository)
• Open the lab 3 folder in VSCode

cd ~/lab_3_fall_2025
code .

• Examine lab_3.py to understand the structure of the InverseKinematics class and its methods.

tar -xvf pupper_viz.tar.gz
cd pupper_viz/
docker build -t pupper_viz .

./run.sh 3

1. Open lab_3.py and locate the forward_kinematics method in the InverseKinematics class
2. In this lab, we will instead use the right front leg of Pupper. Implement the forward_kinematics method for the right front leg. This should be very similar to your implementation of the front left leg from lab 2.

• Find the inverse_kinematics method in the InverseKinematics class.

TODO 1: Implement the cost_function(theta) for inverse kinematics. This function returns cost, a scalar, and l1, a vector of size 3. Use the forward_kinematics method to get the current end-effector position. Calculate the L1 distance between the current and target end-effector positions. Return the sum of squared L1 distances as the cost (AKA the squared L2 norm of the error vector). TODO 2: Implement the gradient(theta, epsilon) function to calculate the numerical gradient for inverse kinematics. TODO 3: Implement the gradient descent algorithm for inverse kinematics.

• Define the learning rate, maximum number of iterations, and tolerance as hyperparameters. We recommend starting with a relatively large learning rate (e.g., 5), which is higher than what is typically used when training neural networks. Tolerance is measured in meters.
• Update the joint angles using the calculated gradient.
• Stop the iteration if the mean L1 distance is below the tolerance.

• Locate the interpolate_triangle method in the InverseKinematics class.

TODO 4: Implement the interpolation for the triangular trajectory. You need to create a function that performs linear interpolation between the triangle’s vertices. The trajectory should loop smoothly from vertex 1 to 2, vertex 2 to 3, and then from vertex 3 back to vertex 1 based on the time variable. The input to the function is a time variable t that dictates where along the triangle’s edges the point currently lies for a given 3-second period. Each vertex transition (e.g., from vertex 1 to vertex 2) should last approximately 1 second. For example, 0 ⇐ t < 1 should interpolate between vertex 1 and vertex 2.

• Use the provided ee_triangle_positions, which define the 3 vertices of the triangle trajectory (this is a 3×3 matrix).
• Implement linear interpolation between the triangle vertices based on the input time t. You can use the np.interp function from NumPy to handle the interpolation, or write your own weighted sum function to achieve the same effect.
• Ensure the trajectory loops every ~3 seconds approximately.

• Run the launch file using the following command in ~/lab_3_fall_2025:

ros2 launch lab_3.launch.py

• On a separate terminal, run the following command to run the lab_3.py file in ~/lab_3_fall_2025:

python3 lab_3.py

• Observe the robot leg’s movement and the terminal output.
• Experiment with different trajectory shapes by modifying the ee_triangle_positions in the init method. If you have recorded the end-effector positions from lab 2, you can use them to set the ee_triangle_positions to match the recorded positions and replay the recorded trajectory!

• Modify the ik_timer_period and pd_timer_period to see how they affect the system’s performance.
• Try different initial guesses for the inverse kinematics algorithm and observe the convergence behavior.