/*! * 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)); } }
/*! * Set power On * \param power : When set to 1 allows to turn on the motors. When 0 turns them off. */ void vpVirtuose::setPowerOn (const int &power) { init(); if (virtSetPowerOn(m_virtContext, power)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetPowerOn: error code %d", err)); } }
/*! * Set saturation values of the force feedback * \param forceLimit : Value expressed in N. * \param torqueLimit : Value expressed in Nm. */ void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit) { init(); if (virtSaturateTorque(m_virtContext, forceLimit, torqueLimit)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSaturateTorque: error code %d", err)); } }
/*! * Set the speed factor. * \param velocityFactor : Scale factor applied to the velocities set using setVelocity() */ void vpVirtuose::setVelocityFactor (const float &velocityFactor) { init(); if (virtSetSpeedFactor(m_virtContext, velocityFactor)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling setVelocityFactor: error code %d", err)); } }
/*! * Activate or desactivate force feedback. * \param enable : 1 to activate (system's default value), 0 to desactivate. */ void vpVirtuose::enableForceFeedback (int enable) { init(); if (virtEnableForceFeedback(m_virtContext, enable)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtEnableForceFeedback(): error code %d", err)); } }
/*! * Register the periodic function. This function could be started using * startLoop() and stopped using stopLoop(). * \param CallBackVirt : * \param period : * \param virtuose : */ void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt, float &period, vpVirtuose &virtuose) { init(); if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &period, &virtuose)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetPeriodicFunction: error code %d", err)); } }
/*! * Set force factor. * \param forceFactor : Force factor scale applied to the force torque tensor set by setForce(). */ void vpVirtuose::setForceFactor (const float &forceFactor) { init(); if (virtSetForceFactor(m_virtContext, forceFactor)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetForceFactor: error code %d", err)); } }
/*! * Set the the simulation time step. * \param timeStep : */ void vpVirtuose::setTimeStep (const float &timeStep) { init(); if (virtSetTimeStep(m_virtContext, timeStep)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err)); // throw(vpException(vpException::fatalError, // "Error calling virtSetTimeStep: %s", virtGetErrorMessage(err))); } }
/*! * Stop the periodic function set using setPeriodicFunction(). * * \sa startPeriodicFunction(), setPeriodicFunction() */ void vpVirtuose::stopPeriodicFunction() { init(); if (virtStopLoop(m_virtContext)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d", err)); } else std::cout << "Haptic loop closed." << std::endl; }
/*! * 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 command type. * \param type : Possible values are COMMAND_TYPE_NONE, COMMAND_TYPE_IMPEDANCE, COMMAND_TYPE_ARTICULAR_IMPEDANCE, COMMAND_TYPE_VIRTMECH * - COMMAND_TYPE_NONE : * - COMMAND_TYPE_IMPEDANCE : */ void vpVirtuose::setCommandType(const VirtCommandType &type) { init(); if (m_typeCommand != type) { m_typeCommand = type; if (virtSetCommandType(m_virtContext, m_typeCommand)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetCommandType: error code %d", err)); } } }
/*! * 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)); } } }
/*! * Return the status of DeadMan sensor : true if the sensor is ON (a user is holding the handle) * and false if the sensor is OFF (no user detected). */ bool vpVirtuose::getDeadMan() const { if (!m_is_init) { throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } int deadman; if (virtGetDeadMan(m_virtContext, &deadman)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtGetDeadMan: error code %d", err)); } return (deadman ? true : false); }
/*! * Return the command type. */ VirtCommandType vpVirtuose::getCommandType () const { if (!m_is_init) { throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } VirtCommandType type; if (virtGetCommandType(m_virtContext, &type)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtGetCommandType: error code %d", err)); } return type; }
/*! * Return device end effector velocity. */ vpColVector vpVirtuose::getVelocity() const { if (!m_is_init) { throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector vel(6,0); float speed[6]; if (virtGetSpeed(m_virtContext, speed)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Cannot get haptic device velocity: %s", virtGetErrorMessage(err))); } for(unsigned int i=0; i<6; i++) vel[i] = speed[i]; return vel; }
/*! * Return force tensor to be applied to the attached object. */ vpColVector vpVirtuose::getForce() const { if (!m_is_init) { throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector force(6,0); float force_[6]; if (virtGetForce(m_virtContext, force_)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtGetForce: error code %d", err)); } for(unsigned int i=0; i<6; i++) force[i] = force_[i]; return force; }
/*! * Return the 6 joint velocities of the virtuose. */ vpColVector vpVirtuose::getArticularVelocity() const { if (!m_is_init) { throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector articularVelocity(6,0); float articular_velocity_[6]; if (virtGetArticularSpeed(m_virtContext, articular_velocity_)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtGetArticularSpeed: error code %d", err)); } for(unsigned int i=0; i<6; i++) articularVelocity[i] = articular_velocity_[i]; return articularVelocity; }
/*! * Modify the current control speed. * \param velocity : Velocity twist vector, where translations velocities are expressed in * m/s and rotation velocities in rad/s. */ void vpVirtuose::setVelocity (vpColVector &velocity) { init(); if (velocity.size() != 6) { throw(vpException(vpException::dimensionError, "Cannot set a velocity vector (dim %d) that is not 6-dimension", velocity.size())); } float speed[6]; for(unsigned int i=0; i<6; i++) speed[i] = velocity[i]; if (virtSetSpeed(m_virtContext, speed)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetSpeed: error code %d", err)); } }
/*! * Add a force to be applied to the virtuose (impedance effort). * This function works in every mode. * \param force : Is 6 component dynamic tensor (three forces and three torques) wrt virtuose end-effector * and is expressed in the coordinates of the base frame. */ void vpVirtuose::addForce (vpColVector &force) { if (force.size() != 6) { throw(vpException(vpException::dimensionError, "Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension", force.size())); } init(); float virtforce[6]; for(unsigned int i=0; i<6; i++) virtforce[i] = force[i]; if (virtAddForce(m_virtContext, virtforce)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtAddForce: error code %d", err)); } }
/*! * Set articular force. * Works in mode COMMAND_TYPE_ARTICULAR_IMPEDANCE that need to be set with setCommandType(). * \param articularForce : */ void vpVirtuose::setArticularForce (const vpColVector &articularForce) { init(); if (articularForce.size() != 6) { throw(vpException(vpException::dimensionError, "Cannot apply an articular force feedback (dim %d) to the haptic device that is not 6-dimension", articularForce.size())); } float articular_force[6]; for(unsigned int i=0; i<6; i++) articular_force[i] = articularForce[i]; if (virtSetArticularForce(m_virtContext, articular_force)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetArticularForce: error code %d", err)); } }
/*! * Send articular (joint) velocity command to the virtuose. * Works in COMMAND_TYPE_ARTICULAR mode that need to be set with setCommandType(). * \param articularVelocity : Six dimension joint velocity vector. */ void vpVirtuose::setArticularVelocity (const vpColVector &articularVelocity) { init(); if (articularVelocity.size() != 6) { throw(vpException(vpException::dimensionError, "Cannot send an articular velocity command (dim %d) to the haptic device that is not 6-dimension", articularVelocity.size())); } float articular_velocity[6]; for(unsigned int i=0; i<6; i++) articular_velocity[i] = articularVelocity[i]; if (virtSetArticularSpeed(m_virtContext, articular_velocity)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetArticularVelocity: error code %d", err)); } }
/*! * Modify the current control position. * \param position : */ void vpVirtuose::setPosition (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 (virtSetPosition(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); throw(vpException(vpException::fatalError, "Error calling virtSetPosition: 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(); } } }