Weighted and Regularized Pseudo-Inverse Inverse Kinematics Solver The solver solves a weighted and regularised pseudo-inverse problem. It uses backtracking line-search and adaptive regularization. More...

#include <ik_solver.h>

Inheritance diagram for exotica::IKSolver:

[legend]

Public Member Functions
void	Solve (Eigen::MatrixXd &solution) override

void	SpecifyProblem (PlanningProblemPtr pointer) override

Public Member Functions inherited from exotica::MotionSolver
int	GetNumberOfMaxIterations ()

double	GetPlanningTime ()

PlanningProblemPtr	GetProblem () const

void	InstantiateBase (const Initializer &init) override

	MotionSolver ()=default

std::string	Print (const std::string &prepend) const override

void	SetNumberOfMaxIterations (int max_iter)

virtual	~MotionSolver ()=default

Public Member Functions inherited from exotica::Object
std::string	GetObjectName ()

void	InstantiateObject (const Initializer &init)

	Object ()

virtual std::string	type () const

virtual	~Object ()

Public Member Functions inherited from exotica::InstantiableBase
virtual std::vector< Initializer >	GetAllTemplates () const=0

	InstantiableBase ()=default

virtual	~InstantiableBase ()=default

Public Member Functions inherited from exotica::Instantiable< IKSolverInitializer >
std::vector< Initializer >	GetAllTemplates () const override

Initializer	GetInitializerTemplate () override

const C &	GetParameters () const

virtual void	Instantiate (const C &init)

void	InstantiateInternal (const Initializer &init) override

Private Member Functions
void	DecreaseRegularization ()

void	IncreaseRegularization ()

void	PrintDebug (const int i)

void	ScaleToStepSize (Eigen::VectorXdRef xd)
	To scale to maximum step size. More...

Private Attributes
Eigen::VectorXd	alpha_space_
	Steplengths for backtracking line-search. More...

Eigen::MatrixXd	cost_jacobian_
	Jacobian, used during optimisation. More...

double	error_
	Error, used during optimisation. More...

double	error_prev_
	Error at previous iteration, used during optimisation. More...

Eigen::LLT< Eigen::MatrixXd >	J_decomposition_
	Cholesky decomposition for the weighted pseudo-inverse. More...

Eigen::MatrixXd	J_pseudo_inverse_
	Jacobian pseudo-inverse, used during optimisation. More...

Eigen::MatrixXd	J_tmp_
	Temporary variable for inverse computation. More...

double	lambda_ = 0
	Damping factor. More...

UnconstrainedEndPoseProblemPtr	prob_

Eigen::VectorXd	q_
	Joint configuration vector, used during optimisation. More...

Eigen::VectorXd	qd_
	Change in joint configuration, used during optimisation. More...

double	regfactor_ = 10.
	Factor by which the regularization gets increased/decreased. More...

double	regmax_ = 1e9
	Maximum regularization (to exit by divergence) More...

double	regmin_ = 1e-9
	Minimum regularization (will not decrease lower) More...

double	step_
	Size of step: Sum of squared norm of qd_. More...

double	steplength_
	Accepted steplength. More...

double	stop_
	Stop criterion: Norm of cost Jacobian. More...

double	th_stepdec_ = 0.5
	Step-length threshold used to decrease regularization. More...

double	th_stepinc_ = 0.1
	Step-length threshold used to increase regularization. More...

double	th_stop_
	Gradient convergence threshold. More...

Eigen::MatrixXd	W_inv_
	Joint-space weighting (inverse) More...

Eigen::VectorXd	yd_
	Task space difference/error, used during optimisation. More...

Additional Inherited Members
Public Attributes inherited from exotica::Object
bool	debug_

std::string	ns_

std::string	object_name_

Protected Attributes inherited from exotica::MotionSolver
int	max_iterations_

double	planning_time_

PlanningProblemPtr	problem_

Protected Attributes inherited from exotica::Instantiable< IKSolverInitializer >
C	parameters_

Detailed Description

Weighted and Regularized Pseudo-Inverse Inverse Kinematics Solver The solver solves a weighted and regularised pseudo-inverse problem. It uses backtracking line-search and adaptive regularization.

Definition at line 44 of file ik_solver.h.

Member Function Documentation

◆ DecreaseRegularization()

void exotica::IKSolver::DecreaseRegularization ( )

inlineprivate

Definition at line 90 of file ik_solver.h.

◆ IncreaseRegularization()

void exotica::IKSolver::IncreaseRegularization ( )

inlineprivate

Definition at line 81 of file ik_solver.h.

◆ PrintDebug()

void exotica::IKSolver::PrintDebug ( const int i )

inlineprivate

Definition at line 99 of file ik_solver.h.

◆ ScaleToStepSize()

void exotica::IKSolver::ScaleToStepSize ( Eigen::VectorXdRef xd )

private

To scale to maximum step size.

Definition at line 238 of file ik_solver.cpp.

◆ Solve()

