basic/kinematics/DH_Link.cpp

Go to the documentation of this file.

//## begin module%3707C4FC037A.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%3707C4FC037A.cm

//## begin module%3707C4FC037A.cp preserve=no
//## end module%3707C4FC037A.cp

//## Module: DH_Link%3707C4FC037A; Pseudo Package body
//## Source file: E:\mpk\code\Kinematics\DH_Link.cpp

//## begin module%3707C4FC037A.additionalIncludes preserve=no
//## end module%3707C4FC037A.additionalIncludes

//## begin module%3707C4FC037A.includes preserve=yes
#include <assert.h>
#include <math/math2.h>
#include <string/string2.h>
#include <streams/mystreams.h>
#include <serializable/MPK_Dir.h>
//## end module%3707C4FC037A.includes

// VRML_Reader
#include "geometry/VRML_Reader.h"
// Frame
#include "geometry/Frame.h"
// ObjectBase
#include "geometry/ObjectBase.h"
// VRML_20_Reader
#include "geometry/VRML_20_Reader.h"
// DH_Link
#include "Kinematics/DH_Link.h"
// IfstreamWithComments
#include "additional/streams/IfstreamWithComments.h"
//## begin module%3707C4FC037A.additionalDeclarations preserve=yes
//## end module%3707C4FC037A.additionalDeclarations


// Class DH_Link 








DH_Link::DH_Link (FrameManager*  frameManager)
  //## begin DH_Link::DH_Link%924391851.hasinit preserve=no
  //## end DH_Link::DH_Link%924391851.hasinit
  //## begin DH_Link::DH_Link%924391851.initialization preserve=yes
  :LinkBase( frameManager ),
  a( 0 ),
  alpha( 0 ),
  d( 0 ),
  theta( 0 ),
  q( NULL ),
  type( DH_NONE )
  //## end DH_Link::DH_Link%924391851.initialization
{
  //## begin DH_Link::DH_Link%924391851.body preserve=yes
        frame = frameManager->AddFrame() ;
        controlledFrames.push_back( FrameNum() ) ;
  //## end DH_Link::DH_Link%924391851.body
}

DH_Link::DH_Link (const DH_Link& right)
  //## begin DH_Link::DH_Link%925178921.hasinit preserve=no
  //## end DH_Link::DH_Link%925178921.hasinit
  //## begin DH_Link::DH_Link%925178921.initialization preserve=yes
  :
        LinkBase( right ),
        a( right.a ),
        alpha( right.alpha ),
        d( right.d ),
        theta( right.theta ),
        type( right.type )
  //## end DH_Link::DH_Link%925178921.initialization
{
  //## begin DH_Link::DH_Link%925178921.body preserve=yes
        frame = right.frame ;           //IMPROVE: does this leave us with two pointers to the same frame?

        //make sure that our q is mapped to the same joint variable
        if( right.q == &right.a )
        {
                q = &a ;
        }
        else if( right.q == &right.alpha )
        {
                q = &alpha ;
        }
        else if( right.q == &right.d )
        {
                q = &d ;
        }
        else if( right.q == &right.theta )
        {
                q = &theta ;
        }
        else if( right.q == NULL )
        {
                q = NULL ;
        }
        else
        {
                assert( false ) ; //should never get to here 
        }
  //## end DH_Link::DH_Link%925178921.body
}


DH_Link::~DH_Link()
{
  //## begin DH_Link::~DH_Link%.body preserve=yes
        q = NULL ;
        
  //## end DH_Link::~DH_Link%.body
}



