basic/robots/R_OpenChain.cpp

Go to the documentation of this file.

#pragma warning( disable : 4786 )
#include "debug/debug.h"
#include "geometry/Frame.h"
#include "Kinematics/DH_Link.h"
#include "Kinematics/LinkBase.h"
#include "robots/R_OpenChain.h"
#include <stdio.h>

//=============================================================================
// R_OpenChain::R_OpenChain
//=============================================================================
R_OpenChain::R_OpenChain (const R_OpenChain& right)
  : RobotBase( right )
{
        //improve: is there a more elegant way of doing this?
        for( int i = 0; i < right.links.size(); i++ )
        {
                links.push_back( right.links[ i ]->Clone() ) ;
        }

        for (i = 0; i<right.m_ToolFrames.size(); i++)
        {
                m_ToolFrames.push_back(right.m_ToolFrames[i]);
                m_ToolFrameBases.push_back(right.m_ToolFrameBases[i]);
        }
}

//=============================================================================
// R_OpenChain::R_OpenChain
//=============================================================================
R_OpenChain::R_OpenChain( FrameManager* frameManager )
  : RobotBase( frameManager )
{
    //nothing
}

//=============================================================================
// R_OpenChain::~R_OpenChain
//=============================================================================
R_OpenChain::~R_OpenChain()
{
        for( int i = 0; i < links.size(); i++ )
        {
                delete links[ i ] ;
                links[ i ] = NULL ;
        }
}

//=============================================================================
// AddToolFrame
//
// Purpose: adds a tool frame to a particular robot
//=============================================================================
void R_OpenChain::AddToolFrame (const Frame& frame, const int baseFrameNum)
{
        //
        // Get the frame manager
        //
        int toolFrameNum = frameManager->AddFrame();
        Frame* newFrame = frameManager->GetFrameRef( toolFrameNum );
        newFrame->SetValues(frame);
        m_ToolFrames.push_back( toolFrameNum );
        m_ToolFrameBases.push_back( baseFrameNum );
}

//=============================================================================
// R_OpenChain::GetToolFrame
//
// Purpose: allows access to a particular tool frame
//=============================================================================
int R_OpenChain::GetToolFrame( int index ) 
{
        if (m_ToolFrames.size()==0)
                return -1;
        else
                return m_ToolFrames[ index ];
}

//=============================================================================
// R_OpenChain::AppendLink 
//
// Purpose: adds a particular link to the open chain robot
//=============================================================================
void R_OpenChain::AppendLink (const LinkBase* link)
{
        IJG_Assert( link != NULL ) ; //IMPROVE: do something smarter if a null value is passed
        LinkBase* addme = link->Clone() ;
        links.push_back( addme ) ;
        
        //if this is the first link, make sure the base frame has been set to null
        if( links.size() == 1 )
        {
                IJG_Assert( addme->BaseFrameNum() == 0 ) ;      
        }

        //IMPROVE: need to mark this link as a dependant link
}

unsigned int R_OpenChain::DOF () const
{
  //## begin R_OpenChain::DOF%924391857.body preserve=yes
        return links.size() ;
  //## end R_OpenChain::DOF%924391857.body
}

Entity* R_OpenChain::Clone () const
{
  //## begin R_OpenChain::Clone%924739813.body preserve=yes
        R_OpenChain* returnme = new( R_OpenChain )( *this ) ;

        return returnme ;
  //## end R_OpenChain::Clone%924739813.body
}

std::list< LinkBase*> R_OpenChain::GetAllLinks () const
{
        std::list< LinkBase* > returnMe ;
        for( int i = 0; i < links.size(); i++ )
        {
                returnMe.insert( returnMe.end(), links[ i ] ) ;
        }
        return returnMe ;
}

//=============================================================================
// GetAllToolFrames
//
// Purpose: adds a tool frame to a particular robot
//=============================================================================
std::list< int > R_OpenChain::GetAllToolFrames() const
{
        std::list< int > returnMe;
        std::vector< int >::const_iterator i;
        for( i = m_ToolFrames.begin(); i != m_ToolFrames.end(); i++ )
        {
                const int addMe = *i;
                returnMe.insert( returnMe.end(), addMe );
        }
        return returnMe;
}

//=============================================================================
// R_OpenChain::GetLink
//
// Purpose: allows access to a particular link of the robot
//=============================================================================
LinkBase* R_OpenChain::GetLink( const unsigned int i )
{
        if (i >= links.size())
                return NULL;
    LinkBase* returnMe = links[ i ];
    return returnMe;
}

bool R_OpenChain::CanCheckInterference (const Entity* entity) const
{
  //## begin R_OpenChain::CanCheckInterference%926300303.body preserve=yes
        //check each entity in the robot with this entity
        return false ;
  //## end R_OpenChain::CanCheckInterference%926300303.body
}

bool R_OpenChain::IsInterfering (const Entity* entity) const
{
  //## begin R_OpenChain::IsInterfering%926300304.body preserve=yes
        //assert( false ) ; //IMPROVE: this function is not yet written, it should not be called
        return false ;
  //## end R_OpenChain::IsInterfering%926300304.body
}

// Additional Declarations
  //## begin R_OpenChain%3718ED82005A.declarations preserve=yes
bool R_OpenChain::Deserialize( IfstreamWithComments& is ) 
{
        RobotBase::Deserialize( is ) ;

        char line[ 1000 ] = "";
        is.eatwhite() ;
        is >> line ;
    if( stricmp( line, "#links" ) != 0 )
    {
        printf( "R_OpenChain::Deserialize - expected #links, got '%s'", line );
            IJG_Assert( stricmp( line, "#links" ) == 0 ) ;
    }
        int numberOfLinks = -1;
        is >> numberOfLinks ;

    int i;
        for( i = 0; i < numberOfLinks; i++ )
        {
                LinkBase* link = LinkBase::DeSerializeAbstract( is, this->frameManager ) ;
                if (link == NULL)
                {
                        //If fail to get one of the links
                        for( int l = 0; l < links.size(); l++ )
                        {
                                delete links[ l ] ;
                                links[ l ] = NULL ;
                        }
                        return false;
                }

                this->AppendLink( link ) ;
                int rdof = this->DOF() ;
                delete link ;
                link = NULL ;
        }

        //need to read in the DH parameters for the tool frame
        is.eatwhite() ;
        is >> line ;
        IJG_Assert( stricmp( line, "#toolframes" ) == 0 ) ;
        int numberOfToolFrames = -1 ;
        is >> numberOfToolFrames ;
        is.eatwhite() ;
        for( i = 0; i < numberOfToolFrames; i++ )
        {
                double a ;
                double alpha ;
                double d ;
                double theta ;
                int baseFrameNumber ;
                is >> a ;
                is >> alpha ;
                is >> d ;
                is >> theta ;
                is >> baseFrameNumber ;
                Frame offset ;

                DH_Link l( this->frameManager ) ;
                l.SetA( a ) ;
                l.SetAlpha( alpha ) ;
                l.SetD( d ) ;
                l.SetTheta( theta ) ;
                Frame toolFrame = l.GetFrame() ;
                this->AddToolFrame( toolFrame, baseFrameNumber ) ;
        }
        return true;
}
  //## end R_OpenChain%3718ED82005A.declarations

//## begin module%3718ED82005A.epilog preserve=yes
//## end module%3718ED82005A.epilog

---