RobWorkProject
Public Types | Public Member Functions | List of all members
JacobianIKSolverM Class Reference

A Jacobian based iterative inverse kinematics algorithm for devices with multiple end effectors. More...

#include <JacobianIKSolverM.hpp>

Inherits IterativeMultiIK.

Public Types

enum  JacobianSolverType { Transpose, SVD, DLS, SDLS }
 the type of Jacobian solver
 
- Public Types inherited from IterativeMultiIK
typedef rw::common::Ptr< IterativeMultiIKPtr
 smart pointer type to this class
 

Public Member Functions

 JacobianIKSolverM (const models::TreeDevice *device, const kinematics::State &state)
 Constructs JacobianIKSolverM for TreeDevice. Uses the default end effectors of the TreeDevice.
 
 JacobianIKSolverM (const models::JointDevice *device, const std::vector< kinematics::Frame *> &foi, const kinematics::State &state)
 Constructs JacobianIKSolverM for a JointDevice(SerialDevice and TreeDevice). It does not use the default end effectors. A list of interest frames are given instead.
 
virtual ~JacobianIKSolverM ()
 destructor
 
void setReturnBestFit (bool returnBestFit)
 configures the iterative solver to return the best fit found, even though error criteria was not met. More...
 
void setClampToBounds (bool enableClamping)
 enables clamping of the solution such that solution always is within joint limits. 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...
 
void setSolverType (JacobianSolverType type)
 set the type of solver to use for stepping toward a solution More...
 
std::vector< math::Qsolve (const std::vector< math::Transform3D<> > &baseTend, const kinematics::State &state) const
 Calculates the inverse kinematics. More...
 
bool solveLocal (const std::vector< rw::math::Transform3D<> > &bTed, std::vector< double > &maxError, kinematics::State &state, int maxIter, bool untilSmallChange=false) const
 performs a local search toward the the target bTed. No via points are generated to support the convergence and robustness. More...
 
void setMaxLocalStep (double qlength, double plength)
 sets the maximal step length that is allowed on the local search towards the solution. More...
 
- Public Member Functions inherited from IterativeMultiIK
virtual ~IterativeMultiIK ()
 Destructor.
 
virtual void setMaxError (const std::vector< double > &maxError)
 Sets the maximal error for a solution. More...
 
virtual std::vector< 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::common::PropertyMapgetProperties ()
 Returns the PropertyMap. More...
 
virtual const rw::common::PropertyMapgetProperties () const
 Returns the PropertyMap return Reference to the PropertyMap.
 

Additional Inherited Members

- Protected Member Functions inherited from IterativeMultiIK
 IterativeMultiIK (size_t nrOfEndEff)
 Constructor.
 

Detailed Description

A Jacobian based iterative inverse kinematics algorithm for devices with multiple end effectors.

This algorithm does not 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] $

Member Function Documentation

◆ 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

◆ setMaxLocalStep()

void setMaxLocalStep ( double  qlength,
double  plength 
)

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

Parameters
qlength[in] maximal step length in quaternion
plength[in] maximal step length in position

◆ setReturnBestFit()

void setReturnBestFit ( bool  returnBestFit)
inline

configures the iterative solver to return the best fit found, even though error criteria was not met.

Parameters
returnBestFit[in] set to true if you want best fit returned.

◆ 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

◆ solve()

std::vector<math::Q> solve ( const std::vector< math::Transform3D<> > &  baseTend,
const 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 IterativeMultiIK.

◆ solveLocal()

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

performs a local search toward the 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
untilSmallChange[in] if true the error difference between two successive iterations need to be smaller than maxError. If false then the error of a iteration need to be smaller than maxError.
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: