RobWorkProject  23.9.11-
Classes | Public Types | Public Member Functions | List of all members
ParallelIKSolver Class Reference

Inverse kinematics method for parallel devices. More...

#include <ParallelIKSolver.hpp>

Inherits IterativeIK.

Classes

struct  Target
 A target definition used in the multi-target solve function. More...
 

Public Types

typedef rw::core::Ptr< ParallelIKSolverPtr
 smart pointer type to this class
 
- Public Types inherited from IterativeIK
typedef rw::core::Ptr< IterativeIKPtr
 smart pointer type to this class
 
typedef rw::core::Ptr< const IterativeIKCPtr
 smart pointer type to this const class
 
- Public Types inherited from InvKinSolver
typedef rw::core::Ptr< InvKinSolverPtr
 smart pointer type to this class
 
typedef rw::core::Ptr< const InvKinSolverCPtr
 smart pointer type to this const class
 

Public Member Functions

 ParallelIKSolver (const rw::models::ParallelDevice *device)
 Construct new solver. More...
 
virtual ~ParallelIKSolver ()
 Destructor.
 
virtual std::vector< rw::math::Qsolve (const rw::math::Transform3D< double > &baseTend, const kinematics::State &state) const
 Calculates the inverse kinematics. More...
 
virtual std::vector< rw::math::Qsolve (const std::vector< Target > &targets, const rw::kinematics::State &state) const
 Version of solve that allows for definition of multiple targets. More...
 
virtual void setCheckJointLimits (bool check)
 Specifies whether to check joint limits before returning a solution. More...
 
void setClampToBounds (bool enableClamping)
 enables clamping of the solution such that solution always is within joint limits More...
 
virtual rw::core::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::core::PropertyMapgetProperties ()
 Returns the PropertyMap. More...
 
virtual const rw::core::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::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

Inverse kinematics method for parallel devices.

The method is based on solving for two simultaneous constraints. First, the junctions defined in the ParallelDevice must remain connected. Second, the target(s) given by the user should be fulfilled.

A stacked Jacobian is used to form an equation system that includes these objectives. The Singular Value Decomposition is used to find the solution for the joint values in this equation system.

Constructor & Destructor Documentation

◆ ParallelIKSolver()

Construct new solver.

Parameters
device[in] pointer to the parallel device.

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()

virtual void setCheckJointLimits ( bool  check)
virtual

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)

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

Parameters
enableClamping[in] true to enable clamping, false otherwise

◆ solve() [1/2]

virtual std::vector<rw::math::Q> solve ( const rw::math::Transform3D< double > &  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.

This algorithm will return a maximum of one solution, but only if it is able to find one. Before returning a solution, it 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 with one or zero solutions.
Note
The targets baseTend must be defined relative to the base of the robot/device. For a ParallelDevice loaded from an XML file, the base and end frames will normally be the first and last frames of the first leg of the first junction.

Implements InvKinSolver.

◆ solve() [2/2]

virtual std::vector<rw::math::Q> solve ( const std::vector< Target > &  targets,
const rw::kinematics::State state 
) const
virtual

Version of solve that allows for definition of multiple targets.

This is a more flexible version of solve than the one from the standard InvKinSolver interface. As an example, it is useful to define multiple targets for a gripper where each of the fingers can have different target configurations. Furthermore, targets can be defined for different frames, and relative to different frames. There should always be a minimum of one joint between the base and end frames, and the end frame should be in the child tree of the given base frame. The reference frame given must either lie in one of the legs of the junctions in the ParallelDevice, or it must be in the parent path in the frame structure. Finally, the target definitions do not need to be defined as full 6 DOF constraints. It is possible to specify that the constraint should only be defined in some positional or angular directions. In some parallel structures this is very useful, as it might not be possible to do inverse kinematics with full 6 DOF constraints.

Parameters
targets[in] list of targets.
state[in] state containing the current configuration of the device.
Returns
vector with one solution if found, otherwise vector is empty.

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