![]() |
RobWorkProject
23.9.11-
|
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 , 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 | |
![]() | |
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 | |
![]() | |
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... | |
![]() | |
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. | |
![]() | |
virtual | ~InvKinSolver () |
destructor | |
Additional Inherited Members | |
![]() | |
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... | |
![]() | |
IterativeIK () | |
Constructor. | |
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] \)
JacobianIKSolver | ( | rw::core::Ptr< const rw::models::Device > | device, |
const rw::kinematics::State & | state | ||
) |
Constructs JacobianIKSolver for device device.
device | [in] the device to do inverse kinematics for. |
state | [in] the initial state. |
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.
device | [in] the device to do inverse kinematics for. |
foi | [in] end effector frame. |
state | [in] the initial state. |
|
virtual |
Returns the Tool Center Point (TCP) used when solving the IK problem.
Implements InvKinSolver.
|
inlinevirtual |
Specifies whether to check joint limits before returning a solution.
check | [in] If true the method should perform a check that joints are within bounds. |
Implements InvKinSolver.
|
inline |
enables clamping of the solution such that solution always is within joint limits
enableClamping | [in] true to enable clamping, false otherwise |
|
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.
enableInterpolation | [in] set true to enable interpolation, false otherwise |
|
inline |
sets the maximal step length that is allowed on the local search towards the solution.
interpolatorStep | [in] the interpolation step. |
void setJointLimitTolerance | ( | double | tolerance | ) |
setJointLimitTolerance set the tolerance used for bound-checking the solution
tolerance | for joint bounds checking |
|
inline |
set the type of solver to use for stepping toward a solution
type | [in] the type of jacobian solver |
void setWeightVector | ( | Eigen::VectorXd | weights | ) |
setWeightVector sets the weight vector used for solver type "Weighted"
weights | a vector of weights for each degree of freedom, ie weights.size() == DOF |
void setWeightVector | ( | std::vector< double > | weights | ) |
setWeightVector sets the weight vector used for solver type "Weighted"
weights | a vector of weights for each degree of freedom, ie weights.size() == DOF |
|
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) )
baseTend | [in] Desired base to end transformation \( \robabx{}{desired}{\mathbf{T}}\) |
state | [in] State of the device from which to start the iterations |
Implements InvKinSolver.
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.
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 |