30 #ifndef EXOTICA_IK_SOLVER_IK_SOLVER_H_ 31 #define EXOTICA_IK_SOLVER_IK_SOLVER_H_ 36 #include <exotica_ik_solver/ik_solver_initializer.h> 47 void Solve(Eigen::MatrixXd& solution)
override;
84 if (lambda_ > regmax_)
93 if (lambda_ < regmin_)
101 if (i % 10 == 0 || i == 0)
103 std::cout <<
"iter \t cost \t stop \t grad \t reg \t step\n";
106 std::cout << std::setw(4) << i <<
" ";
107 std::cout << std::scientific << std::setprecision(5) << error_ <<
" ";
108 std::cout << step_ <<
" " << stop_ <<
" ";
109 std::cout << lambda_ <<
" ";
110 std::cout << std::fixed << std::setprecision(4) << steplength_ <<
'\n';
117 #endif // EXOTICA_IK_SOLVER_IK_SOLVER_H_ double regmax_
Maximum regularization (to exit by divergence)
void ScaleToStepSize(Eigen::VectorXdRef xd)
To scale to maximum step size.
double th_stepinc_
Step-length threshold used to increase regularization.
double steplength_
Accepted steplength.
Eigen::Ref< Eigen::VectorXd > VectorXdRef
void PrintDebug(const int i)
double regmin_
Minimum regularization (will not decrease lower)
Eigen::MatrixXd J_pseudo_inverse_
Jacobian pseudo-inverse, used during optimisation.
double th_stop_
Gradient convergence threshold.
void DecreaseRegularization()
void IncreaseRegularization()
Eigen::MatrixXd J_tmp_
Temporary variable for inverse computation.
UnconstrainedEndPoseProblemPtr prob_
Eigen::MatrixXd W_inv_
Joint-space weighting (inverse)
Eigen::VectorXd alpha_space_
Steplengths for backtracking line-search.
Eigen::LLT< Eigen::MatrixXd > J_decomposition_
Cholesky decomposition for the weighted pseudo-inverse.
std::shared_ptr< exotica::UnconstrainedEndPoseProblem > UnconstrainedEndPoseProblemPtr
Eigen::VectorXd qd_
Change in joint configuration, used during optimisation.
double th_stepdec_
Step-length threshold used to decrease regularization.
void Solve(Eigen::MatrixXd &solution) override
Weighted and Regularized Pseudo-Inverse Inverse Kinematics Solver The solver solves a weighted and re...
Eigen::VectorXd yd_
Task space difference/error, used during optimisation.
std::shared_ptr< PlanningProblem > PlanningProblemPtr
void SpecifyProblem(PlanningProblemPtr pointer) override
double error_prev_
Error at previous iteration, used during optimisation.
double regfactor_
Factor by which the regularization gets increased/decreased.
double step_
Size of step: Sum of squared norm of qd_.
double error_
Error, used during optimisation.
Eigen::MatrixXd cost_jacobian_
Jacobian, used during optimisation.
double stop_
Stop criterion: Norm of cost Jacobian.
double lambda_
Damping factor.
Eigen::VectorXd q_
Joint configuration vector, used during optimisation.