kinematics-dynamics
Public Types | Public Member Functions | Static Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
roboticslab::ScrewTheoryIkProblem Class Reference

Proxy IK problem solver class that iterates over a sequence of subproblems. More...

#include <ScrewTheoryIkProblem.hpp>

Public Types

using Steps = std::vector< const ScrewTheoryIkSubproblem * >
 Ordered sequence of IK subproblems needed to solve a IK problem.
 
using Solutions = std::vector< KDL::JntArray >
 Collection of global IK solutions.
 

Public Member Functions

 ~ScrewTheoryIkProblem ()
 Destructor.
 
 ScrewTheoryIkProblem (const ScrewTheoryIkProblem &)=delete
 
ScrewTheoryIkProblemoperator= (const ScrewTheoryIkProblem &)=delete
 
bool solve (const KDL::Frame &H_S_T, Solutions &solutions)
 Find all available solutions. More...
 
int solutions () const
 Number of global IK solutions.
 
const StepsgetSteps () const
 Solution of the IK problem (if available)
 
bool isReversed () const
 Whether the computed solution is reversed.
 

Static Public Member Functions

static ScrewTheoryIkProblemcreate (const PoeExpression &poe, const Steps &steps, bool reversed=false)
 Creates an IK solver instance given a sequence of known subproblems. More...
 

Private Types

enum  poe_term { EXP_KNOWN , EXP_COMPUTED , EXP_UNKNOWN }
 
using Frames = std::vector< KDL::Frame >
 
using PoeTerms = std::vector< poe_term >
 

Private Member Functions

 ScrewTheoryIkProblem (const PoeExpression &poe, const Steps &steps, bool reversed)
 
void recalculateFrames (const Solutions &solutions, Frames &frames, PoeTerms &poeTerms)
 
bool recalculateFrames (const Solutions &solutions, Frames &frames, PoeTerms &poeTerms, bool backwards)
 
KDL::Frame transformPoint (const KDL::JntArray &jointValues, const PoeTerms &poeTerms)
 

Private Attributes

const PoeExpression poe
 
const Steps steps
 
const bool reversed
 
const int soln
 

Detailed Description

This class is immutable. Instantiation is allowed by means of a static builder method.

See also
ScrewTheoryIkProblemBuilder

Member Function Documentation

◆ create()

ScrewTheoryIkProblem * ScrewTheoryIkProblem::create ( const PoeExpression poe,
const Steps steps,
bool  reversed = false 
)
static
Parameters
poeA product of exponentials (POE) formula.
stepsCollection of subproblems that solve this particular IK problem.
reversedTrue if the POE has been reversed (in order to find a valid solution).
Returns
An instance of an IK problem solver.

◆ solve()

bool ScrewTheoryIkProblem::solve ( const KDL::Frame &  H_S_T,
Solutions solutions 
)
Parameters
H_S_TTarget pose in cartesian space.
solutionsOutput vector of solutions stored as joint arrays.
Returns
True if all solutions are reachable, false otherwise.

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