Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

robot::control::kinematics::IKOR Class Reference

IKOR inverse kinematics solver. More...

Inheritance diagram for robot::control::kinematics::IKOR:

Inheritance graph
[legend]
Collaboration diagram for robot::control::kinematics::IKOR:

Collaboration graph
[legend]
List of all members.

Public Types

typedef std::bitset< sizeof(Int)*8 OptimizationConstraints )
enum  OptimizationMethod {
  DefaultMethod, PseudoInv, Lagrangian, BangBang,
  Simplex
}
enum  OptimizationCriterion { DefaultCriterion, LeastNorm, LeastFlow }
enum  OptimizationConstraint {
  DefaultConstraints, JointLimits, ObstacleAvoidance, Acceleration,
  EndEffectorImpact
}

Public Member Functions

 IKOR (const robot::KinematicChain &chain, const Vector &jointWeights=Vector(), bool nonHolonomicPlatformActive=false, Real platformL=0)
virtual String className () const
void setFullSpaceSolver (ref< FullSpaceSolver > fsp)
 call to use a different solver, before using solve()

virtual Vector solve (const Vector &dx, const Vector &x, const Vector &q, const base::Matrix &J, OptimizationMethod optMethod=Lagrangian, OptimizationCriterion optCriterion=LeastNorm, OptimizationConstraints optConstraints=OptimizationConstraints(JointLimits), base::Orient::Representation orientationRepresentation=base::Orient::EulerRPY)
 Solve inverse kinematics.

virtual bool isConstraintTypeSupported (OptimizationConstraint optConstraint, OptimizationMethod optMethod=DefaultMethod, OptimizationCriterion optCriterion=DefaultCriterion)
 query if a particular constraint type is supported by this solver for a specific method & criterion

virtual void setProximitySensorData (const array< LinkProximityData > &proximityData, Real d)
virtual void setParameter (const String &name, Real value)
 Set implementation specific parameters. Unknown names will throw a std::invald_argument exception.

virtual bool isSameKindAs (const ReferencedObject &) const
virtual bool isSameKindAs (const Object &) const
void reference () const
bool unreference () const
const int referenceCount () const
void enableOnUnreferenceCall (bool enabled)
virtual void onUnreference () const

Protected Member Functions

void weightedLeastNorm (Matrix &B, Vector &dZr, Vector weights)
void addDependentRowConstraints (const array< Int > &rows, const Matrix &gs, const Matrix &A, const Vector &b, ref< BetaFormConstraints > constraints)
void addJointLimitConstraints (const Matrix &gs, const Vector &q, const Vector &dq, ref< BetaFormConstraints > constraints)
void addNonholonomicConstraint (const Matrix &gs, const Vector &q, ref< BetaFormConstraints > constraints)
void addObstacleAvoidanceConstraints (const Matrix &gs, const Vector &q, const Vector &dq, ref< BetaFormConstraints > constraints)

Protected Attributes

ref< FullSpaceSolversolver
robot::KinematicChain chain
Vector weights
 relative weights for each joint for LeastNorm (vector of all 1s for equal weighting)

bool nonHolonomicPlatformActive
Real L
 platform parameter L - distance from origin P to center of drive axle M (see paper 2)

array< LinkProximityData > proximitySensorData
 !! may cache the objective later, if it speeds things up

Real d
 link obstacle proximity 'danger distance'

int _refCount
bool onUnreferenceEnabled

Detailed Description

IKOR inverse kinematics solver.

Todo:
test.

Definition at line 47 of file IKOR.


Member Typedef Documentation

typedef std::bitset<sizeof(Int)*8 robot::control::kinematics::InverseKinematicsSolver::OptimizationConstraints) [inherited]
 

Definition at line 58 of file InverseKinematicsSolver.


Member Enumeration Documentation

enum robot::control::kinematics::InverseKinematicsSolver::OptimizationConstraint [inherited]
 

Enumeration values:
DefaultConstraints 
JointLimits 
ObstacleAvoidance 
Acceleration 
EndEffectorImpact 

Definition at line 55 of file InverseKinematicsSolver.

enum robot::control::kinematics::InverseKinematicsSolver::OptimizationCriterion [inherited]
 

Enumeration values:
DefaultCriterion 
LeastNorm 
LeastFlow 

Definition at line 54 of file InverseKinematicsSolver.

enum robot::control::kinematics::InverseKinematicsSolver::OptimizationMethod [inherited]
 

Enumeration values:
DefaultMethod 
PseudoInv 
Lagrangian 
BangBang 
Simplex 

Definition at line 53 of file InverseKinematicsSolver.


Constructor & Destructor Documentation

robot::control::kinematics::IKOR::IKOR const robot::KinematicChain chain,
const Vector jointWeights = Vector(),
bool  nonHolonomicPlatformActive = false,
Real  platformL = 0
 


Member Function Documentation

void robot::control::kinematics::IKOR::addDependentRowConstraints const array< Int > &  rows,
const Matrix gs,
const Matrix A,
const Vector b,
ref< BetaFormConstraints constraints
[protected]
 

void robot::control::kinematics::IKOR::addJointLimitConstraints const Matrix gs,
const Vector q,
const Vector dq,
ref< BetaFormConstraints constraints
[protected]
 

void robot::control::kinematics::IKOR::addNonholonomicConstraint const Matrix gs,
const Vector q,
ref< BetaFormConstraints constraints
[protected]
 

void robot::control::kinematics::IKOR::addObstacleAvoidanceConstraints const Matrix gs,
const Vector q,
const Vector dq,
ref< BetaFormConstraints constraints
[protected]
 

virtual String robot::control::kinematics::IKOR::className  )  const [inline, virtual]
 

return the name of the object's class type. Must be defined by derived classes.

