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:

Collaboration diagram for exotica::IKSolver:

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

void	SpecifyProblem (PlanningProblemPtr pointer) override

Public Member Functions inherited from exotica::MotionSolver
	MotionSolver ()=default

virtual	~MotionSolver ()=default

void	InstantiateBase (const Initializer &init) override

PlanningProblemPtr	GetProblem () const

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

void	SetNumberOfMaxIterations (int max_iter)

int	GetNumberOfMaxIterations ()

double	GetPlanningTime ()

Public Member Functions inherited from exotica::Object
	Object ()

virtual	~Object ()

virtual std::string	type () const
	Type Information wrapper: must be virtual so that it is polymorphic... More...

std::string	GetObjectName ()

void	InstantiateObject (const Initializer &init)

Public Member Functions inherited from exotica::InstantiableBase
	InstantiableBase ()=default

virtual	~InstantiableBase ()=default

virtual std::vector< Initializer >	GetAllTemplates () const =0

Public Member Functions inherited from exotica::Instantiable< IKSolverInitializer >
void	InstantiateInternal (const Initializer &init) override

Initializer	GetInitializerTemplate () override

std::vector< Initializer >	GetAllTemplates () const override

virtual void	Instantiate (const IKSolverInitializer &init)

const IKSolverInitializer &	GetParameters () const

Private Member Functions
void	IncreaseRegularization ()

void	DecreaseRegularization ()

void	PrintDebug (const int i)

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

Private Attributes
UnconstrainedEndPoseProblemPtr	prob_

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

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

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

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

double	lambda_ = 0
	Damping factor. More...

double	steplength_
	Accepted steplength. More...

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

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

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

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

Eigen::MatrixXd	J_pseudo_inverse_
	Jacobian pseudo-inverse, 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_tmp_
	Temporary variable for inverse computation. More...

double	th_stop_
	Gradient convergence threshold. More...

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

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

double	regfactor_ = 10.
	Factor by which the regularization gets increased/decreased. 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...

Additional Inherited Members
Public Attributes inherited from exotica::Object
std::string	ns_

std::string	object_name_

bool	debug_

Protected Attributes inherited from exotica::MotionSolver
PlanningProblemPtr	problem_

double	planning_time_ = -1

int	max_iterations_ = 100

Protected Attributes inherited from exotica::Instantiable< IKSolverInitializer >
IKSolverInitializer	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.

Member Function Documentation

◆ DecreaseRegularization()

void exotica::IKSolver::DecreaseRegularization ( )

inlineprivate

◆ IncreaseRegularization()

void exotica::IKSolver::IncreaseRegularization ( )

inlineprivate

◆ PrintDebug()

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

inlineprivate

◆ ScaleToStepSize()

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

private

To scale to maximum step size.

◆ Solve()

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

overridevirtual

Implements exotica::MotionSolver.

◆ SpecifyProblem()

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

overridevirtual

Reimplemented from exotica::MotionSolver.

Member Data Documentation

◆ alpha_space_

Eigen::VectorXd exotica::IKSolver::alpha_space_

private

Steplengths for backtracking line-search.

◆ cost_jacobian_

Eigen::MatrixXd exotica::IKSolver::cost_jacobian_

private

Jacobian, used during optimisation.

◆ error_

double exotica::IKSolver::error_

private

Error, used during optimisation.

◆ error_prev_

double exotica::IKSolver::error_prev_

private

Error at previous iteration, used during optimisation.

◆ J_decomposition_

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

private

Cholesky decomposition for the weighted pseudo-inverse.

◆ J_pseudo_inverse_

Eigen::MatrixXd exotica::IKSolver::J_pseudo_inverse_

private

Jacobian pseudo-inverse, used during optimisation.

◆ J_tmp_

Eigen::MatrixXd exotica::IKSolver::J_tmp_

private

Temporary variable for inverse computation.

◆ lambda_

double exotica::IKSolver::lambda_ = 0

private

Damping factor.

◆ prob_

UnconstrainedEndPoseProblemPtr exotica::IKSolver::prob_

private

◆ q_

Eigen::VectorXd exotica::IKSolver::q_

private

Joint configuration vector, used during optimisation.

◆ qd_

Eigen::VectorXd exotica::IKSolver::qd_

private

Change in joint configuration, used during optimisation.

◆ regfactor_

double exotica::IKSolver::regfactor_ = 10.

private

Factor by which the regularization gets increased/decreased.

◆ regmax_

double exotica::IKSolver::regmax_ = 1e9

private

Maximum regularization (to exit by divergence)

◆ regmin_

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

private

Minimum regularization (will not decrease lower)

◆ step_

double exotica::IKSolver::step_

private

Size of step: Sum of squared norm of qd_.

◆ steplength_

double exotica::IKSolver::steplength_

private

Accepted steplength.

◆ stop_

double exotica::IKSolver::stop_

private

Stop criterion: Norm of cost Jacobian.

◆ th_stepdec_

double exotica::IKSolver::th_stepdec_ = 0.5

private

Step-length threshold used to decrease regularization.

◆ th_stepinc_

double exotica::IKSolver::th_stepinc_ = 0.1

private

Step-length threshold used to increase regularization.

◆ th_stop_

double exotica::IKSolver::th_stop_

private

Gradient convergence threshold.

◆ W_inv_

Eigen::MatrixXd exotica::IKSolver::W_inv_

private

Joint-space weighting (inverse)

◆ yd_

Eigen::VectorXd exotica::IKSolver::yd_

private

Task space difference/error, used during optimisation.

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

/tmp/exotica/exotations/solvers/exotica_ik_solver/include/exotica_ik_solver/ik_solver.h

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_