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 base frame with respect to the observation frame * \param position : Position of the base frame. * * \sa setObservationFrame() */ void vpVirtuose::setBaseFrame (const vpPoseVector &position) { init(); float position_[7]; vpTranslationVector translation; vpQuaternionVector quaternion; position.extract(translation); position.extract(quaternion); for (int i=0; i<3; i++) position_[i] = translation[i]; for (int i=0; i<4; i++) position_[3+i] = quaternion[i]; if (virtSetBaseFrame(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetBaseFrame: 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; VecCoord& posH =*(visualHaptionDOF->x.beginEdit()); posH[0]=posBase.getValue()[0]; visualHaptionDOF->x.endEdit(); VecCoord& posA =*(visualAxesDOF->x.beginEdit()); posA[0]=posBase.getValue()[0]; posA[1]=posBase.getValue()[0]; posA[2]=posBase.getValue()[0]; visualAxesDOF->x.endEdit(); } } }