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