Calculates torques from position commands
(target_q, q, kp, target_dq, dq, kd)
| 330 | |
| 331 | @staticmethod |
| 332 | def pd_control(target_q, q, kp, target_dq, dq, kd): |
| 333 | '''Calculates torques from position commands |
| 334 | ''' |
| 335 | if MujocoRobot.RAND_NOISE: |
| 336 | kp,kd = MujocoRobot.mk_rand_noise(np.array([kp, kd]), MujocoRobot.noise_ratio) |
| 337 | return (target_q - q) * kp + (target_dq - dq) * kd |
| 338 | |
| 339 | def _get_state(self): |
| 340 | '''Extracts physical states from the mujoco data structure |