Biomechanical Joint Model  Author: Anderson Maciel

---

comemovement.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2001 by Anderson Maciel                                 *
 *   andi.maciel@gmail.com                                                 *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 **************************************************************************/

//
//  PROJECT.....: 
//  RESPONSIBLE.: 
//
//  FILE........: vpmovment.cpp
//  DESCRIPTION.: Contain the COME_Movement class definitions.
//
//  AUTHOR......: Anderson Maciel
//  DATE........: June/06/2001
//  DESCRIPTION.: Methods definition.
//
#include        <algebra/comevector3d.h>
#include        <kinematics/comemovement.h>
#include        <general/comexml.h>
#include        <stdio.h>
#include <xercesc/dom/DOM.hpp>
#include <xercesc/util/PlatformUtils.hpp>


COME_Movement::COME_Movement(){

  bodyName = "";
  cycle = false;
  deltaT = 0;
  time = INIT_TIME;
  time_err = 0;
}

COME_Movement::COME_Movement( string bn, bool cy, double dt, double te ){
  bodyName = bn;
  cycle = cy;
  deltaT = dt;
  time = INIT_TIME;
  time_err = te;
}


COME_Movement::~COME_Movement(){
}

double
COME_Movement::getDeltaT(){
  return deltaT;
}

bool
COME_Movement::getCycle(){
  return cycle;
}

double
COME_Movement::getTime(){
  return time;
}

double
COME_Movement::getTimeFinal(){
  return tFinal;
}

double
COME_Movement::getTime_Err(){
  return time_err;
}

int
COME_Movement::getQtdMotion(){
  return qtdMotion;
}

COME_JointMotion *
COME_Movement::getTimeline(){
  return motionTimeline;
}

void
COME_Movement::setTimeLine( COME_JointMotion * newTL ){
  motionTimeline = newTL;
}

void
COME_Movement::setDeltaT( double dt ){
  deltaT = dt;
}

void
COME_Movement::setCycle( bool c ){
  cycle = c;
}

void
COME_Movement::setTime( double t ){
  time = t;
}

void
COME_Movement::setTimeFinal( double tf ){
  tFinal = tf;
}

void
COME_Movement::setTime_Err( double te ){
  time_err = te;
}

void
COME_Movement::setQtdMotion( int qm ){
  qtdMotion = qm;
}

void
COME_Movement::vpLoadMotionFile( const char *fileName ){


    // Initialize the XML4C system
    try
    {
        XMLPlatformUtils::Initialize();
    }

    catch (const XMLException& toCatch)
    {
         printf( "Error during initialization! : %s\n", toCatch.getMessage() );
         return;
    }

    XercesDOMParser::ValSchemes    valScheme = XercesDOMParser::Val_Auto; //Val_Never Val_Always
    bool                     doNamespaces    = false;

    // Instantiate the DOM parser.
    XercesDOMParser parser;
    parser.setValidationScheme(valScheme);
    parser.setDoNamespaces(doNamespaces);

    // And create our error handler and install it
    DOMCountErrorHandler errorHandler;
    parser.setErrorHandler(&errorHandler);

    //
    //  Get the starting time and kick off the parse of the indicated
    //  file. Catch any exceptions that might propogate out of it.
    //
    unsigned long duration;
    try
    {
        const unsigned long startMillis = XMLPlatformUtils::getCurrentMillis();
        parser.parse( fileName );
        const unsigned long endMillis = XMLPlatformUtils::getCurrentMillis();
        duration = endMillis - startMillis;
    }

    catch (const XMLException& toCatch)
    {
        printf( "\nError during parsing: '%s'\n Exception message is: %s \n", fileName, StrX(toCatch.getMessage()).localForm() );
        return;
    }
    catch (const DOMException& toCatch)
    {
                printf( "\nError during parsing: '%s'\n Exception message is: %s \n", fileName, toCatch.msg );
        XMLPlatformUtils::Terminate();
        return;
    }
    catch (...)
    {
       cerr << "\nUnexpected exception during parsing: '" << fileName << "'\n";
        XMLPlatformUtils::Terminate();
        return;
    }

    //
    //  Extract the DOM tree, get the list of all the elements and report the
    //  length as the count of elements.
    //
    if (errorHandler.getSawErrors())
    {
        cout << "\nErrors occured, no output available\n" << endl;
    }
     else
    {
        DOMDocument *doc = parser.getDocument();
        unsigned int elementCount = doc->getElementsByTagName(XMLString::transcode("*"))->getLength();
                        
        DOMElement *root = doc->getDocumentElement();
                                        
        DOMNodeList *move = root->getElementsByTagName(XMLString::transcode("simulation"));

        for( int i = 0; i < 1/*move.getLength()*/; i++ ){ // specific for ONLY ONE Body
                DOMNamedNodeMap *attrmove = move->item(i)->getAttributes();
                bodyName = XMLString::transcode( attrmove->getNamedItem( XMLString::transcode("fps") )->getNodeValue() );
                if( !XMLString::compareString( attrmove->getNamedItem( XMLString::transcode("cycle") )->getNodeValue(), XMLString::transcode( "on" ) ) ){
                        cycle = true;
                } else {
                        cycle = false;
                }
                //deltaT = atof( XMLString::transcode( attrmove->getNamedItem( XMLString::transcode("duration") )->getNodeValue() ) );
                //time_err = 0.001;//atof( XMLString::transcode( attrmove->getNamedItem( XMLString::transcode("time_err") )->getNodeValue()) );
                tFinal = 0.0;
                DOMNodeList *motion = move->item(i)->getChildNodes();
                int numMotion = 0;
                for( int j = 0; j < motion->getLength(); j++ ){
                        if( !XMLString::compareString( motion->item(j)->getNodeName(), XMLString::transcode("motion" ) ) ) numMotion++;
                }
                motionTimeline = new COME_JointMotion [ numMotion ];
                qtdMotion = 0;
                for( int k = 0; k < motion->getLength(); k++ ){

                        if( !XMLString::compareString( motion->item(k)->getNodeName(), XMLString::transcode( "motion" ) ) ){

                                DOMNamedNodeMap *attrmotion = motion->item(k)->getAttributes();
                                string jointName = XMLString::transcode( attrmotion->getNamedItem( XMLString::transcode("joint_name") )->getNodeValue() );
                                float t0 = atof( XMLString::transcode( attrmotion->getNamedItem( XMLString::transcode("t0") )->getNodeValue() ) );
                                float tf = atof( XMLString::transcode( attrmotion->getNamedItem( XMLString::transcode("tf") )->getNodeValue() ) );
                                float parameter = atof( XMLString::transcode( attrmotion->getNamedItem( XMLString::transcode("parameter") )->getNodeValue() ) );
                                int motionType = FLEX;
                                if( !XMLString::compareString( attrmotion->getNamedItem( XMLString::transcode("type") )->getNodeValue(), XMLString::transcode( "ADDUCT" ) ) ){
                                        motionType = ADDUCT;
                                } else if( !XMLString::compareString( attrmotion->getNamedItem( XMLString::transcode("type") )->getNodeValue(), XMLString::transcode( "TWIST" ) ) ){
                                        motionType = TWIST;
                                }

                                motionTimeline[qtdMotion++] = *( new COME_JointMotion( jointName, motionType, t0, tf, parameter ) );
                                if( tf > tFinal ) tFinal = tf;
                        }
                }
        }
                                
        // Print out the stats that we collected and time taken.
        cout << fileName << ": " << duration << " ms (" << elementCount << " elems)." << endl;
    }
    return;
}

---