A Jacobian based iterative inverse kinematics algorithm for devices with a single end effector. More...

#include <JacobianIKSolver.hpp>

Public Types
enum	JacobianSolverType { Transpose , SVD , DLS , Weighted }
	the type of jacobian solver

typedef rw::core::Ptr< JacobianIKSolver >	Ptr
	smart pointer type to this class

typedef rw::core::Ptr< const JacobianIKSolver >	CPtr
	smart pointer type to this const class

Public Types inherited from IterativeIK
typedef rw::core::Ptr< IterativeIK >	Ptr
	smart pointer type to this class

typedef rw::core::Ptr< const IterativeIK >	CPtr
	smart pointer type to this const class

Public Types inherited from InvKinSolver
typedef rw::core::Ptr< InvKinSolver >	Ptr
	smart pointer type to this class

typedef rw::core::Ptr< const InvKinSolver >	CPtr
	smart pointer type to this const class

Public Member Functions
	JacobianIKSolver (rw::core::Ptr< const rw::models::Device > device, const rw::kinematics::State &state)
	Constructs JacobianIKSolver for device device. More...

	JacobianIKSolver (rw::core::Ptr< const rw::models::Device > device, rw::core::Ptr< const rw::kinematics::Frame > foi, const kinematics::State &state)
	Constructs JacobianIKSolver for device, where the frame foi will be used as end effector. More...

std::vector< math::Q >	solve (const rw::math::Transform3D< double > &baseTend, const rw::kinematics::State &state) const
	Calculates the inverse kinematics. More...

void	setInterpolatorStep (double interpolatorStep)
	sets the maximal step length that is allowed on the local search towards the solution. More...

void	setEnableInterpolation (bool enableInterpolation)
	the solver may fail or be very slow if the the solution is too far from the initial configuration. This function enables the use of via points generated using an interpolation from initial end effector configuration to the goal target. More...

bool	solveLocal (const rw::math::Transform3D< double > &bTed, double maxError, rw::kinematics::State &state, int maxIter) const
	performs a local search toward the target bTed. No via points are generated to support the convergence and robustness. More...

void	setClampToBounds (bool enableClamping)
	enables clamping of the solution such that solution always is within joint limits More...

void	setSolverType (JacobianSolverType type)
	set the type of solver to use for stepping toward a solution More...

void	setWeightVector (Eigen::VectorXd weights)
	setWeightVector sets the weight vector used for solver type "Weighted" More...

void	setWeightVector (std::vector< double > weights)
	setWeightVector sets the weight vector used for solver type "Weighted" More...

void	setJointLimitTolerance (double tolerance)
	setJointLimitTolerance set the tolerance used for bound-checking the solution More...

void	setCheckJointLimits (bool check)
	Specifies whether to check joint limits before returning a solution. More...

virtual rw::core::Ptr< const rw::kinematics::Frame >	getTCP () const
	Returns the Tool Center Point (TCP) used when solving the IK problem. More...

Public Member Functions inherited from IterativeIK
virtual	~IterativeIK ()
	Destructor.

virtual void	setMaxError (double maxError)
	Sets the maximal error for a solution. More...

virtual double	getMaxError () const
	Returns the maximal error for a solution. More...

virtual void	setMaxIterations (int maxIterations)
	Sets the maximal number of iterations allowed. More...

virtual int	getMaxIterations () const
	Returns the maximal number of iterations.

virtual rw::core::PropertyMap &	getProperties ()
	Returns the PropertyMap. More...

virtual const rw::core::PropertyMap &	getProperties () const
	Returns the PropertyMap return Reference to the PropertyMap.

Public Member Functions inherited from InvKinSolver
virtual	~InvKinSolver ()
	destructor

Additional Inherited Members
Static Public Member Functions inherited from IterativeIK
static IterativeIK::Ptr	makeDefault (rw::core::Ptr< rw::models::Device > device, const rw::kinematics::State &state)
	Default iterative inverse kinematics solver for a device and state. More...

Protected Member Functions inherited from IterativeIK
	IterativeIK ()
	Constructor.

Detailed Description

A Jacobian based iterative inverse kinematics algorithm for devices with a single end effector.

This algorithm does implicitly handle joint limits, however it is possible to force the solution within joint limits using clamping in each iterative step. If joint clamping is not enabled then this algorithm might contain joint values that are out of bounds.

The method uses an Newton-Raphson iterative approach and is based on using the inverse of the device Jacobian to compute each local solution in each iteration. Several methods for calculating/approximating the inverse Jacobian are available, where the SVD method currently is the most stable, see the JacobianSolverType option for additional options.

The method is based on the following approximation:

\( \Delta \mathbf{x}\approx \mathbf{J}(\mathbf{q})\Delta \mathbf{q} \)

Where \( \Delta \mathbf{x} \) is calculated like:

\( \robabx{b}{e}{\mathbf{T}}(\mathbf{q}) = \robabx{b}{e}{\mathbf{T}}(\mathbf{q}_{init}+\Delta \mathbf{q}) \approx \robabx{b}{e}{\mathbf{T}}(\mathbf{q}_{init})\Delta \mathbf{T}(\Delta \mathbf{q}) = \robabx{b}{e*}{\mathbf{T}} \)

