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]

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:
  • 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.
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:
  • 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.
Returns:

LQR controller that can track the specified trajectory.

Return type:

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
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:
  • x (numpy array (state_dim,)) – state of the system
  • alpha (float) – step size
  • i (int) – time step
Returns:

feedback control that should be applied

Return type:

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
Returns:

cost if system were to follow optimal f eedback control from now

Return type:

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.
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:
  • init_controls (numpy array) – Initial control sequence to start optimization from.
  • ilqr_iters – Number of iterations of linearizations and LQR solutions.
  • ilqr_iters – int
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:
  • 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.
Returns:

Sequence of states

Return type:

numpy array