Implementation of the hierarchical weighted damped least squares solver (HWLS), similar to Schutter, J. et al. “Constraint-based Task Specification and Estimation for Sensor-Based Robot Systems in the Presence of Geometric Uncertainty.” The International Journal of Robotics Research 26 (2007): 433 - 455. It solves the following optimization problem. More...
#include <HierarchicalLSSolver.hpp>
Classes | |
| class | PriorityData |
| The PriorityDataIntern class Manages all priority dependent information, i.e. all matrices that have to be resized according to the number of rows per priority. More... | |
Public Member Functions | |
| HierarchicalLSSolver () | |
| virtual | ~HierarchicalLSSolver () |
| bool | configure (const std::vector< int > &n_constraints_per_prio, const unsigned int n_joints) |
| configure Resizes member variables | |
| virtual void | solve (const wbc::HierarchicalQP &hierarchical_qp, base::VectorXd &solver_output) |
| solve Solve the given quadratic program | |
| void | setJointWeights (const base::VectorXd &weights) |
| setJointWeights Sets the joint weight vector for all priorities | |
| void | setJointWeights (const base::VectorXd &weights, const uint prio) |
| setJointWeights Sets the joint weight vector for the given priority | |
| void | setTaskWeights (const base::VectorXd &weights, const uint prio) |
| setTaskWeights Sets the weights for the constraints of the given priority. | |
| void | setMinEigenvalue (double min_eigenvalue) |
| setMinEigenvalue Sets the minimum Eigenvalue that is allowed to occur in normal (undamped) matrix inversion. This matrix inversion is used for nullspace projection. An Eigenvalue is set to zero if it smaller that min_eigenvalue for sake of numerical stability. | |
| double | getMinEigenvalue () |
| void | setMaxSolverOutputNorm (double norm_max) |
| setMaxSolverOutputNorm Sets the maximum norm term. The solution of the solver will have a norm that is below this value. This value will be used to compute a suitable damping factor for matrix inversion. | |
| double | getMaxSolverOutputNorm () |
| bool | isConfigured () |
| Has configure() been called already? | |
 Public Member Functions inherited from wbc::QPSolver | |
