We’ll be using ROS2 (Robot Operating System) throughout this course. ROS2 provides tools, libraries, and conventions that facilitate building robotic applications and allow different parts of the robot to interact with each other.

Familiarize yourself with the basics of ROS by reviewing the ROS introduction guide (https://wiki.ros.org/ROS/Introduction). Keep this guide handy as a ROS2 cheat sheet (https://github.com/ubuntu-robotics/ros2_cheats_sheet/tree/master) that you can refer to throughout the course. We also have a list of important ROS2 commands for this course here: What is ROS 2?

ROS services in Pupper: robot.service manages control code (face controller, rl or heuristic controller, etc). See if controller is running: systemctl status robot.service You should see the status as “active (running)”. Checkout all topics and services: ros2 topic list and ros2 service list.

Since we are running custom code, we must disable the robot service before working on your code. This will prevent the robot from running any pre-existing code that may interfere with your work. Make sure to place Pupper on the stand during this process. To disable the robot service, run the following commands:

Note: Robot Service should already be disabled. So you can skip this step and proceed to Part 2.

sudo systemctl disable robot.service
sudo reboot

• Open the lab 1 code repository (https://github.com/cs123-stanford/lab_1_fall_2025) on your GitHub account. Then, fork the repository to your own GitHub account.

• Open the lab 1 folder in VSCode

cd ~/lab_1_fall_2025
code .

• Examine lab_1_fall_2025/lab_1.py to understand where the motor angle and velocity are read and where the motor is commanded.

Before running your code, explain in your lab document what you understand about the publishers and subscribers. What gets sent and received on each message publish? How does this correspond to what is physically commanded in the motor?

• Check the launch description in lab_1.launch.py and lab_1.yaml. Familiarize yourself with the structure and parameters defined in these files.
• Run the launch file using the following command:

ros2 launch lab_1.launch.py

This command will start all the necessary nodes for your PD control experiment.

• After running the launch file, you should see output in your terminal indicating that the nodes have been started successfully. If you encounter any errors, double-check your file paths and make sure all dependencies are installed.
• Open a new terminal window (if using SSH, you can open multiple connections to your Raspberry Pi, or add a terminal from VSCode) and run the following command to see the list of active topics:

    ros2 topic list

You should see topics related to joint states and commands. These are the topics your node will be publishing to and subscribing from.

• To inspect the data being published on a specific topic, you can use the ros2 topic echo command. For example:

    ros2 topic echo /joint_states

This will show you real-time data about the joint states of your robot leg.

Provide screenshots of:

1. The terminal output after running the launch file, showing successful node startup.
2. The list of active topics you observed.
3. A sample of the joint states data you saw when using the ros2 topic echo command.

Also, answer the following questions:

1. What nodes are being launched by your lab_1.launch.py file?
2. What parameters are being set in the lab_1.yaml file, and what do you think they control?
3. Based on the topics you observed, how do you think the different parts of your robot control system are communicating with each other?

Remember, understanding how the launch system works and how to inspect your ROS2 system is crucial for debugging and developing more complex robotic systems in the future.

Open lab_1.py and locate the control_loop() implementation. For this step, you will implement Bang-bang control before PD control. Remember that bang-bang control is a simple control strategy where the control input is either on or off. In this case, the control input is either positive maximum torque or negative maximum torque. The control input switches when the motor angle crosses a threshold.

This can be accomplished by a block of if statements. Implement bang-bang control in the lab_1.py file by implementing the get_target_joint_info(self) and calculate_torque(self, joint_pos, joint_vel, target_joint_pos, target_joint_vel) functions. Run your code by starting a new terminal, navigating to the lab folder, and running python lab_1.py

def get_target_joint_info(self):
        ####
        # Hold the joint steady at the zero position
        target_joint_pos = 0.0
        
        # We want the leg to be still at that position, so target velocity is 0
        target_joint_vel = 0.0
        ####

        return target_joint_pos, target_joint_vel

def calculate_torque(self, joint_pos, joint_vel, target_joint_pos, target_joint_vel):
        ####
        # 1. Calculate the error in position (Proportional)
        pos_error = target_joint_pos - joint_pos
        
        # 2. Calculate the error in velocity (Derivative)
        vel_error = target_joint_vel - joint_vel
        
        # 3. Apply the PD formula
        torque = (KP * pos_error) + (KD * vel_error)
        ####
        
        return torque

Implement P control in the lab_1.py file by replacing your implementation of bang-bang control. The P controller is more robust than bang-bang control. The proportional gain (Kp) is used to tune the controller. For reference, all the joint states published by ros2 systems are typically in radians.

Start with Kp = 2.0

Implement PD control in the lab_1.py file by replacing your implementation of P control. The PD controller is more robust than only P control, and is common control strategy used in robotics to stabilize systems (both Pupper and Toddy use PD controllers!). The proportional gain (Kp) and derivative gain (Kd) are used to tune the controller.

Start with Kp = 2.0 and Kd = 0.3. Implement the PD control law using the following update equation:

Where:

• τ is the commanded torque for the motor
• θ_target is the target angle
• ω_target is the target angular velocity (usually 0)
• θ_current is the current motor angle
• ω_current is the current motor angular velocity
• K_p and K_d are the proportional and derivative gains
• r(t), known as a feedforward_term, is a constant term that you can use to send a constant torque to the motor. For us, we just use 0.

Run your code python lab_1.py and observe the behavior of the PD controller.

Answer the following questions in your lab document:

1. How does the leg respond to manual movements?
2. What happens when you change Kp and Kd values?
3. Find and report the optimal Kp and Kd values for your setup.

Experiment with different Kp and Kd values and observe the effects. Be prepared for potential instability!

For each situation, manually rotate the leg to get a physical sense of the PD behavior. Report your findings in your lab document.

1. Vary Kp while keeping Kd constant (0.1). Try Kp values from 0.5 to 5.0.
2. Vary Kd while keeping Kp constant (2.0). Try Kd values from 0.1 to 1.0.

Report your findings for each experiment in your lab document.

Introduce a delay in the system by adding a buffer in the current motor angle and velocity readings. This simulates the delay in the physical system.

Experiment with different delay values (e.g., several steps of delay).

from collections import deque

# In your initialization:
self.delay_buffer_size = int(delay_seconds * control_frequency)
self.angle_buffer = deque(maxlen=self.delay_buffer_size)
self.velocity_buffer = deque(maxlen=self.delay_buffer_size)

# In your control loop:
self.angle_buffer.append(joint_pos)
self.velocity_buffer.append(joint_vel)
joint_pos = self.angle_buffer[0]
joint_vel = self.velocity_buffer[0]

#####
# You can also instead delay the output torque
#####

Report your findings in your lab document. How does the delay affect the performance of the PD controller?

Program the leg to track a sinusoidal (smooth and continuous back and forth motion) position:

import time
import math

current_time = time.time()
joint_pos_desired = math.sin(current_time)

Experiment with different frequencies of the sine wave.

• ROS2 Workspace:
  • All robot-relevant code is in ros2_ws
  • Key packages:
    • Neural controller (policy support)
    • Hardware interface (motor control)
    • Pupper feelings (face control)
    • Pupper descriptions (URDF files)
• Motor Control:
  • Refer to the control node and joy node
  • URDF is the source of truth for CAN IDs
  • Joi

Congratulations on completing your first lab! All the ROS code may look a bit overwhelming, but you will definitely get more comfortable with it in a few weeks, especially after you see what Pupper can do! This hands-on experience with ROS2 and PD control on a real robot will serve as a foundation for the more advanced topics we’ll cover in future labs.