Differences
This shows you the differences between two versions of the page.
|
rasb:lab:04 [2026/06/28 09:05] jan.vaduva [Additional Notes] |
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 === | ||