| QPSolver () | |
| virtual | ~QPSolver () |
| void | reset () |
| reset Enforces reconfiguration at next call to solve() | |
Protected Attributes | |
| std::vector< PriorityData > | priorities |
| base::MatrixXd | proj_mat |
| base::VectorXd | s_vals |
| base::MatrixXd | s_vals_inv |
| base::MatrixXd | sing_vect_r |
| base::MatrixXd | damped_s_vals_inv |
| base::MatrixXd | Wq_V |
| base::MatrixXd | Wq_V_s_vals_inv |
| base::MatrixXd | Wq_V_damped_s_vals_inv |
| unsigned int | no_of_joints |
| double | min_eigenvalue |
| double | max_solver_output_norm |
| base::VectorXd | tmp |
| Protected Attributes inherited from wbc::QPSolver | |
| bool | configured |
Detailed Description
Implementation of the hierarchical weighted damped least squares solver (HWLS), similar to Schutter, J. et al. “Constraint-based Task Specification and Estimation for Sensor-Based Robot Systems in the Presence of Geometric Uncertainty.” The International Journal of Robotics Research 26 (2007): 433 - 455. It solves the following optimization problem.
\[ \begin{array}{ccc} min(\dot{\mathbf{q}}) & ||\dot{\mathbf{q}}||_2 \\ & & \\ s.t. & \mathbf{A}_{w,1} \dot{\mathbf{q}} = \dot{\mathbf{x}}_1 & \\ & \mathbf{A}_{w,2} \dot{\mathbf{q}} = \dot{\mathbf{x}}_2 & \\ & ... & \\ & \mathbf{A}_{w,N} \mathbf{\dot{q}} = \dot{\mathbf{x}}_N & \\ \end{array} \]
where A is the weighted constraint matrix of priority level i, \(\dot{\mathbf{q}}\) the robot joint velocities, \(\dot{\mathbf{x}}_i\) the desired task space velocities of priority level i. The solver ensures a hierarchy between the different tasks using nullspace projections. That is, the equation system with the highest priority will be solved fully if (n_rows <= n_cols), the eqn. system of the next priority will be solved in the nullspace of the priovious priority, and so on. Additionally the solver can include weights in joint space and task space.
Constructor & Destructor Documentation
◆ HierarchicalLSSolver()
| wbc::HierarchicalLSSolver::HierarchicalLSSolver | ( | ) |
◆ ~HierarchicalLSSolver()
|
virtual |
Member Function Documentation
◆ configure()
| bool wbc::HierarchicalLSSolver::configure | ( | const std::vector< int > & | n_constraints_per_prio, |
| const unsigned int | n_joints ) |
configure Resizes member variables
- Parameters
-
constraint_variables_per_prio Number of constraint variables per priority, i.e. number of row of the constraint Jacobian of that priority no_of_joints Number of robot joints
- Returns
- true in case of successful initialization, false otherwise
◆ getMaxSolverOutputNorm()
|
inline |
Return the maximum norm term.
◆ getMinEigenvalue()
|
inline |
Return the min eigenvalue term.
◆ isConfigured()
|
inline |
Has configure() been called already?
◆ setJointWeights() [1/2]
| void wbc::HierarchicalLSSolver::setJointWeights | ( | const base::VectorXd & | weights | ) |
setJointWeights Sets the joint weight vector for all priorities
- Parameters
-
weights Has to have same size as number of joints. Values have to be >= 0! A values of 0 means that the joint does not contribute to the solution at all.
◆ setJointWeights() [2/2]
| void wbc::HierarchicalLSSolver::setJointWeights | ( | const base::VectorXd & | weights, |
| const uint | prio ) |
setJointWeights Sets the joint weight vector for the given priority
- Parameters
-
weights Has to have same size as number of joints. Values have to be >= 0! A values of 0 means that the joint does not contribute to the solution at all. prio Priority the joint weight vector should be applied to
◆ setMaxSolverOutputNorm()
| void wbc::HierarchicalLSSolver::setMaxSolverOutputNorm | ( | double | norm_max | ) |
setMaxSolverOutputNorm Sets the maximum norm term. The solution of the solver will have a norm that is below this value. This value will be used to compute a suitable damping factor for matrix inversion.
- Parameters
-
norm_max Maximum output norm. Has to be > 0!
◆ setMinEigenvalue()
| void wbc::HierarchicalLSSolver::setMinEigenvalue | ( | double | min_eigenvalue | ) |
setMinEigenvalue Sets the minimum Eigenvalue that is allowed to occur in normal (undamped) matrix inversion. This matrix inversion is used for nullspace projection. An Eigenvalue is set to zero if it smaller that min_eigenvalue for sake of numerical stability.
- Parameters
-
min_eigenvalue Has to be > 0
◆ setTaskWeights()
| void wbc::HierarchicalLSSolver::setTaskWeights | ( | const base::VectorXd & | weights, |
| const uint | prio ) |
setTaskWeights Sets the weights for the constraints of the given priority.
- Parameters
-
weights Size has to be same number of constraint variables of that priority. prio Priority the constraint weight vector should be applied to
◆ solve()
|
virtual |
solve Solve the given quadratic program
- Parameters
-
hierarchical_qp Description of the hierarchical quadratic program to solve. solver_output solution of the quadratic program
Implements wbc::QPSolver.
Member Data Documentation
◆ damped_s_vals_inv
|
protected |
Matrix of right singular vectors
◆ max_solver_output_norm
|
protected |
Precision for eigenvalue inversion. Inverse of an Eigenvalue smaller than this will be set to zero
◆ min_eigenvalue
|
protected |
Number of joints
◆ no_of_joints
|
protected |
Wq_V * damped_s_vals_inv
◆ priorities
|
protected |
◆ proj_mat
|
protected |
Contains priority specific matrices etc.
◆ s_vals
|
protected |
Projection Matrix that performs the nullspace projection onto the next lower priority
◆ s_vals_inv
|
protected |
Singular value vector
◆ sing_vect_r
|
protected |
Diagonal matrix containing the reciprocal singular values
◆ tmp
|
protected |
Maximum norm of (J#) * y
◆ Wq_V
|
protected |
Diagonal matrix containing the reciprocal singular values with damping
◆ Wq_V_damped_s_vals_inv
|
protected |
Wq_V * s_vals_inv
◆ Wq_V_s_vals_inv
|
protected |
Column weight matrix times Matrix of Vectors of right singular vectors
The documentation for this class was generated from the following files:
- src/solvers/hls/HierarchicalLSSolver.hpp
- src/solvers/hls/HierarchicalLSSolver.cpp
Generated by
1.12.0