/*!
* 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();
}
}
}