sdurw_invkin module

class sdurw_invkin.sdurw_invkin.AmbiguityResolver(invkin, device)

Bases: InvKinSolver

Wraps a InvKinSolver and searches for ambiguities due to joint able to rotate \(2 \ pi\) or more.

For each solution \(\mathbf{q}\) the method tries to see if a \(j\) exists s.t. \(\mathbf{q}(i)=\mathbf{q}(i)+j * 2 \ pi\) is a valid solution.

The AmbiguityResolver always tests for joint limits.

__init__(invkin, device): Constructs an AmbiguityResolver :type invkin: rw::core::Ptr< rw::invkin::InvKinSolver > :param invkin: [in] The inverse kinematics solver to obtain solutions from :type device: rw::core::Ptr< rw::models::JointDevice > :param device: [in] the device for which to calculate inverse kinematics

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check): No effect. The AmbiguityResolver always tests for joint limits.

solve(baseTend, state): Calls the InvKinSolver provided and resolves ambiguities.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.CCDSolver(*args)

Bases: IterativeIK

This inverse kinematics method is a heuristic search technique called the Cyclic-Coordinate Descent method. The method attempts to minimize position and orientation errors by varying individual joints at a time.

Notice that the CCDSolver only work on devices with 1-dof joints.

__init__(*args)

Overload 1:

Constructor

Overload 2:

Construct new CCSSolver Notes: The dimensions will be automatically extracted from the device, using an arbitrary state. :type device: rw::core::Ptr< rw::models::SerialDevice const > :param device: [in] the device. :type state: State :param state: [in] the state to use to extract dimensions. :raises: rw::core::Exception if device is not castable to SerialDevice

Overload 3:

Construct new closed form solver for a Universal Robot. Notes: The dimensions will be automatically extracted from the device, using an arbitrary state. :type device: rw::core::Ptr< rw::models::Device const > :param device: [in] the device. :type state: State :param state: [in] the state to use to extract dimensions. :raises: rw::core::Exception if device is not castable to SerialDevice

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

setMaxLocalStep(quatlength): Sets the maximal size of a local step :type quatlength: float :param quatlength: [in] Maximal length for quartenion

solve(baseTend, state)

Example:

CCDAlgorithm r;

r.inverseKinematics(device, Ttarget);

solveLocal(bTed, maxError, state, maxIter)

performs a local search toward the the target bTed. No via points are generated to support the convergence and robustness. :type bTed: rw::math::Transform3D< double > :param bTed: [in] the target end pose :type maxError: float :param maxError: [in] the maximal allowed error :type state: State :param state: [in/out] the starting position for the search. The end position will

also be saved in this state.

Parameters: maxIter (int) – [in] max number of iterations
Return type: boolean
Returns: true if error is below max error

Notes: the result will be saved in state

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIK(*args, **kwargs)

Bases: InvKinSolver

Interface for closed form inverse kinematics algorithms.

The ClosedFormIK interface provides an interface for calculating the inverse kinematics of a device. That is to calculate the solutions \(\mathbf{q}_i, i=0,\ldots,\), such that \(\robabx{base}{end}{\mathbf{T}}(\mathbf{q}_i)=\robabx{}{desired}{\mathbf{T}}\).

By default it solves the problem beginning at the robot base and ending with the frame defined as the end of the devices, and which is accessible through the Device::getEnd() method.

__init__(*args, **kwargs)

static make(device, state)

Closed-form IK solver for a device.

The device must be a serial device with 6 revolute joints described by DH parameters.

The IK solver is currently implemented in terms of PieperSolver. See the documentation of PieperSolver for the specific requirements for the DH parameters.

An exception is thrown if closed-form IK for the device is not supported, except that all such cases are currently not discovered. You should check for yourself that the closed-form IK for the device is correct.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKCPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

cptr()

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

make(device, state)

Closed-form IK solver for a device.

The device must be a serial device with 6 revolute joints described by DH parameters.

The IK solver is currently implemented in terms of PieperSolver. See the documentation of PieperSolver for the specific requirements for the DH parameters.

