| """PID / line-of-sight controller.""" | |
| import numpy as np | |
| from experiments.shared.src.control.geometric import goal_action | |
| from experiments.shared.src.vision.pose_from_image import estimate_pose_from_clean_image | |
| def compute_action(observation, task_state, config): | |
| """Compute one controller action.""" | |
| pose = estimate_pose_from_clean_image(observation) | |
| goal = np.asarray(task_state["goal"], dtype=np.float32) | |
| return goal_action(pose, goal, int(config["action_dim"]), float(config["forward"]), float(config["kp_heading"])) | |