\( \Delta \mathbf{T}(\Delta \mathbf{q}) = \robabx{j}{i}{\mathbf{T}}\robabx{b}{e*}{\mathbf{T}}=\mathbf{I}^{4x4}+\mathbf{L} \)

\( \mathbf{L} = \left[ \begin{array}{cccc} 0 & -\omega_z & \omega_y & v_x \\ \omega_z & 0 & -\omega_x & v_y \\ -\omega_y & \omega_x & 0 & v_z \\ 0 & 0 & 0 & 0 \end{array} \right] \)

Constructor & Destructor Documentation

◆ JacobianIKSolver() [1/2]

JacobianIKSolver	(	rw::core::Ptr< const rw::models::Device >	device,
		const rw::kinematics::State &	state
	)

Constructs JacobianIKSolver for device device.

Parameters

device	[in] the device to do inverse kinematics for.
state	[in] the initial state.

◆ JacobianIKSolver() [2/2]

JacobianIKSolver	(	rw::core::Ptr< const rw::models::Device >	device,
		rw::core::Ptr< const rw::kinematics::Frame >	foi,
		const kinematics::State &	state
	)

Constructs JacobianIKSolver for device, where the frame foi will be used as end effector.

Parameters

device	[in] the device to do inverse kinematics for.
foi	[in] end effector frame.
state	[in] the initial state.

Member Function Documentation

◆ getTCP()

virtual rw::core::Ptr<const rw::kinematics::Frame> getTCP ( ) const

virtual

Returns the Tool Center Point (TCP) used when solving the IK problem.

Returns: The TCP Frame used when solving the IK.

Implements InvKinSolver.

◆ setCheckJointLimits()

void setCheckJointLimits ( bool check )

inlinevirtual

Specifies whether to check joint limits before returning a solution.

Parameters

check [in] If true the method should perform a check that joints are within bounds.

Implements InvKinSolver.

◆ setClampToBounds()

void setClampToBounds ( bool enableClamping )

inline

enables clamping of the solution such that solution always is within joint limits

Parameters

enableClamping [in] true to enable clamping, false otherwise

◆ setEnableInterpolation()

void setEnableInterpolation ( bool enableInterpolation )

inline

the solver may fail or be very slow if the the solution is too far from the initial configuration. This function enables the use of via points generated using an interpolation from initial end effector configuration to the goal target.

Parameters

enableInterpolation [in] set true to enable interpolation, false otherwise

◆ setInterpolatorStep()

void setInterpolatorStep ( double interpolatorStep )

inline

sets the maximal step length that is allowed on the local search towards the solution.

Parameters

interpolatorStep [in] the interpolation step.

◆ setJointLimitTolerance()

void setJointLimitTolerance ( double tolerance )

setJointLimitTolerance set the tolerance used for bound-checking the solution

Parameters

tolerance for joint bounds checking

◆ setSolverType()

void setSolverType ( JacobianSolverType type )

inline

set the type of solver to use for stepping toward a solution

Parameters

type	[in] the type of jacobian solver

◆ setWeightVector() [1/2]

void setWeightVector ( Eigen::VectorXd weights )

setWeightVector sets the weight vector used for solver type "Weighted"

Parameters

weights a vector of weights for each degree of freedom, ie weights.size() == DOF

◆ setWeightVector() [2/2]

void setWeightVector ( std::vector< double > weights )

setWeightVector sets the weight vector used for solver type "Weighted"

Parameters

weights a vector of weights for each degree of freedom, ie weights.size() == DOF

◆ solve()

std::vector<math::Q> solve	(	const rw::math::Transform3D< double > &	baseTend,
		const rw::kinematics::State &	state
	)		const

virtual

Calculates the inverse kinematics.

Given a desired \(\robabx{}{desired}{\mathbf{T}}\) and the current state, the method solves the inverse kinematics problem.

If the algorithm is able to identify multiple solutions (e.g. elbow up and down) it will return all of these. Before returning a solution, they may be checked to be within the bounds of the configuration space. (See setCheckJointLimits(bool) )

Parameters

baseTend	[in] Desired base to end transformation \( \robabx{}{desired}{\mathbf{T}}\)
state	[in] State of the device from which to start the iterations

Returns: List of solutions. Notice that the list may be empty.

Note: The targets baseTend must be defined relative to the base of the robot/device.

Implements InvKinSolver.

◆ solveLocal()

bool solveLocal	(	const rw::math::Transform3D< double > &	bTed,
		double	maxError,
		rw::kinematics::State &	state,
		int	maxIter
	)		const

performs a local search toward the target bTed. No via points are generated to support the convergence and robustness.

Parameters

bTed	[in] the target end pose
maxError	[in] the maximal allowed error
state	[in/out] the starting position for the search. The end position will also be saved in this state.
maxIter	[in] max number of iterations

Returns: true if error is below max error

Note: the result will be saved in state

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

JacobianIKSolver.hpp

Public Types

Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ JacobianIKSolver() [1/2]

◆ JacobianIKSolver() [2/2]

Member Function Documentation

◆ getTCP()

◆ setCheckJointLimits()

◆ setClampToBounds()

◆ setEnableInterpolation()

◆ setInterpolatorStep()

◆ setJointLimitTolerance()

◆ setSolverType()

◆ setWeightVector() [1/2]

◆ setWeightVector() [2/2]

◆ solve()

◆ solveLocal()