//## Other Operations (implementation)
void DH_Link::UpdateFrames () const
{
  //## begin DH_Link::UpdateFrames%924391847.body preserve=yes
        //get a reference to the frame
        //Frame* theFrame = frameManager->GetFrameRef( frame ) ;
        Frame* theFrame = new Frame;
        assert( theFrame != NULL ) ;

        //start updating the indicies of the frame
        //row 0
        (*theFrame)( 0, 0 ) = CosDeg( theta );
        (*theFrame)( 0, 1 ) = -SinDeg( theta );
        (*theFrame)( 0, 2 ) = 0;
        (*theFrame)( 0, 3 ) = a;

        //row 1
        (*theFrame)( 1, 0 ) = SinDeg( theta ) * CosDeg( alpha );
        (*theFrame)( 1, 1 ) = CosDeg( theta ) * CosDeg( alpha );
        (*theFrame)( 1, 2 ) = -SinDeg( alpha );
        (*theFrame)( 1, 3 ) = -SinDeg( alpha ) * ( d );

        //row 2
        (*theFrame)( 2, 0 ) = SinDeg( theta ) * SinDeg( alpha );
        (*theFrame)( 2, 1 ) = CosDeg( theta ) * SinDeg( alpha );
        (*theFrame)( 2, 2 ) = CosDeg( alpha );
        (*theFrame)( 2, 3 ) = CosDeg( alpha ) * ( d );

        //row 3
        (*theFrame)( 3, 0 ) = 0;
        (*theFrame)( 3, 1 ) = 0;
        (*theFrame)( 3, 2 ) = 0;
        (*theFrame)( 3, 3 ) = 1;

        frameManager->GetFrameRef( frame )->SetValues( this->positioningFrame * *theFrame );
        frameManager->MarkAllFramesChanged() ;
        delete theFrame;
  //## end DH_Link::UpdateFrames%924391847.body
}

unsigned int DH_Link::FrameNum () const
{
  //## begin DH_Link::FrameNum%924391852.body preserve=yes
        return frame ;
  //## end DH_Link::FrameNum%924391852.body
}

LinkBase* DH_Link::Clone () const
{
  //## begin DH_Link::Clone%924739818.body preserve=yes
        return new DH_Link( *this ) ;
  //## end DH_Link::Clone%924739818.body
}

void DH_Link::SetAlpha (const double value)
{
  //## begin DH_Link::SetAlpha%925324839.body preserve=yes
        alpha = value ;
  //## end DH_Link::SetAlpha%925324839.body
}

void DH_Link::SetA (const double value)
{
  //## begin DH_Link::SetA%925324840.body preserve=yes
        a = value ;
  //## end DH_Link::SetA%925324840.body
}

void DH_Link::SetD (const double value)
{
  //## begin DH_Link::SetD%925324841.body preserve=yes
        d = value ;
  //## end DH_Link::SetD%925324841.body
}

void DH_Link::SetTheta (const double value)
{
  //## begin DH_Link::SetTheta%925324842.body preserve=yes
        theta = value ;
  //## end DH_Link::SetTheta%925324842.body
}

void DH_Link::SetControllingParameter (DH_Parameter parameter)
{
  //## begin DH_Link::SetControllingParameter%925420850.body preserve=yes
        //IMPROVE: is this function properly named?
        type = parameter ;
        switch( parameter )
        {
        case DH_ALPHA:
                {
                        q = &alpha ;
                }
                break ;
        case DH_A :
                {
                        q = &a ;
                }
                break ;
        case DH_D :
                {
                        q = &d ;
                }
                break ;
        case DH_THETA :
                {
                        q = &theta ;
                }
                break ;
        default :
                {
                        assert( false ) ;       //should never get to here
                }
        }
  //## end DH_Link::SetControllingParameter%925420850.body
}

void DH_Link::SetJointVariable (const double newValue)
{
  //## begin DH_Link::SetJointVariable%925420852.body preserve=yes
        assert( q != NULL ) ;
        *q = newValue ;
        //IMPROVE: should we check here to see if this value is a valid one or not?
  //## end DH_Link::SetJointVariable%925420852.body
}

bool DH_Link::DoesLinkControlFrame (const unsigned int frameNum) const
{
  //## begin DH_Link::DoesLinkControlFrame%928263349.body preserve=yes
        if( frameNum == frame )
        {
                return true ;
        }
        return false ;
  //## end DH_Link::DoesLinkControlFrame%928263349.body
}

void DH_Link::Serialize (ostream& os) const
{
  //## begin DH_Link::Serialize%933801284.body preserve=yes
        os << *this ;
  //## end DH_Link::Serialize%933801284.body
}

