Package org.robwork.sdurw
Class JacobianIKSolverPtr
- java.lang.Object
-
- org.robwork.sdurw.JacobianIKSolverPtr
-
public class JacobianIKSolverPtr extends java.lang.Object
Ptr stores a pointer and optionally takes ownership of the value.
-
-
Constructor Summary
Constructors Constructor Description JacobianIKSolverPtr()
Default constructor yielding a NULL-pointer.JacobianIKSolverPtr(long cPtr, boolean cMemoryOwn)
JacobianIKSolverPtr(JacobianIKSolver ptr)
Do not take ownership of ptr.
ptr can be null.
The constructor is implicit on purpose.
-
Method Summary
All Methods Static Methods Instance Methods Concrete Methods Modifier and Type Method Description JacobianIKSolver
__ref__()
Dereferencing operator.void
delete()
JacobianIKSolver
deref()
The pointer stored in the object.boolean
equals(JacobianIKSolver p)
static long
getCPtr(JacobianIKSolverPtr obj)
JacobianIKSolver
getDeref()
Member access operator.double
getMaxError()
Returns the maximal error for a solution
int
getMaxIterations()
Returns the maximal number of iterationsPropertyMap
getProperties()
Returns the PropertyMap
FrameCPtr
getTCP()
Returns the Tool Center Point (TCP) used when solving the IK problem.
boolean
isNull()
checks if the pointer is nullboolean
isShared()
check if this Ptr has shared ownership or none
ownershipIterativeIKPtr
makeDefault(DevicePtr device, State state)
Default iterative inverse kinematics solver for a device and
state.
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 limits
void
setEnableInterpolation(boolean enableInterpolation)
the solver may fail or be very slow if the the solution is too far from the
initial configuration.void
setInterpolatorStep(double interpolatorStep)
sets the maximal step length that is allowed on the
local search towards the solution.
void
setMaxError(double maxError)
Sets the maximal error for a solution
The error between two transformations is defined as the maximum of infinite-norm
of the positional error and the angular error encoded as EAA.
void
setMaxIterations(int maxIterations)
Sets the maximal number of iterations allowed
void
setSolverType(JacobianIKSolver.JacobianSolverType type)
set the type of solver to use for stepping toward a solution
VectorQ
solve(Transform3Dd baseTend, State state)
Calculates the inverse kinematics
Given a desired transform
and the current state, the method solves the inverse kinematics
problem.boolean
solveLocal(Transform3Dd bTed, double maxError, State state, int maxIter)
performs a local search toward the target bTed.
-
-
-
Constructor Detail
-
JacobianIKSolverPtr
public JacobianIKSolverPtr(long cPtr, boolean cMemoryOwn)
-
JacobianIKSolverPtr
public JacobianIKSolverPtr()
Default constructor yielding a NULL-pointer.
-
JacobianIKSolverPtr
public JacobianIKSolverPtr(JacobianIKSolver ptr)
Do not take ownership of ptr.
ptr can be null.
The constructor is implicit on purpose.
-
-
Method Detail
-
getCPtr
public static long getCPtr(JacobianIKSolverPtr obj)
-
delete
public void delete()
-
deref
public JacobianIKSolver deref()
The pointer stored in the object.
-
__ref__
public JacobianIKSolver __ref__()
Dereferencing operator.
-
getDeref
public JacobianIKSolver getDeref()
Member access operator.
-
equals
public boolean equals(JacobianIKSolver p)
-
isShared
public boolean isShared()
check if this Ptr has shared ownership or none
ownership- Returns:
- true if Ptr has shared ownership, false if it has no ownership.
-
isNull
public boolean isNull()
checks if the pointer is null- Returns:
- Returns true if the pointer is null
-
solve
public VectorQ solve(Transform3Dd baseTend, State state)
Calculates the inverse kinematics
Given a desired transform
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.
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.
-
setInterpolatorStep
public void setInterpolatorStep(double interpolatorStep)
sets the maximal step length that is allowed on the
local search towards the solution.
- Parameters:
interpolatorStep
- [in] the interpolation step.
-
setEnableInterpolation
public void setEnableInterpolation(boolean 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.
- Parameters:
enableInterpolation
- [in] set true to enable interpolation, false otherwise
-
solveLocal
public boolean solveLocal(Transform3Dd bTed, double maxError, State state, int maxIter)
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 posemaxError
- [in] the maximal allowed errorstate
- [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
-
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
-
setSolverType
public void setSolverType(JacobianIKSolver.JacobianSolverType type)
set the type of solver to use for stepping toward a solution
- Parameters:
type
- [in] the type of jacobian solver
-
setCheckJointLimits
public void setCheckJointLimits(boolean check)
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.
-
setMaxError
public void setMaxError(double maxError)
Sets the maximal error for a solution
The error between two transformations is defined as the maximum of infinite-norm
of the positional error and the angular error encoded as EAA.
- Parameters:
maxError
- [in] the maxError. It will be assumed that maxError > 0
-
getMaxError
public double getMaxError()
Returns the maximal error for a solution
- Returns:
- Maximal error
-
setMaxIterations
public void setMaxIterations(int maxIterations)
Sets the maximal number of iterations allowed
- Parameters:
maxIterations
- [in] maximal number of iterations
-
getMaxIterations
public int getMaxIterations()
Returns the maximal number of iterations
-
getProperties
public PropertyMap getProperties()
Returns the PropertyMap
- Returns:
- Reference to the PropertyMap
-
makeDefault
public IterativeIKPtr makeDefault(DevicePtr device, State state)
Default iterative inverse kinematics solver for a device and
state.
- Parameters:
device
- [in] Device for which to solve IK.state
- [in] Fixed state for which IK is solved.
-
getTCP
public FrameCPtr getTCP()
Returns the Tool Center Point (TCP) used when solving the IK problem.
- Returns:
- The TCP Frame used when solving the IK.
-
-