自学教程：C++ virtGetErrorCode函数代码示例

/*! * Initialize virtuose device opening the connection to the device and setting the default command type. * If the device is already initialized, a call to init() does nothing. */void vpVirtuose::init(){  if (! m_is_init) {    m_virtContext = virtOpen(m_ip.c_str());    if (m_virtContext == NULL) {      int err = virtGetErrorCode(m_virtContext);      throw(vpException(vpException::fatalError, "Cannot open haptic device: %s",                        virtGetErrorMessage(err)));    }    if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, &m_ctrlMinorVersion)) {      int err = virtGetErrorCode(m_virtContext);      throw(vpException(vpException::fatalError, "Cannot get haptic device controller version: %s",                        virtGetErrorMessage(err)));    }    if (m_verbose) {      std::cout << "Controller version: " << m_ctrlMajorVersion << "." << m_ctrlMinorVersion << std::endl;    }    if (virtSetCommandType(m_virtContext, m_typeCommand)) {      int err = virtGetErrorCode(m_virtContext);      throw(vpException(vpException::fatalError, "Cannot set haptic device command type: %s",                        virtGetErrorMessage(err)));    }    m_is_init = true;  }}

/*! * Return position of the virtuose (or the object attached to it) wrt the environment frame. */vpPoseVector vpVirtuose::getPosition() const{  if (!m_is_init) {    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));  }  vpPoseVector position;  float position_[7];  vpTranslationVector translation;  vpQuaternionVector quaternion;  if (virtGetPosition(m_virtContext, position_)) {    int err = virtGetErrorCode(m_virtContext);    throw(vpException(vpException::fatalError,                      "Error calling virtGetPosition: error code %d", err));  }  else  {    for (int i=0; i<3; i++)      translation[i] = position_[i];    for (int i=0; i<4; i++)      quaternion[i] = position_[3+i];    vpThetaUVector thetau(quaternion);    position.buildFrom(translation, thetau);  }  return position;}

int HaptionDriver::initDevice(char* ip){    cout<<"HaptionDriver::initDevice() called"<<endl;    connection_device = 0;    /*m_indexingMode = INDEXING_ALL_FORCE_FEEDBACK_INHIBITION;*/    m_indexingMode = INDEXING_ALL;    m_speedFactor = 1.0;    haptic_time_step = 0.003f;    //haptic_time_step = 0.5f;    myData.m_virtContext = NULL;    cout<<"tentative de connection sur: "<<ip<<endl;    myData.m_virtContext = virtOpen (ip);    if (myData.m_virtContext == NULL)    {        cout<<"erreur connection"<<endl;        return 0;    }    else        cout<<"connection OK"<<endl;    virtSetIndexingMode(myData.m_virtContext, m_indexingMode);    cout<<"virtSetSpeedFactor return "<<virtSetSpeedFactor(myData.m_virtContext, m_speedFactor)<<endl;    float speddFactor[1];    virtGetSpeedFactor(myData.m_virtContext, speddFactor);    cout<<"virtGetSpeedFactor return "<<speddFactor[0]<<endl;    virtSetTimeStep(myData.m_virtContext,haptic_time_step);    cout<<"set base frame ok"<<endl;    m_typeCommand = COMMAND_TYPE_IMPEDANCE;    m_forceFactor = 1.0f;    virtSetCommandType(myData.m_virtContext, m_typeCommand);    virtSetForceFactor(myData.m_virtContext, m_forceFactor);    virtSetPowerOn(myData.m_virtContext, 1);    cout<<"init callback"<<endl;    virtSetPeriodicFunction(myData.m_virtContext, haptic_callback, &haptic_time_step, &myData);    cout<<"callback initialise"<<endl;    virtSaturateTorque(myData.m_virtContext, 15.0f,0.7f);    cout<<posBase.getValue()[0].getCenter()<<" "<<posBase.getValue()[0].getOrientation()<<endl;    float baseFrame[7] = { (float) posBase.getValue()[0].getCenter().x()/(float) scale.getValue(),            (float) posBase.getValue()[0].getCenter().y()/(float) scale.getValue(),            (float) posBase.getValue()[0].getCenter().z()/(float) scale.getValue(),            (float) posBase.getValue()[0].getOrientation()[0],            (float) posBase.getValue()[0].getOrientation()[1],            (float) posBase.getValue()[0].getOrientation()[2],            (float) posBase.getValue()[0].getOrientation()[3]                         };    cout<<"virtSetBaseFrame return "<<virtSetBaseFrame(myData.m_virtContext, baseFrame)<<endl;    cout<<"virtGetErrorCode return "<<virtGetErrorCode(myData.m_virtContext)<<endl;    return 1;}

/*! * Set indexing (offset) mode. * /param type : Possible choices: INDEXING_ALL, INDEXING_TRANS (only translations), * INDEXING_NONE */void vpVirtuose::setIndexingMode (const VirtIndexingType &type){  init();  if (virtSetIndexingMode(m_virtContext, type)) {    int err = virtGetErrorCode(m_virtContext);    throw(vpException(vpException::fatalError,                      "Error calling setIndexingMode: error code %d", err));  }}

/*! * Return the status of the emergency stop button : true if the system is operational (button correctly plugged and not triggered) * and false if the system is not operational (button not plugged or triggered). */bool vpVirtuose::getEmergencyStop() const{  if (!m_is_init) {    throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));  }  int emergencyStop;  if (virtGetEmergencyStop(m_virtContext, &emergencyStop)) {    int err = virtGetErrorCode(m_virtContext);    throw(vpException(vpException::fatalError,                      "Error calling virtGetEmergencyStop: error code %d", err));  }  return (emergencyStop ? true : false);}

