# Base Controllers¶

Here are the controller classes for the Base.

class 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: configs (dict) – configurations read from config file base_state (BaseState) – an object consisting of an instance of BaseState. ctrl_pub (rospy.Publisher) – a ros publisher used to publish velocity commands to base of the robot.
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: xyt_position (list or np.ndarray) – The goal state of the form (x,y,t) in the world (map) frame. close_loop (bool) – When set to “True”, ensures that controler is operating in open loop by taking account of odometry. smooth (bool) – When set to “True”, ensures that the motion leading to the goal is a smooth one.
goto(xyt_position=None)[source]

Moves the robot to the robot to given goal state in the relative frame (base frame).

Parameters: xyt_position (list) – The goal state of the form (x,y,t) in the relative (base) frame.
class locobot.base_controllers.ILQRControl(bot_base, ctrl_pub, configs)[source]

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: bot_base – Object that has necessary variables that capture the robot state, collision checking, etc. ctrl_pub (rospy.Publisher) – Publisher topic to send commands to. configs – yacs configuration object.
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: xyt_position (list or np.ndarray) – The goal state of the form (x,y,t) in the world (map) frame. close_loop (bool) – When set to “True”, ensures that controler is operating in open loop by taking account of odometry. smooth (bool) – When set to “True”, ensures that the motion leading to the goal is a smooth one.
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: states (list) – sequence of (x,y,t) states that the robot should track. controls (list) – optionally specify control sequence as well. close_loop (bool) – whether to close loop on the computed control sequence or not.
class 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: configs (dict) – configurations read from config file base_state (BaseState) – an object consisting of an instance of BaseState.
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: xyt_position (list or np.ndarray) – The goal state of the form (x,y,t) in the world (map) frame. close_loop (bool) – When set to “True”, ensures that controler is operating in open loop by taking account of odometry. smooth (bool) – When set to “True”, ensures that the motion leading to the goal is a smooth one.
class 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.

__init__(system)[source]

Provide system that should track 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: plan (LQRSolver) – Object returned from generate_plan function. close_loop (bool) – Whether to use feedback controller, or apply open-loop commands. Weather plan execution was successful or not. 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: xs – List of states that define the trajectory. us – List of controls that define the trajectory. If None, controls are inferred from the state trajectory. LQR controller that can track the specified trajectory. LQRSolver
plot_plan_execution(file_name=None)[source]

Plots the execution of the plan.

Parameters: file_name (string) – Name of file to plot plan execution.
stop()[source]

Stops the base.

class 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: goal (list) – The goal state of the form (x,y,t) in the world (map) frame. go_to_relative – This is a method that moves the robot the appropiate relative goal while NOT taking map into account.
parse_plan(plan)[source]

Parses the plan generated by move_base service

class 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: As (List of state_dim x state_dim numpy matrices) – List of time-varying matrices A_t for dynamics Bs (List of state_dim x control_dim numpy matrices) – List of time-varying matrices B_t for dynamics Cs (List of state_dim x 1 numpy matrices) – List of time-varying matrices C_t for dynamics Qs (List of 3 tuples with numpy array of size state_dim x state_dim, state_dim x 1, 1 x 1) – List of time-varying matrices Q_t for state cost Rs (List of control_dim x control_dim numpy matrices) – List of time-varying matrices R_t for control cost
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: x (numpy array (state_dim,)) – state of the system i (int) – time step feedback control that should be applied numpy array (control_dim,)
get_control_ls(x, alpha, i)[source]

Returns control but modulated via a step-size alpha.

Parameters: x (numpy array (state_dim,)) – state of the system alpha (float) – step size i (int) – time step feedback control that should be applied numpy array (control_dim,)
get_cost_to_go(x, i)[source]

Returns cost to go if the system is in state x at time step i.

Parameters: x (numpy array (state_dim,)) – state of the system i (int) – time step cost if system were to follow optimal f eedback control from now scalar
solve()[source]

Solves the LQR system and stores the solution, such that it can be accessed using get_control() function.

class 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: Q_fn (python function) – State cost function that takes as input x_goal, x_ref, time_step, lqr_iteration, and returns quadratic approximations of state cost around x_ref. R_fn (python function) – Control cost function that takes as input u_ref and returns quadtratic approximation around it. dyn_fn (python function) – Dynamics function thattakes in state, controls, return_only_state flag, and returns the linearization and the next state. start_state (numpy vector) – Starting state of the system. goal_state (numpy vector) – Goal state of the system.
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: ilqr_iter (int) – Which ILQR iteration are we doing so as to compute the cost function which may depend on the ilqr_iteration for a log barrier method. ref_controls (numpy array) – Reference controls, we are starting iterations from. ref_cost (float) – Refernce cost that we want to improve over. step size, updated controls, updated cost float, numpy array, float
solve(init_controls, ilqr_iters)[source]

Solve the non-linear system.

Parameters: init_controls (numpy array) – Initial control sequence to start optimization from. ilqr_iters – Number of iterations of linearizations and LQR solutions. ilqr_iters – int LQR Tracker, step_size, cost of solution 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: Q_fn (python function) – State cost function that takes as input x_goal, x_ref, time_step, lqr_iteration, and returns quadratic approximations of state cost around x_ref. start_state (numpy vector) – Starting state of the system. controls (numpy array) – Sequence of controls to apply to the system. Sequence of states numpy array