KrisLibrary  1.0.0
Classes | Functions
IKFunctions.h File Reference

Helper classes and functions for solving iterative inverse kinematics problems. More...

#include "RobotKinematics3D.h"
#include "IK.h"
#include <KrisLibrary/math/vectorfunction.h>
#include <KrisLibrary/optimization/Newton.h>
#include <KrisLibrary/utils/ArrayMapping.h>
#include <KrisLibrary/utils/DirtyData.h>

Go to the source code of this file.

Classes

struct  RobotIKFunction
 A vector field function class defining C(q)=0 for IK. More...
 
struct  RobotIKSolver
 A Newton-Raphson robot IK solver. More...
 
struct  WorldPositionFunction
 Function class that returns the world-space position of a point on the robot. More...
 
struct  IKGoalFunction
 Function class that returns the position/rotation errors associated with a given IKGoal. More...
 
struct  RobotCOMFunction
 Function class that measures the difference between the robot's center of mass and a desired COM. More...
 

Functions

void GetDefaultIKDofs (const RobotKinematics3D &robot, const std::vector< IKGoal > &ik, ArrayMapping &m)
 
void GetPassiveChainDOFs (const RobotKinematics3D &robot, const IKGoal &ikGoal, ArrayMapping &passiveDofs)
 
void GetPassiveChainDOFs (const RobotKinematics3D &robot, int link, int numTerms, ArrayMapping &passiveDofs)
 
bool SolveIK (RobotIKFunction &f, Real tolerance, int &iters, int verbose=1)
 
bool SolveIK (RobotKinematics3D &, const std::vector< IKGoal > &problem, Real tolerance, int &iters, int verbose=1)
 Same as above, constructing the ik function from an IK problem.
 
bool SolveIK (RobotKinematics3D &, const std::vector< IKGoal > &problem, const std::vector< int > &activeDofs, Real tolerance, int &iters, int verbose=1)
 
bool SolvePassiveChainIK (RobotKinematics3D &, const IKGoal &goal, Real tolerance, int &iters, int verbose=0)
 
void EvalIKError (const IKGoal &g, const RigidTransform &T, Real *poserr, Real *orierr)
 For the goal link's transformation T, computes the error of the IK goal g.
 
void EvalIKError (const IKGoal &g, const RigidTransform &T, Vector &err)
 
Real RobotIKError (const RobotKinematics3D &robot, const IKGoal &goal)
 
void EvalIKGoalDeriv (const IKGoal &g, const RigidTransform &T, const Vector3 &dw, const Vector3 &dv, Vector &derr)
 
bool IntersectGoals (const IKGoal &a, const IKGoal &b, IKGoal &c, Real tolerance=1e-5)
 
bool AddGoalNonredundant (const IKGoal &goal, std::vector< IKGoal > &goalSet, Real tolerance=1e-5)
 
bool IsReachableGoal (const RobotKinematics3D &, const IKGoal &a, const IKGoal &b)
 
bool IsReachableGoal (const IKGoal &a, const IKGoal &b, Real jointDistance)
 

Detailed Description

Helper classes and functions for solving iterative inverse kinematics problems.