Calculates torques from position commands
(target_q, q, kp, target_dq, dq, kd)
| 684 | |
| 685 | |
| 686 | def pd_control(target_q, q, kp, target_dq, dq, kd): |
| 687 | """Calculates torques from position commands""" |
| 688 | return (target_q - q) * kp + (target_dq - dq) * kd |
| 689 | |
| 690 | |
| 691 | if __name__ == "__main__": |