Here are the controller classes for the Base.
locobot.base_controllers.
ProportionalControl
(bot_base, ctrl_pub, configs)[source]¶This class encapsulates and provides interface to a Proportional controller used to control the base
__init__
(bot_base, ctrl_pub, configs)[source]¶The constructor for ProportionalControl class.
Parameters: |
|
---|
go_to_absolute
(xyt_position, close_loop=True, smooth=False)[source]¶Moves the robot to the robot to given goal state in the world frame using proportional control.
Parameters: |
|
---|
locobot.base_controllers.
ILQRControl
(bot_base, ctrl_pub, configs)[source]¶Bases: locobot.base_control_utils.TrajectoryTracker
Class to implement LQR based feedback controllers on top of mobile bases.
__init__
(bot_base, ctrl_pub, configs)[source]¶Constructor for ILQR based Control.
Parameters: |
|
---|
go_to_absolute
(xyt_position, close_loop=True, smooth=True)[source]¶Moves the robot to the robot to given goal state in the world frame using ILQR control.
Parameters: |
|
---|
go_to_relative
(xyt_position, close_loop=True, smooth=True)[source]¶Relative pose that the robot should go to.
track_trajectory
(states, controls=None, close_loop=True)[source]¶State trajectory that the robot should track using ILQR control.
Parameters: |
|
---|
locobot.base_controllers.
MoveBaseControl
(base_state, configs)[source]¶Bases: object
This class encapsulates and provides interface to move_base controller used to control the base
__init__
(base_state, configs)[source]¶The constructor for MoveBaseControl class.
Parameters: |
|
---|
go_to_absolute
(xyt_position, close_loop=True, smooth=False)[source]¶Moves the robot to the robot to given goal state in the world frame.
Parameters: |
|
---|
locobot.base_control_utils.
TrajectoryTracker
(system)[source]¶Bases: object
Class to track a given trajectory. Uses LQR to generate a controller around the system that has been linearized around the trajectory.
execute_plan
(plan, close_loop=True)[source]¶Executes the plan, check for conditions like bumps, etc. Plan is object returned from generate_plan. Also stores the plan execution into variable self._trajectory_tracker_execution.
Parameters: |
|
---|---|
Returns: | Weather plan execution was successful or not. |
Return type: | bool |
generate_plan
(xs, us=None)[source]¶Generates a feedback controller that tracks the state trajectory specified by xs. Optionally specify a control trajectory us, otherwise it is automatically inferred.
Parameters: |
|
---|---|
Returns: | LQR controller that can track the specified trajectory. |
Return type: |
locobot.base_control_utils.
MoveBasePlanner
(configs)[source]¶This class enscapsulates the planning capabilities of the move_base and provides planning services and plan tracking services.
get_plan_absolute
(x, y, theta)[source]¶Gets plan as a list of poses in the world frame for the given (x, y, theta
move_to_goal
(goal, go_to_relative)[source]¶Moves the robot to the robot to given goal state in the absolute frame (world frame).
Parameters: |
|
---|
locobot.base_control_utils.
LQRSolver
(As=None, Bs=None, Cs=None, Qs=None, Rs=None, x_refs=None, u_refs=None)[source]¶Bases: object
This class implements a solver for a time-varying Linear Quadratic Regulator System. A time-varying LQR system is defined via affine time varying transition functions, and time-varying quadratic cost functions.Such a system can be solved using dynamic programming to obtain time-varying feedback control matrices that can be used to compute the control command given the state of the system at any given time step.
__init__
(As=None, Bs=None, Cs=None, Qs=None, Rs=None, x_refs=None, u_refs=None)[source]¶Constructor for LQR solver.
System definition: \(x_{t+1} = A_tx_t + B_tu_t + C_t\)
State cost: \(x^t_tQ_tx_t + x^t_tq + q\_\)
Control cost: \(u^t_tR_tu_t\)
Parameters: |
|
---|
get_control
(x, i)[source]¶Uses the stored solution to the system to output control cost if the system is in state x at time step i.
Parameters: |
|
---|---|
Returns: | feedback control that should be applied |
Return type: | numpy array (control_dim,) |
get_control_ls
(x, alpha, i)[source]¶Returns control but modulated via a step-size alpha.
Parameters: |
|
---|---|
Returns: | feedback control that should be applied |
Return type: | numpy array (control_dim,) |
locobot.base_control_utils.
ILQRSolver
(dyn_fn, Q_fn, R_fn, start_state, goal_state)[source]¶Bases: object
Iterative LQR solver for non-linear systems. Computes a linearization of the system at the current trajectory, and solves that linear system using LQR. This process is repeated.
__init__
(dyn_fn, Q_fn, R_fn, start_state, goal_state)[source]¶Constructor that sets up functions describing the system and costs.
Parameters: |
|
---|
get_step_size
(lqr_, ref_controls, ref_cost, ilqr_iter)[source]¶Search for the step size that improves the cost function over LQR iterations.
Parameters: |
|
---|---|
Returns: | step size, updated controls, updated cost |
Return type: | float, numpy array, float |
solve
(init_controls, ilqr_iters)[source]¶Solve the non-linear system.
Parameters: |
|
---|---|
Returns: | LQR Tracker, step_size, cost of solution |
Return type: | LQRSolver, int, cost |
unroll
(dyn_fn, start_state, controls)[source]¶Obtain state trajectory by applying controls on the system defined by the dynamics function dyn_fn.
Parameters: |
|
---|---|
Returns: | Sequence of states |
Return type: | numpy array |