def __init__(self, bot_base, ctrl_pub, configs): """ Constructor for ILQR based Control. :param bot_base: Object that has necessary variables that capture the robot state, collision checking, etc. :param ctrl_pub: Publisher topic to send commands to. :param configs: yacs configuration object. :type base_state: BaseState :type ctrl_pub: rospy.Publisher """ self.configs = configs self.max_v = self.configs.BASE.MAX_ABS_FWD_SPEED self.min_v = -self.configs.BASE.MAX_ABS_FWD_SPEED self.max_w = self.configs.BASE.MAX_ABS_TURN_SPEED self.min_w = -self.configs.BASE.MAX_ABS_TURN_SPEED self.rate = rospy.Rate(self.configs.BASE.BASE_CONTROL_RATE) self.dt = 1. / self.configs.BASE.BASE_CONTROL_RATE self.ctrl_pub = ctrl_pub self.bot_base = bot_base self.system = BicycleSystem(self.dt, self.min_v, self.max_v, self.min_w, self.max_w)
def get_state_trajectory_from_controls(start_pos, dt, controls): system = BicycleSystem(dt) states = system.unroll(start_pos, controls) return states