Differences
This shows you the differences between two versions of the page.
|
rasb:lab:06 [2026/06/05 00:08] jan.vaduva |
rasb:lab:06 [2026/07/03 23:40] (current) andrei.batasev [Part 0: Setup] |
||
|---|---|---|---|
| Line 1: | Line 1: | ||
| ===== Lab 6: Inverse Kinematics and Trajectory Tracking ===== | ===== Lab 6: Inverse Kinematics and Trajectory Tracking ===== | ||
| + | ==== Goal ==== | ||
| + | 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 | ||
| + | ==== Part 0: Setup ==== | ||
| + | * Open the lab 3 code repository ([[https://github.com/cs123-stanford/lab_3_fall_2025|lab 3 code repository]]) | ||
| + | * Open the lab 3 folder in VSCode | ||
| + | <code> | ||
| + | cd ~/lab_3_fall_2025 | ||
| + | code . | ||
| + | </code> | ||
| + | * Examine ''lab_3.py'' to understand the structure of the ''InverseKinematics'' class and its methods. | ||
| + | <note tip> | ||
| + | For docker users, you may try to simulate with this setup (**If you already did this for lab 2, it is the same archive, no need to redownload or rebuild the image**): | ||
| + | {{:rasb:lab:pupper_viz.tar.gz|Download this}} and then do these following commands: | ||
| + | <code> | ||
| + | tar -xvf pupper_viz.tar.gz | ||
| + | cd pupper_viz/ | ||
| + | docker build -t pupper_viz . | ||
| + | </code> | ||
| + | Once done, you may paste your lab 3 solution into the lab_3.py and then | ||
| + | <code> | ||
| + | ./run.sh 3 | ||
| + | </code> | ||
| + | </note> | ||
| + | ==== Part 1: Forward Kinematics on Right Front Leg ==== | ||
| + | - Open lab_3.py and locate the forward_kinematics method in the InverseKinematics class | ||
| + | - 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. | ||
| + | ==== Part 2: Implement Inverse Kinematics ==== | ||
| + | * 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. | ||
| + | <note tip>What happens if the learning rate is too small… what if the learning rate gets too big? (Note: for Pupper’s safety, don’t change the learning rate in the code)</note> | ||
| + | ==== Part 3: Implement Trajectory Generation ==== | ||
| + | * 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 3x3 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. | ||
| + | ==== Part 4: Run and Test your implementation ==== | ||
| + | * Run the launch file using the following command in ~/lab_3_fall_2025: | ||
| + | <code> | ||
| + | ros2 launch lab_3.launch.py | ||
| + | </code> | ||
| + | * On a separate terminal, run the following command to run the lab_3.py file in ~/lab_3_fall_2025: | ||
| + | <code> | ||
| + | python3 lab_3.py | ||
| + | </code> | ||
| + | * 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! | ||
| + | ==== Part 5: Analyze and Improve Performance ==== | ||
| + | * 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. | ||
| + | |||
