Differences
This shows you the differences between two versions of the page.
|
rasb:lab:04 [2026/06/28 08:57] jan.vaduva [Part 2: Hello PD] |
rasb:lab:04 [2026/06/30 12:26] (current) jan.vaduva [Part 2: Hello PD] |
||
|---|---|---|---|
| Line 82: | Line 82: | ||
| 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'' | 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'' | ||
| + | |||
| + | <code> | ||
| + | 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 | ||
| + | </code> | ||
| === Step 4: Implement P Control === | === Step 4: Implement P Control === | ||
| - | 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. | + | 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 | Start with Kp = 2.0 | ||
| - | {{:rasb:lab:pid_eqn.png?200|}} | + | |
| + | {{:rasb:lab:p_control.png?200|}} | ||
| === Step 5: Implement PD Control === | === Step 5: Implement PD Control === | ||
| - | 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. | + | 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: | Start with Kp = 2.0 and Kd = 0.3. Implement the PD control law using the following update equation: | ||
| - | {{:rasb:lab:p_control.png?200|}} | + | {{:rasb:lab:pid_eqn.png?600|}} |
| Where: | Where: | ||
| - | + | * **τ** is the commanded torque for the motor | |
| - | is the commanded torque for the motor | + | * **θ<sub>target</sub>** is the target angle |
| - | + | * **ω<sub>target</sub>** is the target angular velocity (usually 0) | |
| - | is the target angle | + | * **θ<sub>current</sub>** is the current motor angle |
| - | + | * **ω<sub>current</sub>** is the current motor angular velocity | |
| - | is the target angular velocity (usually 0) | + | * **K<sub>p</sub>** and **K<sub>d</sub>** 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. | |
| - | is the current motor angle | + | |
| - | + | ||
| - | is the current motor angular velocity | + | |
| - | + | ||
| - | and are the proportional and derivative gains | + | |
| - | + | ||
| - | 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. | Run your code ''python lab_1.py'' and observe the behavior of the PD controller. | ||
| Line 175: | Line 196: | ||
| * ROS2 Workspace: | * ROS2 Workspace: | ||
| - | * All robot-relevant code is in ros2_ws | + | * All robot-relevant code is in ''ros2_ws'' |
| * Key packages: | * Key packages: | ||
| * Neural controller (policy support) | * Neural controller (policy support) | ||