void DH_Link::AddObject (const ObjectBase* object)
{
  //## begin DH_Link::AddObject%932766018.body preserve=yes
        //IMPROVE: this should call a member function in the base class not use object directly
        assert( object != NULL ) ;
        ObjectBase* newObject = dynamic_cast< ObjectBase* >( object->Clone() ) ;
        newObject->SetBaseFrame( FrameNum() ) ;
        objects.push_back( newObject ) ;
  //## end DH_Link::AddObject%932766018.body
}

bool DH_Link::DeSerialize (IfstreamWithComments& is)
{
  //## begin DH_Link::DeSerialize%933801287.body preserve=yes

        //do the deserialization of the base class
        LinkBase::DeSerialize( is ) ;

        is.eatwhite() ;

        is >> a ;
        is >> alpha ;
        is >> d ;
        is >> theta ;
        is >> frame ;
        
        int typeInt ;
        is >> typeInt ;
        type = DH_Parameter( typeInt ) ;                //IMPROVE: enough with the old style casts
        SetControllingParameter( type ) ;

        //read in shapes
        char marker[ 300 ] = "" ;
        is.eatwhite() ;
        is >> marker ;
        while( stricmp( marker, "#shape" ) == 0 )
        {
                //read in the filename
                char filename[ 300 ] ;
                is.getline( filename, 300 ) ;
                MPKstring::TrimLeadingWhitespace( filename ) ;
                MPKstring::TrimTrailingWhitespace( filename ) ;
                
                char fullname[_MAX_PATH];
                GetAbsoluteFileName(fullname, _MAX_PATH, is.path, filename);
                //VRML_Reader reader ;
                VRML_20_Reader reader ;
                //reader.SetRootPath( currentPath );
                reader.SetFilename( fullname ) ;

                bool readerSuccess = reader.Read() ;
                assert( readerSuccess ) ;
                //AddObject( reader.GetObjectA() ) ;
                assert( reader.GetTheObject() != NULL ) ;
                if ((readerSuccess == false) || (reader.GetTheObject()==NULL))
                        return false;

                Entity* theEntity = reader.GetTheObject()->Clone()  ;
                ObjectBase* theObject = dynamic_cast< ObjectBase* >( theEntity );
                assert( theObject != NULL );
                theObject->SetName( filename ) ;
                AddObject( theObject ) ;
                delete theObject ;

                //open the file and add it to the link
                is.eatwhite() ;
                is >> marker ;
        }
        assert( stricmp( marker, "#end_dh_link" ) == 0 ) ;
        return true;
  //## end DH_Link::DeSerialize%933801287.body
}

//exposes the type of this joint
DH_Parameter DH_Link::GetJointType() const
{
        return this->type;
}

double DH_Link::GetJointVariable () const
{
  //## begin DH_Link::GetJointVariable%940357561.body preserve=yes
        return *q ;
  //## end DH_Link::GetJointVariable%940357561.body
}

Frame DH_Link::GetFrame () const
{
  //## begin DH_Link::GetFrame%943395407.body preserve=yes

        //IMPROVE: this is a HACK method of building a frame out of DH parameters.
        //IMPROVE: this code should not be in two places
        //get a reference to the frame
        Frame returnMe ;
        Frame* theFrame = &returnMe ;

        //start updating the indicies of the frame
        //row 0
        (*theFrame)( 0, 0 ) = CosDeg( theta ) ;
        (*theFrame)( 0, 1 ) = -SinDeg( theta ) ;
        (*theFrame)( 0, 2 ) = 0 ;
        (*theFrame)( 0, 3 ) = a ;

        //row 1
        (*theFrame)( 1, 0 ) = SinDeg( theta ) * CosDeg( alpha ) ;
        (*theFrame)( 1, 1 ) = CosDeg( theta ) * CosDeg( alpha ) ;
        (*theFrame)( 1, 2 ) = -SinDeg( alpha ) ;
        (*theFrame)( 1, 3 ) = -SinDeg( alpha ) * ( d ) ;

        //row 2
        (*theFrame)( 2, 0 ) = SinDeg( theta ) * SinDeg( alpha ) ;
        (*theFrame)( 2, 1 ) = CosDeg( theta ) * SinDeg( alpha ) ;
        (*theFrame)( 2, 2 ) = CosDeg( alpha ) ;
        (*theFrame)( 2, 3 ) = CosDeg( alpha ) * ( d ) ;

        //row 3
        (*theFrame)( 3, 0 ) = 0 ;
        (*theFrame)( 3, 1 ) = 0 ;
        (*theFrame)( 3, 2 ) = 0 ;
        (*theFrame)( 3, 3 ) = 1 ;
        return returnMe ;
  //## end DH_Link::GetFrame%943395407.body
}

