![]() |
RobWorkProject
23.9.11-
|
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::Q > | solve (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::Frame > | getTCP () const |
Returns the Tool Center Point (TCP) used when solving the IK problem. More... | |
![]() | |
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::PropertyMap & | getProperties () |
Returns the PropertyMap. More... | |
virtual const rw::core::PropertyMap & | getProperties () const |
Returns the PropertyMap return Reference to the PropertyMap. | |
![]() | |
virtual | ~InvKinSolver () |
destructor | |
Additional Inherited Members | |
![]() | |
typedef rw::core::Ptr< IterativeIK > | Ptr |
smart pointer type to this class | |
typedef rw::core::Ptr< const IterativeIK > | CPtr |
smart pointer type to this const class | |
![]() | |
typedef rw::core::Ptr< InvKinSolver > | Ptr |
smart pointer type to this class | |
typedef rw::core::Ptr< const InvKinSolver > | CPtr |
smart pointer type to this const class | |
![]() | |
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... | |
![]() | |
IterativeIK () | |
Constructor. | |
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.
CCDSolver | ( | const rw::core::Ptr< const rw::models::SerialDevice > | device, |
const rw::kinematics::State & | state | ||
) |
Construct new CCSSolver.
device | [in] the device. |
state | [in] the state to use to extract dimensions. |
rw::core::Exception | if device is not castable to SerialDevice |
|
virtual |
Returns the Tool Center Point (TCP) used when solving the IK problem.
Implements InvKinSolver.
|
inlinevirtual |
Specifies whether to check joint limits before returning a solution.
check | [in] If true the method should perform a check that joints are within bounds. |
Implements InvKinSolver.
void setMaxLocalStep | ( | double | quatlength | ) |
Sets the maximal size of a local step.
quatlength | [in] Maximal length for quartenion |
|
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) )
baseTend | [in] Desired base to end transformation \( \robabx{}{desired}{\mathbf{T}}\) |
state | [in] State of the device from which to start the iterations |
Example:
CCDAlgorithm r;
r.inverseKinematics(device, Ttarget);
Implements InvKinSolver.
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.
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 |