An exception is thrown if closed-form IK for the device is not supported, except that all such cases are currently not discovered. You should check for yourself that the closed-form IK for the device is correct.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKSolverKukaIIWA(device, state)

Bases: ClosedFormIK

Analytical inverse solver for the Kuka LBR IIWA 7 R800 robot.

Notice that this is a 7 DOF robot and that there is an infinite number of solutions. The extra DOF means that the middle joint of the robot is able to move in a circle. This solver will choose a point on this circle randomly and return up to 8 possible solutions.

__init__(device, state): Construct new closed form solver for a Kuka 7 DOF IIWA robot. :type device: rw::core::Ptr< rw::models::SerialDevice const > :param device: [in] the device. :type state: State :param state: [in] the state to get the frame structure and extract the dimensions from.

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

solve(*args)

Overload 1:

Overload 2:

Find inverse kinematic solutions deterministically by pulling joint 4 as much in the given direction as possible. :type baseTend: rw::math::Transform3D< double > :param baseTend: [in] Desired base to end transformation

\(\robabx{}{desired}{\mathbf{T}}\).

Parameters

state (State) – [in] State of the device from which to start the iterations.
dir4 (rw::math::Vector3D< double >) – [in] unit vector giving the direction to pull joint 4 in (given in base coordinate system).

Return type

std::vector< rw::math::Q >

Returns

List of up to 8 solutions. Notice that the list may be empty.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKSolverKukaIIWACPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

solve(*args)

Overload 1:

Overload 2:

Find inverse kinematic solutions deterministically by pulling joint 4 as much in the given direction as possible. :type baseTend: rw::math::Transform3D< double > :param baseTend: [in] Desired base to end transformation

\(\robabx{}{desired}{\mathbf{T}}\).

Parameters

state (State) – [in] State of the device from which to start the iterations.
dir4 (rw::math::Vector3D< double >) – [in] unit vector giving the direction to pull joint 4 in (given in base coordinate system).

Return type

std::vector< rw::math::Q >

Returns

List of up to 8 solutions. Notice that the list may be empty.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKSolverKukaIIWAPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

cptr()

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

make(device, state)

Closed-form IK solver for a device.

The device must be a serial device with 6 revolute joints described by DH parameters.

The IK solver is currently implemented in terms of PieperSolver. See the documentation of PieperSolver for the specific requirements for the DH parameters.

An exception is thrown if closed-form IK for the device is not supported, except that all such cases are currently not discovered. You should check for yourself that the closed-form IK for the device is correct.

setCheckJointLimits(check)

solve(*args)

Overload 1:

Overload 2:

Find inverse kinematic solutions deterministically by pulling joint 4 as much in the given direction as possible. :type baseTend: rw::math::Transform3D< double > :param baseTend: [in] Desired base to end transformation

\(\robabx{}{desired}{\mathbf{T}}\).

Parameters

state (State) – [in] State of the device from which to start the iterations.
dir4 (rw::math::Vector3D< double >) – [in] unit vector giving the direction to pull joint 4 in (given in base coordinate system).

Return type

std::vector< rw::math::Q >

Returns

List of up to 8 solutions. Notice that the list may be empty.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKSolverUR(*args)

Bases: ClosedFormIK

Analytical inverse kinematics solver to the kinematics of a Universal Robots.

__init__(*args)

Overload 1:

Construct new closed form solver for a Universal Robot. Notes: The dimensions will be automatically extracted from the device, using an arbitrary state. :type device: rw::core::Ptr< rw::models::SerialDevice const > :param device: [in] the device. :type state: State :param state: [in] the state to use to extract dimensions.

Overload 2:

Construct new closed form solver for a Universal Robot. Notes: The dimensions will be automatically extracted from the device, using an arbitrary state. :type device: rw::core::Ptr< rw::models::Device const > :param device: [in] the device. :type state: State :param state: [in] the state to use to extract dimensions. :raises: rw::core::Exception if device is not castable to SerialDevice

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKSolverURCPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

solve(baseTend, state)

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ClosedFormIKSolverURPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

cptr()

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

make(device, state)

Closed-form IK solver for a device.