/*! * Set the the simulation time step. * The function must be called before the selection of the type of control mode. * /param timeStep : Simulation time step (seconds). */void vpVirtuose::setTimeStep (const float &timeStep){  init();  if (m_period != timeStep){    m_period = timeStep;    if (virtSetTimeStep(m_virtContext, m_period)) {      int err = virtGetErrorCode(m_virtContext);      throw(vpException(vpException::fatalError,                        "Error calling virtSetTimeStep: error code %d", err));    }  }}

void HaptionDriver::onKeyPressedEvent(core::objectmodel::KeypressedEvent *kpe){    if(!visuAxes && kpe->getKey()==49)    {        sofa::simulation::tree::GNode *parent = dynamic_cast<sofa::simulation::tree::GNode*>(this->getContext());        parent->getParent()->addChild(nodeAxesVisual);        nodeAxesVisual->updateContext();        visuAxes=true;    }    else if(visuAxes && kpe->getKey()==49)    {        sofa::simulation::tree::GNode *parent = dynamic_cast<sofa::simulation::tree::GNode*>(this->getContext());        parent->getParent()->removeChild(nodeAxesVisual);        nodeAxesVisual->updateContext();        visuAxes=false;    }    if(visuAxes  && haptionVisu.getValue())    {        double pi = 3.1415926535;        if ((kpe->getKey()=='X' || kpe->getKey()=='x') && !modX )        {            modX=true;        }        if ((kpe->getKey()=='Y' || kpe->getKey()=='y') && !modY )        {            modY=true;        }        if ((kpe->getKey()=='Z' || kpe->getKey()=='z') && !modZ )        {            modZ=true;        }        if ((kpe->getKey()=='Q' || kpe->getKey()=='q') && !modS )        {            modS=true;        }        if (kpe->getKey()==18) //left        {            if(modX || modY || modZ)            {                VecCoord& posB =(*posBase.beginEdit());                posB[0].getCenter()+=posB[0].getOrientation().rotate(Vec3d(-(int)modX,-(int)modY,-(int)modZ));                posBase.endEdit();            }            else            {                scale.setValue(scale.getValue()-5);                changeScale = true;            }        }        else if (kpe->getKey()==20) //right        {            if(modX || modY || modZ)            {                VecCoord& posB =(*posBase.beginEdit());                posB[0].getCenter()+=posB[0].getOrientation().rotate(Vec3d((int)modX,(int)modY,(int)modZ));                posBase.endEdit();            }            else            {                scale.setValue(scale.getValue()+5);                changeScale = true;            }        }        else if ((kpe->getKey()==21) && (modX || modY || modZ)) //down        {            VecCoord& posB =(*posBase.beginEdit());            sofa::helper::Quater<double> quarter_transform(Vec3d((int)modX,(int)modY,(int)modZ),-pi/50);            posB[0].getOrientation()*=quarter_transform;            posBase.endEdit();        }        else if ((kpe->getKey()==19) && (modX || modY || modZ)) //up        {            VecCoord& posB =(*posBase.beginEdit());            sofa::helper::Quater<double> quarter_transform(Vec3d((int)modX,(int)modY,(int)modZ),+pi/50);            posB[0].getOrientation()*=quarter_transform;            posBase.endEdit();        }        if ((kpe->getKey()=='E' || kpe->getKey()=='e'))        {            VecCoord& posB =(*posBase.beginEdit());            posB[0].clear();            posBase.endEdit();        }        if(modX || modY || modZ)        {            float baseFrame[7] = { (float) posBase.getValue()[0].getCenter()[0]/(float) scale.getValue(),                    (float) posBase.getValue()[0].getCenter()[1]/(float) scale.getValue(),                    (float) posBase.getValue()[0].getCenter()[2]/(float) scale.getValue(),                    (float) posBase.getValue()[0].getOrientation()[0],                    (float) posBase.getValue()[0].getOrientation()[1],                    (float) posBase.getValue()[0].getOrientation()[2],                    (float) posBase.getValue()[0].getOrientation()[3]                                 };            cout<<"virtSetBaseFrame return "<<virtSetBaseFrame(myData.m_virtContext, baseFrame)<<endl;            cout<<"virtGetErrorCode return "<<virtGetErrorCode(myData.m_virtContext)<<endl;//.........这里部分代码省略.........