planners/ik_aca/IK_ACA.h

Go to the documentation of this file.

//=========================================================================
//
//  File:  IK_ACA.h
//
//  Created 31-Jul-01 by Shane Schneider
//
//              Inverse ACA planner for inverse kinematics.
//              Contains the inheritance from InvKinBase.

#ifndef IK_ACA_h
#define IK_ACA_h 1

#pragma warning( disable : 4250 )


// Parent Classes
#include "../inversekin/IK_InvKinBase.h"
#include "Planners/PL_GraphBase.h"
#include <math/Matrix4x4.h>
#include <Kinematics/Jacobian.h>

// Additional LEDA Classes
#include <LEDA/node_map.h>


//----------- Forward Declarations -----------------

//## Class: IK_ACA%3921BC600296
//## Category: Inverse Planners%36FB140B003C
//## Persistence: Transient
//## Cardinality/Multiplicity: n

class IK_ACA : 
        virtual public IK_InvKinBase,  //## Inherits: <unnamed>%3921BC73013F
        virtual public PL_GraphBase
{

  public:
          // Default Constructor
          IK_ACA();

    //## Destructor (generated)
      virtual ~IK_ACA();

      virtual bool DrawExplicit () const;

          virtual bool Load (const char *filename);

      virtual bool Plan ();

          virtual bool Save (const char *filename) const;

          virtual void SetStartConfig( const Configuration& configuration );

          virtual void SetGoalConfig( const Configuration& configuration );

          void SetEmbryosPerLandmark( const int& num_of_embryos );
          int  GetEmbryosPerLandmark() const;

      // Assigns the collision detector
      virtual void SetCollisionDetector (CollisionDetectorBase* collisionDetector);

        // Additional Public Declarations

          virtual void CopySettings( PlannerBase* original );
          
  protected:
        // Additional Implementation Declarations

          virtual void AssignGoalConfig( const Configuration& config );

          // SaveContents
          //    Saves the graph and other relevent ACA parameters to the given ostream pointer
          virtual bool SaveContents( std::ofstream& outfile ) const;

          // LoadContents
          //    Load the graph and other relevent ACA parameters to the given ostream pointer
          virtual bool LoadContents( std::ifstream& infile );

          //    DrawSpecific:  Draw the additional ACA embryos that do not get normally drawn with
          //                               PL_GraphBase::DrawExplicit().
          virtual bool DrawSpecific() const;

          // InitNewSearch
          //    Initiates parameters to indicate a new search has been requested.
          virtual void InitNewSearch();


          // Distance...
          //    Computes the squared distance between two frames based on the distance metric
          //    defined by Ahuactizin and Gupta (Aug 1999).
          virtual double FrameDistance ( const Matrix4x4& frameA, const Matrix4x4& frameB ) const;

          // ComputePath
          //    Computes the final path from the tree and search path to the actual or best goal config
          SuccessResultType ComputePath ( const node& landmark, const list<Configuration>& myPath );
          

        // ACA Alogrithmic Functions
          node Explore();       // Finds the furthest embryo in road map and coverts it into a landmark.
          SuccessResultType Search( const node& landmark ); // searchs C-space for goal frame from given config.

          SuccessResultType Search1( const node& landmark );
          VectorN ComputeEndEffDiff(Matrix4x4 &frStart, Matrix4x4 &frEnd);
          Configuration GetNextConfig( Configuration& q0 );


        // Cost Minimizing Functions
          double FindJointAdjust( const unsigned int& jointNum, const Matrix4x4& toolFrame ) const; 
                                                                        // finds the joint adjustment to minimize goal and tool frame distance
          Configuration MinimizeDistance( const Configuration& config , list<Configuration>& minPath );
                                                                        // returns a valid configuration that minimizes goal and tool frame distance
                                                                        // based on the given configuration



          // Embryo Functions
          //    Functions are used to create a new embryo
          void CreateEmbryo(const node& landmark, const int& embryoID); // Creates a valid embryo for a given landmark.
          void CreateEmbryos(const node& landmark ); // Creates required number of new embryos for given landmark


    // Additional Protected Declarations
          node_map<Configuration>* embryo;      // pointer to the embryo configurations associated with each landmark (node)


          list<Configuration> searchPath;   // records the successive configurations the search routine computes.

  private:
    // Additional Private Declarations
          int embryosPerLandmark;                       // indicates how many embryos are associated with each landmark.
          node searchLandmark;                          // indicates which landmark will be used by the search algorithm

          // Best config parameters
          node bestLandmark;                            // indicates which landmark results in config that is closest to goal frame
          list<Configuration> bestPath;         // records the successive configurations that result in the best path.
          double bestDistance_sq;                       // records the square of the best frame distance.
          int m_FrameID;                                        // the id of the goal frame in the frame manager
          CJacobian *jacobian;


  private: //## implementation
    // Additional Implementation Declarations

          void InitBestParams();                                // initializes the parameters used to keep track of the best path that
                                                                                // gets config closest to goal frame.


};


// Class IK_ACA 


#endif

---