void exotica::IKSolver::Solve ( Eigen::MatrixXd & solution )

overridevirtual

Implements exotica::MotionSolver.

Definition at line 74 of file ik_solver.cpp.

◆ SpecifyProblem()

void exotica::IKSolver::SpecifyProblem ( PlanningProblemPtr pointer )

overridevirtual

Reimplemented from exotica::MotionSolver.

Definition at line 36 of file ik_solver.cpp.

Member Data Documentation

◆ alpha_space_

Eigen::VectorXd exotica::IKSolver::alpha_space_

private

Steplengths for backtracking line-search.

Definition at line 54 of file ik_solver.h.

◆ cost_jacobian_

Eigen::MatrixXd exotica::IKSolver::cost_jacobian_

private

Jacobian, used during optimisation.

Definition at line 64 of file ik_solver.h.

◆ error_

double exotica::IKSolver::error_

private

Error, used during optimisation.

Definition at line 66 of file ik_solver.h.

◆ error_prev_

double exotica::IKSolver::error_prev_

private

Error at previous iteration, used during optimisation.

Definition at line 67 of file ik_solver.h.

◆ J_decomposition_

Eigen::LLT<Eigen::MatrixXd> exotica::IKSolver::J_decomposition_

private

Cholesky decomposition for the weighted pseudo-inverse.

Definition at line 68 of file ik_solver.h.

◆ J_pseudo_inverse_

Eigen::MatrixXd exotica::IKSolver::J_pseudo_inverse_

private

Jacobian pseudo-inverse, used during optimisation.

Definition at line 65 of file ik_solver.h.

◆ J_tmp_

Eigen::MatrixXd exotica::IKSolver::J_tmp_

private

Temporary variable for inverse computation.

Definition at line 69 of file ik_solver.h.

◆ lambda_

double exotica::IKSolver::lambda_ = 0

private

Damping factor.

Definition at line 59 of file ik_solver.h.

◆ prob_

UnconstrainedEndPoseProblemPtr exotica::IKSolver::prob_

private

Definition at line 51 of file ik_solver.h.

◆ q_

Eigen::VectorXd exotica::IKSolver::q_

private

Joint configuration vector, used during optimisation.

Definition at line 61 of file ik_solver.h.

◆ qd_

Eigen::VectorXd exotica::IKSolver::qd_

private

Change in joint configuration, used during optimisation.

Definition at line 62 of file ik_solver.h.

◆ regfactor_

double exotica::IKSolver::regfactor_ = 10.

private

Factor by which the regularization gets increased/decreased.

Definition at line 77 of file ik_solver.h.

◆ regmax_

double exotica::IKSolver::regmax_ = 1e9

private

Maximum regularization (to exit by divergence)

Definition at line 76 of file ik_solver.h.

◆ regmin_

double exotica::IKSolver::regmin_ = 1e-9

private

Minimum regularization (will not decrease lower)

Definition at line 75 of file ik_solver.h.

◆ step_

double exotica::IKSolver::step_

private

Size of step: Sum of squared norm of qd_.

Definition at line 58 of file ik_solver.h.

◆ steplength_

double exotica::IKSolver::steplength_

private

Accepted steplength.

Definition at line 60 of file ik_solver.h.

◆ stop_

double exotica::IKSolver::stop_

private

Stop criterion: Norm of cost Jacobian.

Definition at line 57 of file ik_solver.h.

◆ th_stepdec_

double exotica::IKSolver::th_stepdec_ = 0.5

private

Step-length threshold used to decrease regularization.

Definition at line 78 of file ik_solver.h.

◆ th_stepinc_

double exotica::IKSolver::th_stepinc_ = 0.1

private

Step-length threshold used to increase regularization.

Definition at line 79 of file ik_solver.h.

◆ th_stop_

double exotica::IKSolver::th_stop_

private

Gradient convergence threshold.

Definition at line 72 of file ik_solver.h.

◆ W_inv_

Eigen::MatrixXd exotica::IKSolver::W_inv_

private

Joint-space weighting (inverse)

Definition at line 53 of file ik_solver.h.

◆ yd_

Eigen::VectorXd exotica::IKSolver::yd_

private

Task space difference/error, used during optimisation.

Definition at line 63 of file ik_solver.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ DecreaseRegularization()

◆ IncreaseRegularization()

◆ PrintDebug()

◆ ScaleToStepSize()

◆ Solve()

◆ SpecifyProblem()

Member Data Documentation

◆ alpha_space_

◆ cost_jacobian_

◆ error_

◆ error_prev_

◆ J_decomposition_

◆ J_pseudo_inverse_

◆ J_tmp_

◆ lambda_

◆ prob_

◆ q_

◆ qd_

◆ regfactor_

◆ regmax_

◆ regmin_

◆ step_

◆ steplength_

◆ stop_

◆ th_stepdec_

◆ th_stepinc_

◆ th_stop_

◆ W_inv_

◆ yd_