Package org.robwork.sdurw_invkin
Class ParallelIKSolver
- java.lang.Object
-
- org.robwork.sdurw_invkin.InvKinSolver
-
- org.robwork.sdurw_invkin.IterativeIK
-
- org.robwork.sdurw_invkin.ParallelIKSolver
-
public class ParallelIKSolver extends IterativeIK
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.
-
-
Nested Class Summary
Nested Classes Modifier and Type Class Description static class
ParallelIKSolver.Target
A target definition used in the multi-target solve function.
-
Constructor Summary
Constructors Constructor Description ParallelIKSolver(long cPtr, boolean cMemoryOwn)
ParallelIKSolver(ParallelDevice device)
Construct new solver.
-
Method Summary
All Methods Static Methods Instance Methods Concrete Methods Modifier and Type Method Description void
delete()
static long
getCPtr(ParallelIKSolver obj)
FrameCPtr
getTCP()
Returns the Tool Center Point (TCP) used when solving the IK problem.
void
setCheckJointLimits(boolean check)
Specifies whether to check joint limits before returning a solution.
void
setClampToBounds(boolean enableClamping)
enables clamping of the solution such that solution always is within joint limitsVectorQ
solve(SWIGTYPE_p_std__vectorT_rw__invkin__ParallelIKSolver__Target_t targets, State state)
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.VectorQ
solve(Transform3D baseTend, State state)
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.-
Methods inherited from class org.robwork.sdurw_invkin.IterativeIK
getCPtr, getMaxError, getMaxIterations, getProperties, makeDefault, setMaxError, setMaxIterations
-
Methods inherited from class org.robwork.sdurw_invkin.InvKinSolver
getCPtr
-
-
-
-
Constructor Detail
-
ParallelIKSolver
public ParallelIKSolver(long cPtr, boolean cMemoryOwn)
-
ParallelIKSolver
public ParallelIKSolver(ParallelDevice device)
Construct new solver.- Parameters:
device
- [in] pointer to the parallel device.
-
-
Method Detail
-
getCPtr
public static long getCPtr(ParallelIKSolver obj)
-
delete
public void delete()
- Overrides:
delete
in classIterativeIK
-
solve
public VectorQ solve(Transform3D baseTend, State state)
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) )
- Overrides:
solve
in classInvKinSolver
- 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.
-
solve
public VectorQ solve(SWIGTYPE_p_std__vectorT_rw__invkin__ParallelIKSolver__Target_t targets, State state)
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.
-
setCheckJointLimits
public void setCheckJointLimits(boolean check)
Description copied from class:InvKinSolver
Specifies whether to check joint limits before returning a solution.
- Overrides:
setCheckJointLimits
in classInvKinSolver
- Parameters:
check
- [in] If true the method should perform a check that joints are within
bounds.
-
setClampToBounds
public void setClampToBounds(boolean enableClamping)
enables clamping of the solution such that solution always is within joint limits- Parameters:
enableClamping
- [in] true to enable clamping, false otherwise
-
getTCP
public FrameCPtr getTCP()
Description copied from class:InvKinSolver
Returns the Tool Center Point (TCP) used when solving the IK problem.
- Overrides:
getTCP
in classInvKinSolver
- Returns:
- The TCP Frame used when solving the IK.
-
-