The basic controller for FLIQC. More...
#include <FLIQC_controllers.hpp>
Public Member Functions | |
| FLIQC_controller_joint_velocity_basic (int dim_q) | |
| Construct a new fliqc controller basic object. | |
| ~FLIQC_controller_joint_velocity_basic ()=default | |
| Destroy the FLIQC_controller_joint_velocity_basic object. | |
| Eigen::VectorXd | runController (const FLIQC_state_input &state_input, const std::vector< FLIQC_distance_input > &dist_inputs, FLIQC_control_output &control_output) |
| Run the controller. | |
Public Attributes | |
| FLIQC_quad_cost_type | quad_cost_type = FLIQC_QUAD_COST_IDENTITY |
| The type of the cost function, either identity or mass matrix. | |
| FLIQC_linear_cost_type | linear_cost_type = FLIQC_LINEAR_COST_NONE |
| The type of the linear cost function, currently not used. | |
| double | lambda_cost_penalty = 1.0 |
| The penalty for the lambda constraint in the optimization problem \( k_\lambda \). | |
| bool | enable_lambda_constraint_in_L = false |
| Enable the lambda constraint in left complementarity constraint. If true, the lambda_max will be used in upper bound of \( \boldsymbol{L} \). If false, it will be left as unconstrained. | |
| bool | enable_lambda_constraint_in_x = true |
| Enable the lambda constraint in the optimization variable x. If true, the lambda_max will be used in upper bound of \( \boldsymbol{x} \). If false, the upper bound is maximum of numeric limits of double. | |
| bool | enable_esc_vel_constraint = false |
| Enable the escape velocity constraint in right complementarity constraint. If true, the esc_vel_max will be used in upper bound of \( \boldsymbol{R} \). If false, it will be left as unconstrained. | |
| bool | enable_nullspace_projector_in_A = true |
| Enable the nullspace projector in the A matrix. If true, \( \boldsymbol{P}^{\mathrm{null}} \) will be \( \boldsymbol{I} - \boldsymbol{J}^\dagger_{\mathrm{pos}} \boldsymbol{J}_{\mathrm{pos}} \). If false, it will be left as identity matrix. | |
| double | dt = 0.02 |
| The forward step for the optimization prediction in right complementarity constraint \( \Delta t \). | |
| double | eps = 0.02 |
| The safety margin for the forward step prediction in right complementarity constraint \( \epsilon \). | |
| double | active_threshold = 0.05 |
| The active tolerance for considering the distance as active. | |
| double | lambda_max = std::numeric_limits<double>::max() |
| The maximum lambda value \( \lambda_{\max} \). | |
| double | esc_vel_max = std::numeric_limits<double>::max() |
| The maximum escape velocity calculated in R \( v_{\mathrm{esc},\max} \). | |
| Eigen::VectorXd | q_dot_max |
| The maximum joint velocity \( \dot{\boldsymbol{q}}_{\max} \). | |
| Eigen::VectorXd | weight_on_mass_matrix |
| The weight on the mass matrix, this is used to scale the mass matrix to the same scale as the other cost function. | |
| LCQPow_bridge | lcqp_solver |
| The solver for the optimization problem. | |
Protected Attributes | |
| int | nJoint |
| The dimension of the control variables (number of joints) | |
| LCQProblemInput | lcqp_input |
| The input of the LCQProblem. | |
| LCQProblemOutput | lcqp_output |
| The output of the LCQProblem. | |
Detailed Description
The basic controller for FLIQC.
This controller is activated by running runController() function.
When running the controller, we need input:
- a velocity \( \boldsymbol{\dot{q}}_{\text{guide}} \) that the controller would like to follow, the mass matrix \( \boldsymbol{M} \) and the Jacobian matrix \( \boldsymbol{J} \) for the current end-effector configuration.
- a series of distances \( \psi_i \) that would like to be positive, along with the projector from joint velocity to the distance velocity \( \boldsymbol{P}_i \), and its reverse \( \boldsymbol{P}_i^{\mathrm{inv}} \).
The controller will run according to the optimization problem:
\[ \begin{aligned} \min_{\boldsymbol{\dot{q}}} \ &\frac{1}{2} \boldsymbol{\dot{q}}^\top \boldsymbol{Q} \boldsymbol{\dot{q}} + \boldsymbol{\dot{q}}^\top \boldsymbol{g} \\ \text{s.t.} \ &\boldsymbol{\dot{q}} = \boldsymbol{\dot{q}}_{\text{guide}} + \boldsymbol{P}^{\mathrm{null}} \sum_{i\in n_{\mathrm{act}}} \boldsymbol{P}^{\mathrm{inv}}_i \lambda_i \\ &0 \leq \lambda_i \perp \psi_i + \Delta t\, \boldsymbol{P}_i \boldsymbol{\dot{q}} \geq \epsilon \end{aligned} \]
In background, we use LCQPow_bridge(), which connects to the LCQPow library to solve the optimization problem. To formulate this as the LCQPow standard form, we define the optimization variable as:
\[ \boldsymbol{x} = \left[ \boldsymbol{\dot{q}}^\top,\ \lambda_1,\ \lambda_2,\ \ldots,\ \lambda_n \right]^\top \]
The result will be converted to LCQP formulation:
\[ \min_{\boldsymbol{x}} \quad \frac{1}{2} \boldsymbol{x}^\top \begin{bmatrix} \boldsymbol{Q} & 0 \\ 0 & k_\lambda \boldsymbol{I} \end{bmatrix} \boldsymbol{x} + \boldsymbol{x}^\top \begin{bmatrix} \boldsymbol{g} \\ 0 \end{bmatrix} \]
\[ \begin{array}{rrrcl} \mathrm{s}.\mathrm{t}.& \boldsymbol{0}\le& \left[ \begin{matrix} 0& \boldsymbol{I}\\ \end{matrix} \right] \boldsymbol{x}\le& \lambda _{\max}& \left( \text{inequality constraint } \boldsymbol{L} \right)\\ & \left[ \begin{array}{c} -\psi _1\\ \vdots\\ -\psi _n\\ \end{array} \right] +\epsilon \le& \left[ \begin{array}{c} \Delta t\,\boldsymbol{P}_1\\ \vdots\\ \Delta t\,\boldsymbol{P}_n\\ 0\\ \end{array} \right] \boldsymbol{x}\le& v_{\mathrm{esc},\max}& \left( \text{left compementarity constraint }\boldsymbol{L} \right)\\ & \dot{\boldsymbol{q}}_{\mathrm{guide}}\le& \left[ \begin{matrix} \boldsymbol{I}& -\boldsymbol{P}^{\mathrm{null}} \boldsymbol{P}_{1}^{\mathrm{inv}}& \cdots& -\boldsymbol{P}^{\mathrm{null}} \boldsymbol{P}_{n}^{\mathrm{inv}}\\ \end{matrix} \right] \boldsymbol{x}\le& \dot{\boldsymbol{q}}_{\mathrm{guide}}& \left( \text{right compementarity constraint } \boldsymbol{R} \right)\\ & \left[ \begin{array}{c} -\dot{\boldsymbol{q}}_{\min}\\ 0\\ \end{array} \right] \le& \boldsymbol{x}\le& \left[ \begin{array}{c} \dot{\boldsymbol{q}}_{\max}\\ \lambda _{\max}\\ \end{array} \right]& \left( \text{variable constraint } \right)\\ \end{array} \]
Before running the controller, there are many public attributes that can be configured to change the behavior of the controller.
Options for the cost function:
- quad_cost_type: the type of the quadratic cost function.
- linear_cost_type: [Currently unused] the type of the linear cost function.
- lambda_cost_penalty: the penalty for the lambda constraint in the optimization problem.
Options for the controller constraint formulation:
- buffer_history: [Unimplemented] buffer the last solution as initial guess for the next run
- enable_lambda_constraint_in_L: enable the lambda constraint in left complementarity constraint
- enable_lambda_constraint_in_x: enable the lambda constraint in the optimization variable
- enable_esc_vel_constraint: enable the escape velocity constraint in right complementarity constraint
- enable_nullspace_projector_in_A: enable the nullspace projector in the A matrix
Parameters for the controller:
- dt: the forward step for the optimization prediction in right complementarity constraint \( \Delta t \)
- eps: the safety margin for the forward step prediction in right complementarity constraint \( \epsilon \)
- active_threshold: The active tolerance for considering the distance as active
- lambda_max: The maximum lambda value \( \lambda_{\max} \)
- esc_vel_max: The maximum escape velocity calculated in R \( v_{\mathrm{esc},\max} \)
- q_dot_max: The maximum joint velocity \( \dot{\boldsymbol{q}}_{\max} \)
- weight_on_mass_matrix: [Currently don't suggest to use] the weight on the mass matrix, this is used to scale the mass matrix to the same scale as the other cost function.
Definition at line 165 of file FLIQC_controllers.hpp.
Constructor & Destructor Documentation
◆ FLIQC_controller_joint_velocity_basic()
| FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::FLIQC_controller_joint_velocity_basic | ( | int | dim_q | ) |
Construct a new fliqc controller basic object.
- Parameters
-
dim_q The dimension of the control variable q_dot
Definition at line 27 of file FLIQC_controllers.cpp.
Member Function Documentation
◆ runController()
| Eigen::VectorXd FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::runController | ( | const FLIQC_state_input & | state_input, |
| const std::vector< FLIQC_distance_input > & | dist_inputs, | ||
| FLIQC_control_output & | control_output | ||
| ) |
Run the controller.
- Parameters
-
[in] state_input The cost input for the optimization problem [in] dist_inputs The input of the controller, which is the sensory signals to obstacles [out] control_output The output of the controller, which is the full solution of the optimization problem
- Returns
- Eigen::VectorXd The result control variable
- Exceptions
-
LCQPowException If the LCQPow solver fails to solve the optimization problem, it will throw an exception.
Definition at line 32 of file FLIQC_controllers.cpp.
Member Data Documentation
◆ active_threshold
| double FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::active_threshold = 0.05 |
The active tolerance for considering the distance as active.
Definition at line 207 of file FLIQC_controllers.hpp.
◆ dt
| double FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::dt = 0.02 |
The forward step for the optimization prediction in right complementarity constraint \( \Delta t \).
Definition at line 205 of file FLIQC_controllers.hpp.
◆ enable_esc_vel_constraint
| bool FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::enable_esc_vel_constraint = false |
Enable the escape velocity constraint in right complementarity constraint. If true, the esc_vel_max will be used in upper bound of \( \boldsymbol{R} \). If false, it will be left as unconstrained.
Definition at line 201 of file FLIQC_controllers.hpp.
◆ enable_lambda_constraint_in_L
| bool FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::enable_lambda_constraint_in_L = false |
Enable the lambda constraint in left complementarity constraint. If true, the lambda_max will be used in upper bound of \( \boldsymbol{L} \). If false, it will be left as unconstrained.
Definition at line 199 of file FLIQC_controllers.hpp.
◆ enable_lambda_constraint_in_x
| bool FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::enable_lambda_constraint_in_x = true |
Enable the lambda constraint in the optimization variable x. If true, the lambda_max will be used in upper bound of \( \boldsymbol{x} \). If false, the upper bound is maximum of numeric limits of double.
Definition at line 200 of file FLIQC_controllers.hpp.
◆ enable_nullspace_projector_in_A
| bool FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::enable_nullspace_projector_in_A = true |
Enable the nullspace projector in the A matrix. If true, \( \boldsymbol{P}^{\mathrm{null}} \) will be \( \boldsymbol{I} - \boldsymbol{J}^\dagger_{\mathrm{pos}} \boldsymbol{J}_{\mathrm{pos}} \). If false, it will be left as identity matrix.
Definition at line 202 of file FLIQC_controllers.hpp.
◆ eps
| double FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::eps = 0.02 |
The safety margin for the forward step prediction in right complementarity constraint \( \epsilon \).
Definition at line 206 of file FLIQC_controllers.hpp.
◆ esc_vel_max
| double FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::esc_vel_max = std::numeric_limits<double>::max() |
The maximum escape velocity calculated in R \( v_{\mathrm{esc},\max} \).
Definition at line 209 of file FLIQC_controllers.hpp.
◆ lambda_cost_penalty
| double FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::lambda_cost_penalty = 1.0 |
The penalty for the lambda constraint in the optimization problem \( k_\lambda \).
Definition at line 196 of file FLIQC_controllers.hpp.
◆ lambda_max
| double FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::lambda_max = std::numeric_limits<double>::max() |
The maximum lambda value \( \lambda_{\max} \).
Definition at line 208 of file FLIQC_controllers.hpp.
◆ lcqp_input
|
protected |
The input of the LCQProblem.
Definition at line 218 of file FLIQC_controllers.hpp.
◆ lcqp_output
|
protected |
The output of the LCQProblem.
Definition at line 219 of file FLIQC_controllers.hpp.
◆ lcqp_solver
| LCQPow_bridge FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::lcqp_solver |
The solver for the optimization problem.
Definition at line 213 of file FLIQC_controllers.hpp.
◆ linear_cost_type
| FLIQC_linear_cost_type FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::linear_cost_type = FLIQC_LINEAR_COST_NONE |
The type of the linear cost function, currently not used.
Definition at line 195 of file FLIQC_controllers.hpp.
◆ nJoint
|
protected |
The dimension of the control variables (number of joints)
Definition at line 216 of file FLIQC_controllers.hpp.
◆ q_dot_max
| Eigen::VectorXd FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::q_dot_max |
The maximum joint velocity \( \dot{\boldsymbol{q}}_{\max} \).
Definition at line 210 of file FLIQC_controllers.hpp.
◆ quad_cost_type
| FLIQC_quad_cost_type FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::quad_cost_type = FLIQC_QUAD_COST_IDENTITY |
The type of the cost function, either identity or mass matrix.
Definition at line 194 of file FLIQC_controllers.hpp.
◆ weight_on_mass_matrix
| Eigen::VectorXd FLIQC_controller_core::FLIQC_controller_joint_velocity_basic::weight_on_mass_matrix |
The weight on the mass matrix, this is used to scale the mass matrix to the same scale as the other cost function.
Definition at line 211 of file FLIQC_controllers.hpp.
The documentation for this class was generated from the following files:
- include/FLIQC_controller_core/FLIQC_controllers.hpp
- src/FLIQC_controllers.cpp
