File size: 540 Bytes
604e535 | 1 2 3 4 5 6 7 8 9 10 11 12 13 | """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"]))
|