The device must be a serial device with 6 revolute joints described by DH parameters.

The IK solver is currently implemented in terms of PieperSolver. See the documentation of PieperSolver for the specific requirements for the DH parameters.

An exception is thrown if closed-form IK for the device is not supported, except that all such cases are currently not discovered. You should check for yourself that the closed-form IK for the device is correct.

setCheckJointLimits(check)

solve(baseTend, state)

property thisown: The membership flag

sdurw_invkin.sdurw_invkin.ClosedFormIK_make(device, state)

Closed-form IK solver for a device.

The device must be a serial device with 6 revolute joints described by DH parameters.

The IK solver is currently implemented in terms of PieperSolver. See the documentation of PieperSolver for the specific requirements for the DH parameters.

An exception is thrown if closed-form IK for the device is not supported, except that all such cases are currently not discovered. You should check for yourself that the closed-form IK for the device is correct.

class sdurw_invkin.sdurw_invkin.IKMetaSolver(*args)

Bases: IterativeIK

Solve the inverse kinematics problem with respect to joint limits and collisions.

Given an arbitrary iterative inverse kinematics solver, the IKMetaSolver attempts to find a collision free solution satisfying joint limits. It repeatingly calls the iterative solver with new random start configurations until either a solution is found or a specified max attempts has been reached.

Usage example:

// create a inverse kinematics solver for your dvs. here we use ResolvedRateSolver
ResolvedRateSolver iksolver(&myDevice); // takes a pointer to your device
// if we want colision free ik results then create or get the collisiondetector
CollisionDetector *detector = NULL; // here we don't care about collisions
// now create the meta solver
IKMetaSolver mSolver(&iksolver, &myDevice, detector);
// the pose that you want the endeffector to be in
Transform3D<> pose(Vector3D<>(0,0,1),RPY<>(1,0,0));
// and use it to generate joint configurations
std::vector<Q> result;
result = mSolver.solve( pose , state, 200, true );

__init__(*args)

Overload 1:

Constructs IKMetaSolver

The IKMetaSolver takes ownership of the iksolver. The IKMetaSolver does NOT take ownership of the collisionDetector. To skip testing for collision use null as collision detector

Parameters

iksolver (rw::core::Ptr< rw::invkin::IterativeIK >) – [in] The basic iksolver to use. Ownership is taken
device (rw::core::Ptr< rw::models::Device >) – [in] Device to solve for
collisionDetector (rw::core::Ptr< rw::proximity::CollisionDetector >, optional) – [in] CollisionDetector to use. If null no collision detection used.

Overload 2:

Constructs IKMetaSolver

The IKMetaSolver takes ownership of the iksolver. To skip testing for feasibility set constraint to NULL.

Parameters

iksolver (rw::core::Ptr< rw::invkin::IterativeIK >) – [in] The basic iksolver to use. Ownership is taken
device (rw::core::Ptr< rw::models::Device >) – [in] Device to solve for
constraint (rw::core::Ptr< rw::pathplanning::QConstraint >) – [in] QConstraint pointer to use. If null no constraints is applied

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

setMaxAttempts(maxAttempts): Sets up the maximal number of attempts :type maxAttempts: int :param maxAttempts: [in] Maximal number of attempts

setProximityLimit(limit)

Sets the distance for which two solutions are considered the same.

For distance measure an infinite norm is used. Default value is set to 1e-5.

Set limit < 0 to allow doublets in the solution.

Parameters: limit (float) – [in] The proximity limit.

setStopAtFirst(stopAtFirst): Sets whether to stop searching after the first solution :type stopAtFirst: boolean :param stopAtFirst: [in] True to stop after first solution has been found

solve(*args)

Overload 1:

Searches for a valid solution using the parameters set in the IKMetaSolver

Overload 2:

Solves the inverse kinematics problem

Tries to solve the inverse kinematics problem using the iterative inverse kinematics solver provided in the constructor. It tries at most cnt times and can either be stopped after the first solution is found or continue to search for solutions. If multiple solutions are returned there might be duplicates in the list.

Parameters

baseTend (rw::math::Transform3D< double >) – [in] Desired base to end transform
state (State) – [in] State of the workcell
cnt (int) – [in] Maximal number of attempts
stopatfirst (boolean) – [in] If true the method will return after the first solution is found. If false it will continue searching for more solution until the maximal number of attemps is met.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.IKMetaSolverCPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getMaxError()

Returns the maximal error for a solution

Return type: float
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

solve(*args)

Overload 1:

Searches for a valid solution using the parameters set in the IKMetaSolver

Overload 2:

Solves the inverse kinematics problem

Tries to solve the inverse kinematics problem using the iterative inverse kinematics solver provided in the constructor. It tries at most cnt times and can either be stopped after the first solution is found or continue to search for solutions. If multiple solutions are returned there might be duplicates in the list.

Parameters

baseTend (rw::math::Transform3D< double >) – [in] Desired base to end transform
state (State) – [in] State of the workcell
cnt (int) – [in] Maximal number of attempts
stopatfirst (boolean) – [in] If true the method will return after the first solution is found. If false it will continue searching for more solution until the maximal number of attemps is met.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.IKMetaSolverPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

cptr()

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getMaxError()

Returns the maximal error for a solution

Return type: float
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

makeDefault(device, state)

Default iterative inverse kinematics solver for a device and state.

Parameters

device (rw::core::Ptr< rw::models::Device >) – [in] Device for which to solve IK.
state (State) – [in] Fixed state for which IK is solved.

setCheckJointLimits(check)

setMaxAttempts(maxAttempts): Sets up the maximal number of attempts :type maxAttempts: int :param maxAttempts: [in] Maximal number of attempts

