planners/inversekin/IK_InvKinBase.cpp

Go to the documentation of this file.

//=========================================================================
//
//  File:  IK_InvKinBase.cpp
//
//  Created 07-Feb-01 by Shane Schneider
//
//              Planner Base for planners requiring a graph data structure
//              Contains the actual graph structure as well as several draw and
//              computation functions.



// IK_InvKinBase
#include "synchronization/semaphore.h"
#include "planners/InverseKin/IK_InvKinBase.h"
#include "robots/R_OpenChain.h"

#include <assert.h>
#include <math.h>
#include <math/math2.h>
#include <iosfwd>
#include <iostream>
#include <opengl/gl_Frame.h>

// fix gl path...
// #include <gl/glos.h>
#ifndef NOGL
    #include "opengl/glos.h"
    #include <gl/gl.h>
#endif

using std::endl;

//--------------------- Constants ---------------
static const int FRAMEDOF = 6;

// Class IK_InvKinBase 

// Default Constructor
IK_InvKinBase::IK_InvKinBase()
:
        m_GoalFrameIndex( -1 )
{
}


IK_InvKinBase::~IK_InvKinBase()
{
        // Default Destructor
}


//=============================================================================
// DrawExplicit
//
// Purpose: Draws the planner data to the planner window when called
//=============================================================================
bool IK_InvKinBase::DrawExplicit () const
{

        return false;
}

//=============================================================================
// DrawExplicit
//
// Purpose: Draws the planner data to the planner window when called
//=============================================================================
void IK_InvKinBase::DrawUniversePortion() const
{
#ifndef NOGL
        GL_Frame glFrame( goalFrame );
        ::glPushMatrix();
        glFrame.GLDraw();
        ::glPopMatrix();
#endif
}


//=============================================================================
// Load
//
// Purpose: Loads partailly complete planner data from the disk
//=============================================================================
bool IK_InvKinBase::Load (const char *filename)
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();
        
        // Add file loading code here...

        // IMPROVE:  This class and all derived classes will need to save the goal frame
        
        semaphore.Unlock();
        return true;
}


//=============================================================================
// Does Planning
//
// Purpose: performs the planning task
//=============================================================================
bool IK_InvKinBase::Plan ()
{
        assert( false );                //this is an abstract base class, and should never be called
        return false;
}

//=============================================================================
// Save
//
// Purpose: Saves partial plan data to disk
//=============================================================================
bool IK_InvKinBase::Save (const char *filename) const
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        // Add file saving data here...

        // IMPROVE:  This class and all derived classes will need to save the goal frame

        semaphore.Unlock();
        return true;
}

//=============================================================================
// SetStartConfig
//
// Purpose: Sets the start config.
//
//=============================================================================
/*void IK_InvKinBase::SetStartConfig( const Configuration& config )
{
        // Call the parent SetStartConfig to ensure all required initialization is established.
        PL_Boolean_Output::SetStartConfig( config );

}
*/
//=============================================================================
// SetGoalConfig
//
// Purpose: Overloads the SetGoalConfig of normal planners and prevents
//                      a goal configuration from being assigned since the IK planner
//                      uses a goal frame and not a goal configuration.  Once the 
//                      the IK planner has computed a configuration and path to the goal frame,
//                      the AssignGoalConfig is called which saves the computed configuration
//                      as the goal configuration.
//
//=============================================================================
/*void IK_InvKinBase::SetGoalConfig( const Configuration& config )
{

        // does nothing!

        //IMPROVE:  Remove call to parent's assign goal config
        PL_Boolean_Output::SetGoalConfig( config );

        
        return;

} */

//=============================================================================
// CopySettings
//
// Purpose: copies the settings from a different planner
//=============================================================================
void IK_InvKinBase::CopySettings( PlannerBase* original )
{
        IK_InvKinBase* originalIKBase = dynamic_cast< IK_InvKinBase* >( original );
        if( originalIKBase == NULL )
        {
                return;
        }

        this->goalFrame = originalIKBase->goalFrame;
}


//=============================================================================
// AssignGoalConfig
//
// Purpose: locally called function that assigns the computed goal configuration
//                      for the solved goal frame in the server and the planner.
//                      
//                      This function is used after the planner has solved the IK planning
//                      problem and computes a configuration that meets the goal frame.
//                      This function then saves the computed configuration so other
//                      planners can try to solve for the same configuration.
//=============================================================================
void IK_InvKinBase::AssignGoalConfig( const Configuration& config ) 
{
        // Assign the goal config to the planner
        // Call the parent SetGoalConfig to ensure all required initialization is established.
        PL_Boolean_Output::SetGoalConfig( config );

        // IMPROVE:  Add code to set the goal config to the server.

}


//=============================================================================
// SetGoalFrame
//
// Purpose: Assign the desired goal frame to solve for.
//
// NOTE:    This is the function that should be eventually called by all
//                      overloaded versions of the function.  It is used to assign and set
//                      any other parameters required when setting the goal frame.
//=============================================================================
void IK_InvKinBase::SetGoalFrame( const Matrix4x4& newFrame )
{
        if ( goalFrame != newFrame )
        {
                goalFrame = newFrame;

                // Initialize a new search.
                this->InitNewSearch();
        }
}

//=============================================================================
// SetGoalFrame
//
// Purpose: Assign the desired goal frame to solve for based on the tool frame
//                      generated by the given configuration.
//=============================================================================
void IK_InvKinBase::SetGoalFrame( const Configuration& config )
{
        // find the tool frame from the given configuration
        Matrix4x4 tempFrame = GetToolFrame( config );

        // assign the goal frame
        SetGoalFrame( tempFrame );
}

//=============================================================================
// SetGoalFrame
//
// Purpose: Assign the desired goal frame to solve for based on DOF vector given
//=============================================================================
void IK_InvKinBase::SetGoalFrame( const VectorN& dofVector )
{
        // verify the vector has the correct number of components.
        assert( dofVector.Length() == FRAMEDOF );

        // assign the goal frame
        SetGoalFrame( dofVector[0], dofVector[1], dofVector[2], 
                          dofVector[3], dofVector[4], dofVector[5] ); 

}

//=============================================================================
// SetGoalFrame
//
// Purpose: Assign the desired goal frame to solve for based on fixed angle
//                  X-Y-Z rotation and translated accordingly.
//      
// Note:    Roll, Pitch, and Yaw are to be expressed in degrees.
//=============================================================================
void IK_InvKinBase::SetGoalFrame( const double& x, const double& y, const double& z, 
                                                                  const double& roll, const double& pitch, const double& yaw )
{
        Matrix4x4 newFrame = GetTransformFrame( x, y, z, roll, pitch, yaw );

        // Assign the generated frame as the new goal frame
        SetGoalFrame( newFrame );
}

//=============================================================================
// GetTransformFrame
//
// Purpose: Calculate a frame transformation based on fixed angle
//                  X-Y-Z rotation and translated accordingly wrt to the world frame.
//                      The parameters are passed as a VectorN of size FRAMEDOF.
//                      The resulting frame can then be used as the desired goal frame.
//      
// Note:    Roll, Pitch, and Yaw are to be expressed in degrees.
//=============================================================================
Matrix4x4 IK_InvKinBase::GetTransformFrame( const VectorN& dofVector ) const
{
        // verify the vector has the correct number of components.
        assert( dofVector.Length() == FRAMEDOF );

        // get the frame transformation
        Matrix4x4 newFrame = GetTransformFrame( dofVector[0], dofVector[1], dofVector[2], 
                                                    dofVector[3], dofVector[4], dofVector[5] ); 

        return newFrame;
}

//=============================================================================
// GetTransformFrame
//
// Purpose: Calculate a frame transformation based on fixed angle
//                  X-Y-Z rotation and translated accordingly wrt to the world frame.
//                      The resulting frame can then be used as the desired goal frame.
//      
// Note:    Roll, Pitch, and Yaw are to be expressed in degrees.
//=============================================================================
Matrix4x4 IK_InvKinBase::GetTransformFrame( const double& x, const double& y, const double& z, 
                                                                                    const double& roll, const double& pitch, const double& yaw )
{
        // translate angles from degress to radians.
        double alpha = Deg2Rad( yaw );
        double beta  = Deg2Rad( pitch );
        double gamma = Deg2Rad( roll );

        // Cache trig values for quicker multiplcation.
        double cA = cos( alpha );
        double sA = sin( alpha );
        double cB = cos( beta );
        double sB = sin( beta );
        double cY = cos( gamma );
        double sY = sin( gamma );

        // Generate frame based on translation and fixed angle rotation
        Matrix4x4 newFrame;
        newFrame( 0 , 0 ) = cA*cB;
        newFrame( 1 , 0 ) = sA*cB;
        newFrame( 2 , 0 ) = -sB;
        newFrame( 3 , 0 ) = 0;

        newFrame( 0 , 1 ) = cA*sB*sY - sA*cY;
        newFrame( 1 , 1 ) = sA*sB*sY + cA*cY;
        newFrame( 2 , 1 ) = cB*sY;
        newFrame( 3 , 1 ) = 0;

        newFrame( 0 , 2 ) = cA*sB*cY + sA*sY;
        newFrame( 1 , 2 ) = sA*sB*cY - cA*sY;
        newFrame( 2 , 2 ) = cB*cY;
        newFrame( 3 , 2 ) = 0;

        newFrame( 0 , 3 ) = x;
        newFrame( 1 , 3 ) = y;
        newFrame( 2 , 3 ) = z;
        newFrame( 3 , 3 ) = 1;

        // return resulting new frame
        return newFrame;

}

//=============================================================================
// GetRotAngles
//
// Purpose: Calculates the fixed rotation angles and translation for the given
//                  frame wrt to the world frame.  The results are passed by reference.
//      
// Note:    Roll, Pitch, and Yaw are expressed in degrees.
//=============================================================================
void IK_InvKinBase::GetRotAngles( const Matrix4x4& frame, double& x, double& y, double& z,
                                                                  double& roll, double& pitch, double& yaw ) const
{
        // Declare the fixed angle variables.
        double alpha, beta, gamma;
        

        // Calculate beta and cache the cosine.
        beta = atan2( -frame(2,0) , sqrt( Sqr(frame(0,0)) + Sqr(frame(1,0)) ) );
        double cB = cos ( beta );

        // Calculate the other two rotations
        if ( cB == 0.0 )
        { 
                alpha = 0;
                if ( beta > 0 ) // if beta == PI/2
                {
                        gamma = atan2( frame(0,1) , frame(1,1) );
                }
                else
                {
                        gamma = -atan2( frame(0,1), frame(1,1) );
                }
        }
        else
        {
                alpha = atan2( frame(1,0) / cB , frame(0,0) / cB );
                gamma = atan2( frame(2,1) / cB , frame(2,2) / cB );
        }

        // Convert angles to degrees and assign to appropriate parameters
        // NOTE:  These angles must correspond to same angles used in SetGoalFrame
        yaw   = Rad2Deg( alpha );
        pitch = Rad2Deg( beta );
        roll  = Rad2Deg( gamma );

        // Get the translations
        x = frame(0,3);
        y = frame(1,3);
        z = frame(2,3);
}

//=============================================================================
// GetRotVector
//
// Purpose: Calculates the fixed rotation angles and translation for the given
//                  frame wrt to the world frame, and returns the result as a vector of
//                      size FRAMEDOF.
//      
// Note:    Roll, Pitch, and Yaw are expressed in degrees.
//=============================================================================
VectorN IK_InvKinBase::GetRotVector( const Matrix4x4& frame ) const
{
        // create Vector
        VectorN returnMe;
        returnMe.SetLength( FRAMEDOF );

        // assign the values
        GetRotAngles( frame, returnMe[0], returnMe[1], returnMe[2], returnMe[3], returnMe[4], returnMe[5] );

        // return the vector
        return returnMe;
}

//=============================================================================
// GetGoalFrame
//
// Purpose: Returns the goal frame that the IK planner will attempt to solve for
//
//=============================================================================
Matrix4x4 IK_InvKinBase::GetGoalFrame() const
{
        return goalFrame;
}

//=============================================================================
// GetGoalFrame
//
// Purpose: Returns the goal frame that the IK planner will attempt to solve for
//                      as a 6-dof of vector, with each component representing the translation
//                  or fixed angle rotation about the axis
//
//=============================================================================
VectorN IK_InvKinBase::GetGoalFrameVector() const
{
        VectorN returnMe;
        returnMe.SetLength( FRAMEDOF );
        
        GetRotAngles( goalFrame, returnMe[0], returnMe[1], returnMe[2], returnMe[3], returnMe[4], returnMe[5] );

        return returnMe;
}

//=============================================================================
// GetToolFrame
//
// Purpose: Computes the tool frame for the robot for a given configuration wrt world frame.
//
// Note:    This function will move the robot to the given configuration and
//                      force an update of all frames.  This function should not
//                      be called too often.
//=============================================================================
Matrix4x4 IK_InvKinBase::GetToolFrame( const Configuration& config ) const
{
#if 0
        if ( collisionDetector == NULL )
        {
                return Matrix4x4();
        }

        // get pointer to frame manager
        FrameManager* frameManager = collisionDetector->GetFrameManager();

        // move the robot to desired config
        collisionDetector->SetConfiguration( config );

        // IMPROVE:  temporary method of finding the tool frame in frame manager
        int toolFrameNum = 0;
        if ( config.DOF() > 0 )
        {
                toolFrameNum = collisionDetector->JointFrameNum( config.DOF() - 1 );
        }
                
        Matrix4x4 toolFrame = frameManager->GetWorldFrame( toolFrameNum );
#else
        Frame toolFrame = Matrix4x4();

        if ( collisionDetector != NULL )
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                collisionDetector->SetConfiguration( config );
                //toolFrame = frameManager->GetWorldFrame( m_toolFrame );
                R_OpenChain * robot = (R_OpenChain *)collisionDetector->GetRobot(0);
                int toolFrameID = robot->GetToolFrame(0);
                if (toolFrameID == -1)
                {
                        toolFrameID = collisionDetector->JointFrameNum( config.DOF() - 1 );
                        toolFrame = frameManager->GetWorldFrame( toolFrameID );
                }
                else
                {
                        toolFrame = *(frameManager->GetFrameRef(toolFrameID));
                        toolFrame = frameManager->GetTransformRelative(collisionDetector->DOF(), 0) * toolFrame;
                }
        }

#endif
        return toolFrame;
}


//=============================================================================
// InitNewSearch
//
// Purpose: Abstract function that initiates parameters in derived classes to 
//          indicate if a new search has been requested.
//=============================================================================
void IK_InvKinBase::InitNewSearch()
{
        // do nothing...this is an abstract class that should be defined in 
        // derived classes if needed.

        return;
}

---