Implements base::Object.

Definition at line 53 of file IKOR.

void base::Referenced::enableOnUnreferenceCall bool  enabled  )  [inline, inherited]
 

If enabled, each call to unreference() will also call virtual method onUnreference()

Definition at line 115 of file Referenced.

References base::Referenced::onUnreferenceEnabled.

virtual bool robot::control::kinematics::IKOR::isConstraintTypeSupported OptimizationConstraint  optConstraint,
OptimizationMethod  optMethod = DefaultMethod,
OptimizationCriterion  optCriterion = DefaultCriterion
[virtual]
 

query if a particular constraint type is supported by this solver for a specific method & criterion

Reimplemented from robot::control::kinematics::InverseKinematicsSolver.

virtual bool base::Object::isSameKindAs const Object  )  const [inline, virtual, inherited]
 

Definition at line 47 of file Object.

virtual bool base::ReferencedObject::isSameKindAs const ReferencedObject  )  const [inline, virtual, inherited]
 

Definition at line 52 of file ReferencedObject.

virtual void base::Referenced::onUnreference  )  const [inline, virtual, inherited]
 

Called by unreference() if enabled via enableOnUnreferenceCall(true). Typically overridden in subclasses that wish to know about unreference() calls - for example to handle manually breaking cyclic references

Reimplemented in robot::control::ControllableAdaptor::AdaptorControlInterface, and robot::control::ControllableAdaptor.

Definition at line 121 of file Referenced.

Referenced by base::Referenced::unreference().

void base::Referenced::reference  )  const [inline, inherited]
 

Increment the reference count by one, indicating that this object has another pointer which is referencing it.

Definition at line 65 of file Referenced.

References base::Referenced::_refCount.

const int base::Referenced::referenceCount  )  const [inline, inherited]
 

Return the number pointers currently referencing this object.

Definition at line 112 of file Referenced.

References base::Referenced::_refCount.

Referenced by robot::control::ControllableAdaptor::AdaptorControlInterface::onUnreference().

void robot::control::kinematics::IKOR::setFullSpaceSolver ref< FullSpaceSolver fsp  )  [inline]
 

call to use a different solver, before using solve()

Definition at line 57 of file IKOR.

References solver.

virtual void robot::control::kinematics::IKOR::setParameter const String &  name,
Real  value
[virtual]
 

Set implementation specific parameters. Unknown names will throw a std::invald_argument exception.

Valid parameters: name value description none currently

Reimplemented from robot::control::kinematics::InverseKinematicsSolver.

virtual void robot::control::kinematics::IKOR::setProximitySensorData const array< LinkProximityData > &  proximityData,
Real  d
[virtual]
 

set proximity sensor data (array indices correspond to parameter elements of q) d - is 'danger distance' within which a constraint becomes active

Reimplemented from robot::control::kinematics::InverseKinematicsSolver.

virtual Vector robot::control::kinematics::IKOR::solve const Vector dx,
const Vector x,
const Vector q,
const base::Matrix J,
OptimizationMethod  optMethod = Lagrangian,
OptimizationCriterion  optCriterion = LeastNorm,
OptimizationConstraints  optConstraints = OptimizationConstraints(JointLimits),
base::Orient::Representation  orientationRepresentation = base::Orient::EulerRPY
[virtual]
 

Solve inverse kinematics.

Implements robot::control::kinematics::InverseKinematicsSolver.

bool base::Referenced::unreference  )  const [inline, inherited]
 

Decrement the reference count by one, indicating that a pointer to this object is referencing it. If the refence count goes to zero, it is assumed that this object is nolonger referenced and is automatically deleted.

Definition at line 81 of file Referenced.

References base::Referenced::_refCount, Exception, base::Referenced::onUnreference(), and base::Referenced::onUnreferenceEnabled.

void robot::control::kinematics::IKOR::weightedLeastNorm Matrix B,
Vector dZr,
Vector  weights
[protected]
 


Member Data Documentation

int base::Referenced::_refCount [mutable, protected, inherited]
 

Definition at line 136 of file Referenced.

Referenced by base::Referenced::reference(), base::Referenced::referenceCount(), base::Referenced::Referenced(), base::Referenced::unreference(), and base::Referenced::~Referenced().

robot::KinematicChain robot::control::kinematics::IKOR::chain [protected]
 

Definition at line 149 of file IKOR.

Real robot::control::kinematics::IKOR::d [protected]
 

link obstacle proximity 'danger distance'

Definition at line 158 of file IKOR.

Real robot::control::kinematics::IKOR::L [protected]
 

platform parameter L - distance from origin P to center of drive axle M (see paper 2)

Definition at line 152 of file IKOR.

bool robot::control::kinematics::IKOR::nonHolonomicPlatformActive [protected]
 

Definition at line 151 of file IKOR.

bool base::Referenced::onUnreferenceEnabled [protected, inherited]
 

Definition at line 137 of file Referenced.

Referenced by base::Referenced::enableOnUnreferenceCall(), base::Referenced::Referenced(), and base::Referenced::unreference().

array<LinkProximityData> robot::control::kinematics::IKOR::proximitySensorData [protected]
 

!! may cache the objective later, if it speeds things up

Definition at line 157 of file IKOR.

ref<FullSpaceSolver> robot::control::kinematics::IKOR::solver [protected]
 

Definition at line 148 of file IKOR.

Referenced by setFullSpaceSolver().

Vector robot::control::kinematics::IKOR::weights [protected]
 

relative weights for each joint for LeastNorm (vector of all 1s for equal weighting)

Definition at line 150 of file IKOR.


The documentation for this class was generated from the following file:
Generated on Thu Jul 29 16:42:25 2004 for OpenSim by doxygen 1.3.6