//This function is added by Zhenwang.Yao to replace the previous function
Frame& DH_Link::GetFrame (Frame &frame)
{
        Frame* theFrame = &frame ;

        //start updating the indicies of the frame
        //row 0
        (*theFrame)( 0, 0 ) = CosDeg( theta ) ;
        (*theFrame)( 0, 1 ) = -SinDeg( theta ) ;
        (*theFrame)( 0, 2 ) = 0 ;
        (*theFrame)( 0, 3 ) = a ;

        //row 1
        (*theFrame)( 1, 0 ) = SinDeg( theta ) * CosDeg( alpha ) ;
        (*theFrame)( 1, 1 ) = CosDeg( theta ) * CosDeg( alpha ) ;
        (*theFrame)( 1, 2 ) = -SinDeg( alpha ) ;
        (*theFrame)( 1, 3 ) = -SinDeg( alpha ) * ( d ) ;

        //row 2
        (*theFrame)( 2, 0 ) = SinDeg( theta ) * SinDeg( alpha ) ;
        (*theFrame)( 2, 1 ) = CosDeg( theta ) * SinDeg( alpha ) ;
        (*theFrame)( 2, 2 ) = CosDeg( alpha ) ;
        (*theFrame)( 2, 3 ) = CosDeg( alpha ) * ( d ) ;

        //row 3
        (*theFrame)( 3, 0 ) = 0 ;
        (*theFrame)( 3, 1 ) = 0 ;
        (*theFrame)( 3, 2 ) = 0 ;
        (*theFrame)( 3, 3 ) = 1 ;
        
  return frame ;
  //## end DH_Link::GetFrame%943395407.body
}

// Additional Declarations
  //## begin DH_Link%3707C4FC037A.declarations preserve=yes
//-----------------------------------------------------------------------------
std::ostream & operator<<(std::ostream &os, const DH_Link& v)
{
        using std::endl ;
        os << "#link" << endl ;
        os << "#dh_link" << endl ;
        v.LinkBase::Serialize( os ) ;
        os << v.a << '\t' ;
        os << v.alpha << '\t' ;
        os << v.d << '\t' ;
        os << v.theta << '\t' ;
        os << v.frame << '\t' ;
        os << v.type << '\t' ;
        os << endl ;
        os << "#end_dh_link" << endl ;
        return os ;
}
//-----------------------------------------------------------------------------
IfstreamWithComments & operator>>(IfstreamWithComments &is, DH_Link& v)
{
        char marker[ 300 ] = "" ;

        //verify the first few lines - header lines
        eatwhite( is ) ;
        is.getline( marker, 300 ) ;
        assert( strcmp( marker, "#link" ) == 0 ) ;

        eatwhite( is ) ;
        is.getline( marker, 300 ) ;
        assert( strcmp( marker, "#dh_link" ) == 0 ) ;

        v.DeSerialize( is ) ;           //IMPROVE: need to override this function for DH_Links

        eatwhite( is ) ;
        is.getline( marker, 300 ) ;
        assert( strcmp( marker, "#end_dh_link" ) == 0 ) ;

        return is ;
}
//-----------------------------------------------------------------------------
  //## end DH_Link%3707C4FC037A.declarations
//## begin module%3707C4FC037A.epilog preserve=yes
//## end module%3707C4FC037A.epilog

---