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

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

#include <JacobianIKSolver.hpp>

Inherits IterativeIK.

Public Types

enum  JacobianSolverType { Transpose, SVD, DLS, SDLS }
 the type of jacobian solver
 
typedef rw::common::Ptr< JacobianIKSolverPtr
 smart pointer type to this class
 
typedef rw::common::Ptr< const JacobianIKSolverCPtr
 smart pointer type to this const class
 
- Public Types inherited from IterativeIK
typedef rw::common::Ptr< IterativeIKPtr
 smart pointer type to this class
 
typedef rw::common::Ptr< const IterativeIKCPtr
 smart pointer type to this const class
 
- Public Types inherited from InvKinSolver
typedef rw::common::Ptr< InvKinSolverPtr
 smart pointer type to this class
 
typedef rw::common::Ptr< const InvKinSolverCPtr
 smart pointer type to this const class
 

Public Member Functions

 JacobianIKSolver (rw::common::Ptr< const rw::models::Device > device, const kinematics::State &state)
 Constructs JacobianIKSolver for device device. More...
 
 JacobianIKSolver (rw::common::Ptr< const rw::models::Device > device, 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::Qsolve (const math::Transform3D<> &baseTend, const 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 math::Transform3D<> &bTed, double maxError, 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 setCheckJointLimits (bool check)
 Specifies whether to check joint limits before returning a solution. More...
 
virtual rw::common::Ptr< const rw::kinematics::FramegetTCP () 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::common::PropertyMapgetProperties ()
 Returns the PropertyMap. More...
 
virtual const rw::common::PropertyMapgetProperties () 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::common::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::common::Ptr< const rw::models::Device device,
const 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::common::Ptr< const rw::models::Device device,
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::common::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.

◆ 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 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 InvKinSolver.

◆ solveLocal()

bool solveLocal ( const math::Transform3D<> &  bTed,
double  maxError,
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: