RobWorkProject  23.9.11-
Public Member Functions | List of all members
CCDSolver Class Reference

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. More...

#include <CCDSolver.hpp>

Inherits IterativeIK.

Public Member Functions

 CCDSolver (const rw::models::SerialDevice *device, const rw::kinematics::State &state)
 Constructor.
 
 CCDSolver (const rw::core::Ptr< const rw::models::SerialDevice > device, const rw::kinematics::State &state)
 Construct new CCSSolver. More...
 
void setMaxLocalStep (double quatlength)
 Sets the maximal size of a local step. More...
 
std::vector< rw::math::Qsolve (const rw::math::Transform3D< double > &baseTend, const rw::kinematics::State &state) const
 Calculates the inverse kinematics. More...
 
bool solveLocal (const rw::math::Transform3D< double > &bTed, double maxError, rw::kinematics::State &state, int maxIter) const
 performs a local search toward the the target bTed. No via points are generated to support the convergence and robustness. More...
 
virtual void setCheckJointLimits (bool check)
 Specifies whether to check joint limits before returning a solution. 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

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

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.

Constructor & Destructor Documentation

◆ CCDSolver()

CCDSolver ( const rw::core::Ptr< const rw::models::SerialDevice device,
const rw::kinematics::State state 
)

Construct new CCSSolver.

Note
The dimensions will be automatically extracted from the device, using an arbitrary state.
Parameters
device[in] the device.
state[in] the state to use to extract dimensions.
Exceptions
rw::core::Exceptionif device is not castable to SerialDevice

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

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.

◆ setMaxLocalStep()

void setMaxLocalStep ( double  quatlength)

Sets the maximal size of a local step.

Parameters
quatlength[in] Maximal length for quartenion

◆ solve()

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

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 \( \robabx{}{desired}{\mathbf{T}}\)
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.

Example:
CCDAlgorithm r;
r.inverseKinematics(device, Ttarget);

Implements InvKinSolver.

◆ solveLocal()

bool solveLocal ( const rw::math::Transform3D< double > &  bTed,
double  maxError,
rw::kinematics::State state,
int  maxIter 
) const

performs a local search toward the the target bTed. No via points are generated to support the convergence and robustness.

Parameters
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
Returns
true if error is below max error
Note
the result will be saved in state

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