setMaxError(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 (float) – [in] the maxError. It will be assumed that maxError > 0

setMaxIterations(maxIterations)

Sets the maximal number of iterations allowed

Parameters: maxIterations (int) – [in] maximal number of iterations

setProximityLimit(limit)

Sets the distance for which two solutions are considered the same.

For distance measure an infinite norm is used. Default value is set to 1e-5.

Set limit < 0 to allow doublets in the solution.

Parameters: limit (float) – [in] The proximity limit.

setStopAtFirst(stopAtFirst): Sets whether to stop searching after the first solution :type stopAtFirst: boolean :param stopAtFirst: [in] True to stop after first solution has been found

solve(*args)

Overload 1:

Searches for a valid solution using the parameters set in the IKMetaSolver

Overload 2:

Solves the inverse kinematics problem

Tries to solve the inverse kinematics problem using the iterative inverse kinematics solver provided in the constructor. It tries at most cnt times and can either be stopped after the first solution is found or continue to search for solutions. If multiple solutions are returned there might be duplicates in the list.

Parameters

baseTend (rw::math::Transform3D< double >) – [in] Desired base to end transform
state (State) – [in] State of the workcell
cnt (int) – [in] Maximal number of attempts
stopatfirst (boolean) – [in] If true the method will return after the first solution is found. If false it will continue searching for more solution until the maximal number of attemps is met.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.InvKinSolver(*args, **kwargs)

Bases: object

Interface for inverse kinematics algorithms

The InvKinSolver interface provides an interface for calculating the inverse kinematics of a device. That is to calculate \(\mathbf{q}\) such that \(\robabx{base}{end}{\mathbf{T}}(\mathbf{q})=\robabx{}{desired}{\mathbf{T}}\).

By default it solves the problem beginning at the robot base and ending with the frame defined as the end of the devices, and which is accessible through the Device::getEnd() method.

__init__(*args, **kwargs)

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.InvKinSolverPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.IterativeIK(*args, **kwargs)

Bases: InvKinSolver

Interface for iterative inverse kinematics algorithms

The IterativeIK interface provides an interface for calculating the inverse kinematics of a device. That is to calculate \(\mathbf{q}\) such that \(\robabx{base}{end}{\mathbf{T}}(\mathbf{q})=\robabx{}{desired}{\mathbf{T}}\).

By default it solves the problem beginning at the robot base and ending with the frame defined as the end of the devices, and which is accessible through the Device::getEnd() method.

__init__(*args, **kwargs)

getMaxError()

Returns the maximal error for a solution

Return type: float
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

static makeDefault(device, state)

Default iterative inverse kinematics solver for a device and state.

Parameters

device (rw::core::Ptr< rw::models::Device >) – [in] Device for which to solve IK.
state (State) – [in] Fixed state for which IK is solved.

setMaxError(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 (float) – [in] the maxError. It will be assumed that maxError > 0

setMaxIterations(maxIterations)

Sets the maximal number of iterations allowed

Parameters: maxIterations (int) – [in] maximal number of iterations

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.IterativeIKCPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getMaxError()

Returns the maximal error for a solution

Return type: float
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.IterativeIKPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

cptr()

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getMaxError()

Returns the maximal error for a solution

Return type: float
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

makeDefault(device, state)

Default iterative inverse kinematics solver for a device and state.

Parameters

device (rw::core::Ptr< rw::models::Device >) – [in] Device for which to solve IK.
state (State) – [in] Fixed state for which IK is solved.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

setMaxError(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 (float) – [in] the maxError. It will be assumed that maxError > 0

setMaxIterations(maxIterations)

Sets the maximal number of iterations allowed

Parameters: maxIterations (int) – [in] maximal number of iterations

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

sdurw_invkin.sdurw_invkin.IterativeIK_makeDefault(device, state)

Default iterative inverse kinematics solver for a device and state.

Parameters

device (rw::core::Ptr< rw::models::Device >) – [in] Device for which to solve IK.
state (State) – [in] Fixed state for which IK is solved.

class sdurw_invkin.sdurw_invkin.IterativeMultiIK(*args, **kwargs)

Bases: object

Interface for iterative inverse kinematics algorithms for problems or devices that utilize more than one end-effector.

The IterativeMultiIK interface provides an interface for calculating the inverse kinematics of a device with multiple end-effectors. That is to calculate \(\mathbf{q}\) such that \(\robabx{base}{end}{\mathbf{T}}(\mathbf{q})=\robabx{}{desired}{\mathbf{T}}\).

By default it solves the problem beginning at the robot base and ending with the frame defined as the end of the devices, and which is accessible through the Device::getEnd() method.

__init__(*args, **kwargs)

getMaxError()

Returns the maximal error for a solution

Return type: std::vector< double >
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

setMaxError(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 (std::vector< double >) – [in] the maxError. It will be assumed that maxError > 0

setMaxIterations(maxIterations)

Sets the maximal number of iterations allowed

Parameters: maxIterations (int) – [in] maximal number of iterations

solve(baseTend, state)

Calculates the inverse kinematics

Given a desired \(\robabx{}{desired}{\mathbf{T}}\) and the current state, the method solves the inverse kinematics problem. If no solution is found with the required precision and within the specified number of iterations it will return an empty list.

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, it is checked to be within the bounds of the configuration space.

Parameters

baseTend (std::vector< rw::math::Transform3D< double > >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.JacobianIKSolver(*args)

Bases: IterativeIK

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]\)

DLS = 2

SVD = 1

Transpose = 0

Weighted = 3

__init__(*args)

Overload 1:

Constructs JacobianIKSolver for device device. :type device: rw::core::Ptr< rw::models::Device const > :param device: [in] the device to do inverse kinematics for. :type state: State :param state: [in] the initial state.

Overload 2:

Constructs JacobianIKSolver for device, where the frame foi will be used as end effector. :type device: rw::core::Ptr< rw::models::Device const > :param device: [in] the device to do inverse kinematics for. :type foi: rw::core::Ptr< rw::kinematics::Frame const > :param foi: [in] end effector frame. :type state: State :param state: [in] the initial state.

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

setClampToBounds(enableClamping): enables clamping of the solution such that solution always is within joint limits :type enableClamping: boolean :param enableClamping: [in] true to enable clamping, false otherwise

setEnableInterpolation(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. :type enableInterpolation: boolean :param enableInterpolation: [in] set true to enable interpolation, false otherwise

setInterpolatorStep(interpolatorStep): sets the maximal step length that is allowed on the local search towards the solution. :type interpolatorStep: float :param interpolatorStep: [in] the interpolation step.

setJointLimitTolerance(tolerance): setJointLimitTolerance set the tolerance used for bound-checking the solution :type tolerance: float :param tolerance: for joint bounds checking

setSolverType(type): set the type of solver to use for stepping toward a solution :type type: int :param type: [in] the type of jacobian solver

setWeightVector(*args)

Overload 1:

setWeightVector sets the weight vector used for solver type “Weighted” :type weights: Eigen::VectorXd :param weights: a vector of weights for each degree of freedom, ie weights.size() == DOF

Overload 2:

setWeightVector sets the weight vector used for solver type “Weighted” :type weights: std::vector< double > :param weights: a vector of weights for each degree of freedom, ie weights.size() == DOF

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

solveLocal(bTed, maxError, state, maxIter)

performs a local search toward the target bTed. No via points are generated to support the convergence and robustness. :type bTed: rw::math::Transform3D< double > :param bTed: [in] the target end pose :type maxError: float :param maxError: [in] the maximal allowed error :type state: State :param state: [in/out] the starting position for the search. The end position will

also be saved in this state.

Parameters: maxIter (int) – [in] max number of iterations
Return type: boolean
Returns: true if error is below max error

Notes: the result will be saved in state

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.JacobianIKSolverCPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getMaxError()

Returns the maximal error for a solution

Return type: float
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

solve(baseTend, state)

solveLocal(bTed, maxError, state, maxIter)

performs a local search toward the target bTed. No via points are generated to support the convergence and robustness. :type bTed: rw::math::Transform3D< double > :param bTed: [in] the target end pose :type maxError: float :param maxError: [in] the maximal allowed error :type state: State :param state: [in/out] the starting position for the search. The end position will

also be saved in this state.

Parameters: maxIter (int) – [in] max number of iterations
Return type: boolean
Returns: true if error is below max error

Notes: the result will be saved in state

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.JacobianIKSolverM(*args)

Bases: IterativeMultiIK

A Jacobian based iterative inverse kinematics algorithm for devices with multiple end effectors.

This algorithm does not 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]\)

DLS = 2

SDLS = 3

SVD = 1

Transpose = 0

__init__(*args)

Overload 1:

Constructs JacobianIKSolverM for TreeDevice. Uses the default end effectors of the TreeDevice

Overload 2:

Constructs JacobianIKSolverM for a JointDevice(SerialDevice and TreeDevice). It does not use the default end effectors. A list of interest frames are given instead.

setClampToBounds(enableClamping): enables clamping of the solution such that solution always is within joint limits. :type enableClamping: boolean :param enableClamping: [in] true to enable clamping, false otherwise

setEnableInterpolation(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. :type enableInterpolation: boolean :param enableInterpolation: [in] set true to enable interpolation, false otherwise

setReturnBestFit(returnBestFit): configures the iterative solver to return the best fit found, even though error criteria was not met. :type returnBestFit: boolean :param returnBestFit: [in] set to true if you want best fit returned.

setSolverType(type): set the type of solver to use for stepping toward a solution :type type: int :param type: [in] the type of Jacobian solver

solve(baseTend, state)

Calculates the inverse kinematics

Given a desired \(\robabx{}{desired}{\mathbf{T}}\) and the current state, the method solves the inverse kinematics problem. If no solution is found with the required precision and within the specified number of iterations it will return an empty list.

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, it is checked to be within the bounds of the configuration space.

Parameters

baseTend (std::vector< rw::math::Transform3D< double > >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

solveLocal(bTed, maxError, state, maxIter, untilSmallChange=False)

performs a local search toward the the target bTed. No via points are generated to support the convergence and robustness. :type bTed: std::vector< rw::math::Transform3D< double > > :param bTed: [in] the target end pose :type maxError: std::vector< double > :param maxError: [in] the maximal allowed error :type state: State :param state: [in/out] the starting position for the search. The end position will

also be saved in this state.

Parameters

maxIter (int) – [in] max number of iterations
untilSmallChange (boolean, optional) – [in] if true the error difference between two successive iterations need to be smaller than maxError. If false then the error of a iteration need to be smaller than maxError.

Return type

boolean

Returns

true if error is below max error

Notes: the result will be saved in state

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.JacobianIKSolverPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

cptr()

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getMaxError()

Returns the maximal error for a solution

Return type: float
Returns: Maximal error

getMaxIterations(): Returns the maximal number of iterations

getProperties(*args)

Overload 1:

Returns the PropertyMap :rtype: PropertyMap :return: Reference to the PropertyMap

Overload 2:

Returns the PropertyMap return Reference to the PropertyMap

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

makeDefault(device, state)

Default iterative inverse kinematics solver for a device and state.

Parameters

device (rw::core::Ptr< rw::models::Device >) – [in] Device for which to solve IK.
state (State) – [in] Fixed state for which IK is solved.

setCheckJointLimits(check)

setClampToBounds(enableClamping): enables clamping of the solution such that solution always is within joint limits :type enableClamping: boolean :param enableClamping: [in] true to enable clamping, false otherwise

setEnableInterpolation(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. :type enableInterpolation: boolean :param enableInterpolation: [in] set true to enable interpolation, false otherwise

setInterpolatorStep(interpolatorStep): sets the maximal step length that is allowed on the local search towards the solution. :type interpolatorStep: float :param interpolatorStep: [in] the interpolation step.

setJointLimitTolerance(tolerance): setJointLimitTolerance set the tolerance used for bound-checking the solution :type tolerance: float :param tolerance: for joint bounds checking

setMaxError(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 (float) – [in] the maxError. It will be assumed that maxError > 0

setMaxIterations(maxIterations)

Sets the maximal number of iterations allowed

Parameters: maxIterations (int) – [in] maximal number of iterations

setSolverType(type): set the type of solver to use for stepping toward a solution :type type: int :param type: [in] the type of jacobian solver

setWeightVector(*args)

Overload 1:

setWeightVector sets the weight vector used for solver type “Weighted” :type weights: Eigen::VectorXd :param weights: a vector of weights for each degree of freedom, ie weights.size() == DOF

Overload 2:

setWeightVector sets the weight vector used for solver type “Weighted” :type weights: std::vector< double > :param weights: a vector of weights for each degree of freedom, ie weights.size() == DOF

solve(baseTend, state)

solveLocal(bTed, maxError, state, maxIter)

performs a local search toward the target bTed. No via points are generated to support the convergence and robustness. :type bTed: rw::math::Transform3D< double > :param bTed: [in] the target end pose :type maxError: float :param maxError: [in] the maximal allowed error :type state: State :param state: [in/out] the starting position for the search. The end position will

also be saved in this state.

Parameters: maxIter (int) – [in] max number of iterations
Return type: boolean
Returns: true if error is below max error

Notes: the result will be saved in state

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.ParallelIKSolver(device)

Bases: 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.

__init__(device): Construct new solver. :type device: ParallelDevice :param device: [in] pointer to the parallel device.

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

setClampToBounds(enableClamping): enables clamping of the solution such that solution always is within joint limits :type enableClamping: boolean :param enableClamping: [in] true to enable clamping, false otherwise

solve(*args)

Overload 1:

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List with one or zero solutions.

Notes: 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.

Overload 2:

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 (std::vector< rw::invkin::ParallelIKSolver::Target >) – [in] list of targets.
state (State) – [in] state containing the current configuration of the device.

Return type

std::vector< rw::math::Q >

Returns

vector with one solution if found, otherwise vector is empty.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.PieperSolver(*args)

Bases: ClosedFormIK

Calculates the closed form inverse kinematics of a device using Piepers method

To use Piepers method it is required that the device has 6 DOF revolute joints, and that last three axis intersects. In this implementation it will be assumed that the that rotation of these last three axis are equivalent to an Euler ZYZ or Z(-Y)Z rotation.

See Introduction to Robotics Mechanics and Control, by John J. Craig for further information about the algorithm.

__init__(*args)

Overload 1:

Constructor :type dhparams: std::vector< rw::models::DHParameterSet > :param dhparams: [in] DH-parameters corresponding to the device :type joint6Tend: rw::math::Transform3D< double > :param joint6Tend: [in] transform from the 6th joint to the end of the device :type baseTdhRef: rw::math::Transform3D< double >, optional :param baseTdhRef: [in] Transformation between the robot base and the reference frame for

the DH-parameters.

Overload 2:

Constructor - the DH parameters is expected to be on each joint in the serial device. When specifying the DH params in the workcell file this constructor can be used. :type dev: SerialDevice :param dev: [in] the device for which to extract the DH parameters. :type joint6Tend: rw::math::Transform3D< double > :param joint6Tend: [in] transform from the 6th joint to the end of the device :type state: State :param state: [in] State using which the transformation between robot base and the

DH-parameters reference frame are calculated.

Notes: throws an exception if the device has no DH params

getTCP()

Returns the Tool Center Point (TCP) used when solving the IK problem.

Return type: rw::core::Ptr< rw::kinematics::Frame const >
Returns: The TCP Frame used when solving the IK.

setCheckJointLimits(check)

Specifies whether to check joint limits before returning a solution.

Parameters: check (boolean) – [in] If true the method should perform a check that joints are within bounds.

solve(baseTend, state)

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 (rw::math::Transform3D< double >) – [in] Desired base to end transformation \(\robabx{}{desired}{\mathbf{T}}\)
state (State) – [in] State of the device from which to start the iterations

Return type

std::vector< rw::math::Q >

Returns

List of solutions. Notice that the list may be empty.

Notes: The targets baseTend must be defined relative to the base of the robot/device.

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.PieperSolverCPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

solve(baseTend, state)

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.PieperSolverPtr(*args)

Bases: object

Ptr stores a pointer and optionally takes ownership of the value.

__init__(*args)

Overload 1:

Default constructor yielding a NULL-pointer.

Overload 2:

Do not take ownership of ptr.

ptr can be null.

The constructor is implicit on purpose.

cptr()

deref(): The pointer stored in the object.

getDeref(): Member access operator.

getTCP()

isNull(): checks if the pointer is null :rtype: boolean :return: Returns true if the pointer is null

isShared(): check if this Ptr has shared ownership or none ownership :rtype: boolean :return: true if Ptr has shared ownership, false if it has no ownership.

make(device, state)

Closed-form IK solver for a device.

The device must be a serial device with 6 revolute joints described by DH parameters.

The IK solver is currently implemented in terms of PieperSolver. See the documentation of PieperSolver for the specific requirements for the DH parameters.

An exception is thrown if closed-form IK for the device is not supported, except that all such cases are currently not discovered. You should check for yourself that the closed-form IK for the device is correct.

setCheckJointLimits(check)

solve(baseTend, state)

property thisown: The membership flag

class sdurw_invkin.sdurw_invkin.Target(*args)

Bases: object

A target definition used in the multi-target solve function.

__init__(*args)

Overload 1:: Constructor with all directions enabled initially.

Overload 2:

Constructor with specification of a target transformation from the base to a given tcp frame. :type frame: rw::core::Ptr< rw::kinematics::Frame const > :param frame: [in] the end frame. :type refTtcp: rw::math::Transform3D< double > :param refTtcp: [in] the target base to frame transformation.

Overload 3:

Constructor with specification of a target transformation where some directions are not enabled. :type frame: rw::core::Ptr< rw::kinematics::Frame const > :param frame: [in] the end frame. :type refTtcp: rw::math::Transform3D< double > :param refTtcp: [in] the target base to frame transformation. :type enabled: rw::math::VectorND< 6,bool > :param enabled: [in] 6 values specifying if the x, y, z and EAA x, y, z directions

should be enabled.

dof(): Get the number directions enabled. :rtype: int :return: number of directions enabled.

property enabled: Directions of baseTtcp to enable. The 6 values specify x, y, z and EAA x, y, z directions.

property refFrame: The reference frame. If zero, this is equivalent to the device base frame.

property refTtcp: The target transformation from refFrame to the tcpFrame.

property tcpFrame: The frame to specify target for.

property thisown: The membership flag

sdurw_invkin.sdurw_invkin.ownedPtr(*args)

Overload 1: