void SofaHAPIHapticsDevice::onBeginAnimationStep(const double /*dt*/)
{
if (!device.get()) return;
sofa::helper::AdvancedTimer::stepBegin("SofaHAPIHapticsDevice");
sofa::helper::AdvancedTimer::stepBegin("DeviceValues");
HAPI::HAPIHapticsDevice::DeviceValues dv = device->getDeviceValues();
sofa::helper::AdvancedTimer::stepEnd("DeviceValues");
sofa::helper::AdvancedTimer::stepBegin("XForm");
/// COMPUTATION OF THE virtualTool 6D POSITION IN THE World COORDINATES
Vec3d pos = conv(dv.position);
Quat quat = conv(dv.orientation);
Transform baseDevice_H_endDevice(pos*data.scale, quat);
Transform world_H_virtualTool = data.world_H_baseDevice * baseDevice_H_endDevice * data.endDevice_H_virtualTool;
lastToolPosition = world_H_virtualTool;
/*
Transform baseDevice_H_endDevice2 = data.world_H_baseDevice.inversed() * world_H_virtualTool * data.endDevice_H_virtualTool.inversed();
sout << "bHe = " << baseDevice_H_endDevice << sendl;
sout << "wHb = " << data.world_H_baseDevice << sendl;
sout << "dHt = " << data.endDevice_H_virtualTool << sendl;
sout << "wHv = " << world_H_virtualTool << sendl;
sout << "bHe2 = " << baseDevice_H_endDevice2 << sendl;
sout << sendl;
*/
sofa::helper::AdvancedTimer::stepEnd("XForm");
sofa::helper::AdvancedTimer::stepBegin("Button");
int buttonState = fakeButtonState | device->getButtonStatus();
int buttonChanged = buttonState ^ lastButtonState;
// special case: btn2 is mapped to tool selection if "toolSelector" is used
if (toolSelector.getValue() && (buttonChanged & HapticDeviceEvent::Button2StateMask))
{
if ((buttonState & HapticDeviceEvent::Button2StateMask) != 0)
{
// start tool switch : disable feedback on previous instrument
int currentToolIndex = toolIndex.getValue();
int newToolIndex = ((currentToolIndex+1)%toolCount.getValue());
toolIndex.setValue(newToolIndex);
if (toolTransitionSpringStiffness.getValue() != 0.0 && mState)
{
sout << "Enabling tool transition spring" << sendl;
sofa::helper::ReadAccessor<Data<sofa::helper::vector<sofa::defaulttype::RigidCoord<3,double> > > > x = *this->mState->read(sofa::core::VecCoordId::position());
Transform world_H_virtualTool(x[newToolIndex].getCenter(), x[newToolIndex].getOrientation());
Transform baseDevice_H_endDevice2 = data.world_H_baseDevice.inversed() * world_H_virtualTool * data.endDevice_H_virtualTool.inversed();
transitionEffect.reset(
new HAPI::HapticSpring( conv(baseDevice_H_endDevice2.getOrigin()*(1/data.scale)),
toolTransitionSpringStiffness.getValue()));
device->addEffect(transitionEffect.get(), 1.0);
}
setToolFeedback(currentToolIndex, false);
isToolControlled = false; // we disable update of the tool position and feedback until the button is released
}
else
{
if (transitionEffect.get())
{
sout << "Disabling tool transition spring" << sendl;
device->removeEffect(transitionEffect.get());
transitionEffect.reset();
}
setToolFeedback(toolIndex.getValue(), true);
isToolControlled = true;
}
}
sofa::helper::AdvancedTimer::stepNext("Button", "Event");
if (buttonState != lastButtonState)
{
lastButtonState = buttonState;
sendHapticDeviceEvent();
}
sofa::helper::AdvancedTimer::stepEnd("Event");
if (isToolControlled) // ignore haptic device if tool is unselected
{
const int currentToolIndex = toolIndex.getValue();
sofa::helper::AdvancedTimer::stepBegin("FFB");
// store actual position of interface for the forcefeedback (as it will be used as soon as new LCP will be computed)
for (unsigned int i=0; i < feedbackEffects.size(); ++i)
{
SofaHAPIForceFeedbackEffect::SPtr ffe = feedbackEffects[i];
if (ffe->getIndice() != currentToolIndex) continue;
if (ffe->getForceFeedback())
ffe->getForceFeedback()->setReferencePosition(world_H_virtualTool);
}
sofa::helper::AdvancedTimer::stepEnd("FFB");
if (mState)
{
sofa::helper::AdvancedTimer::stepBegin("SetState");
/// TODO : SHOULD INCLUDE VELOCITY !!
sofa::helper::WriteAccessor<Data<sofa::helper::vector<sofa::defaulttype::RigidCoord<3,double> > > > x = *this->mState->write(sofa::core::VecCoordId::position());
sofa::helper::WriteAccessor<Data<sofa::helper::vector<sofa::defaulttype::RigidCoord<3,double> > > > xfree = *this->mState->write(sofa::core::VecCoordId::freePosition());
xfree[currentToolIndex].getCenter() = world_H_virtualTool.getOrigin();
x[currentToolIndex].getCenter() = world_H_virtualTool.getOrigin();
// std::cout << world_H_virtualTool << std::endl;
xfree[currentToolIndex].getOrientation() = world_H_virtualTool.getOrientation();
x[currentToolIndex].getOrientation() = world_H_virtualTool.getOrientation();
sofa::helper::AdvancedTimer::stepEnd("SetState", "UpdateMapping");
sofa::simulation::Node *node = dynamic_cast<sofa::simulation::Node*> (this->getContext());
if (node)
{
sofa::helper::AdvancedTimer::stepBegin("UpdateMapping");
sofa::simulation::MechanicalPropagateOnlyPositionAndVelocityVisitor mechaVisitor(sofa::core::MechanicalParams::defaultInstance()); mechaVisitor.execute(node);
sofa::simulation::UpdateMappingVisitor updateVisitor(sofa::core::ExecParams::defaultInstance()); updateVisitor.execute(node);
sofa::helper::AdvancedTimer::stepEnd("UpdateMapping");
}
}
}
else
{
}
sofa::helper::AdvancedTimer::stepEnd("SofaHAPIHapticsDevice");
}
void OmniDriverEmu::handleEvent(core::objectmodel::Event *event)
{
//std::cout<<"NewEvent detected !!"<<std::endl;
if (dynamic_cast<sofa::simulation::AnimateBeginEvent *>(event))
{
cout << "test handle event "<< endl;
//getData(); // copy data->servoDeviceData to gDeviceData
//if (!simulateTranslation.getValue()) {
copyDeviceDataCallback(&data);
cout << data.deviceData.ready<< endl;
if (data.deviceData.ready)
{
cout << "Data ready, event" << endl;
data.deviceData.quat.normalize();
//sout << "driver is working ! " << data->servoDeviceData.transform[12+0] << endl;
if (isToolControlled) // ignore haptic device if tool is unselected
{
/// COMPUTATION OF THE vituralTool 6D POSITION IN THE World COORDINATES
SolidTypes<double>::Transform baseOmni_H_endOmni(data.deviceData.pos*data.scale, data.deviceData.quat);
SolidTypes<double>::Transform world_H_virtualTool = data.world_H_baseOmni * baseOmni_H_endOmni * data.endOmni_H_virtualTool;
//----------------------------
data.forceFeedbackIndice=currentToolIndex;
// store actual position of interface for the forcefeedback (as it will be used as soon as new LCP will be computed)
//data.forceFeedback->setReferencePosition(world_H_virtualTool);
for (unsigned int i=0; i<data.forceFeedbacks.size(); i++)
if (data.forceFeedbacks[i]->indice==data.forceFeedbackIndice)
data.forceFeedbacks[i]->setReferencePosition(world_H_virtualTool);
//-----------------------------
/// TODO : SHOULD INCLUDE VELOCITY !!
//sofa::core::objectmodel::HapticDeviceEvent omniEvent(data.deviceData.id, world_H_virtualTool.getOrigin(), world_H_virtualTool.getOrientation() , data.deviceData.m_buttonState);
//this->getContext()->propagateEvent(sofa::core::ExecParams::defaultInstance(), &omniEvent);
helper::WriteAccessor<Data<helper::vector<RigidCoord<3,double> > > > x = *this->mState->write(core::VecCoordId::position());
helper::WriteAccessor<Data<helper::vector<RigidCoord<3,double> > > > xfree = *this->mState->write(core::VecCoordId::freePosition());
xfree[currentToolIndex].getCenter() = world_H_virtualTool.getOrigin();
x[currentToolIndex].getCenter() = world_H_virtualTool.getOrigin();
xfree[currentToolIndex].getOrientation() = world_H_virtualTool.getOrientation();
x[currentToolIndex].getOrientation() = world_H_virtualTool.getOrientation();
sofa::simulation::Node *node = dynamic_cast<sofa::simulation::Node*> (this->getContext());
if (node)
{
sofa::simulation::MechanicalPropagatePositionAndVelocityVisitor mechaVisitor(sofa::core::MechanicalParams::defaultInstance()); mechaVisitor.execute(node);
sofa::simulation::UpdateMappingVisitor updateVisitor(sofa::core::ExecParams::defaultInstance()); updateVisitor.execute(node);
}
}
else
{
data.forceFeedbackIndice = -1;
}
if (moveOmniBase)
{
std::cout<<" new positionBase = "<<positionBase_buf[0]<<std::endl;
visu_base->applyTranslation(positionBase_buf[0]-positionBase.getValue()[0],
positionBase_buf[1]-positionBase.getValue()[1],
positionBase_buf[2]-positionBase.getValue()[2]);
positionBase.setValue(positionBase_buf);
setDataValue();
//this->reinitVisual();
}
executeAsynchro=true;
}
else
std::cout<<"data not ready"<<std::endl;
//} else {
}
if (dynamic_cast<core::objectmodel::KeypressedEvent *>(event))
{
core::objectmodel::KeypressedEvent *kpe = dynamic_cast<core::objectmodel::KeypressedEvent *>(event);
if (kpe->getKey()=='Z' ||kpe->getKey()=='z' )
{
moveOmniBase = !moveOmniBase;
std::cout<<"key z detected "<<std::endl;
omniVisu.setValue(moveOmniBase);
if(moveOmniBase)
{
this->cleanup();
positionBase_buf = positionBase.getValue();
}
else
{
this->reinit();
}
}
if(kpe->getKey()=='K' || kpe->getKey()=='k')
{
positionBase_buf.x()=0.0;
positionBase_buf.y()=0.5;
positionBase_buf.z()=2.6;
}
if(kpe->getKey()=='L' || kpe->getKey()=='l')
{
positionBase_buf.x()=-0.15;
positionBase_buf.y()=1.5;
positionBase_buf.z()=2.6;
}
if(kpe->getKey()=='M' || kpe->getKey()=='m')
{
positionBase_buf.x()=0.0;
positionBase_buf.y()=2.5;
positionBase_buf.z()=2.6;
}
// emulated haptic buttons B=btn1, N=btn2
if (kpe->getKey()=='H' || kpe->getKey()=='h')
{
std::cout << "emulated button 1 pressed" << std::endl;
Vector3 dummyVector;
Quat dummyQuat;
sofa::core::objectmodel::HapticDeviceEvent event(currentToolIndex,dummyVector,dummyQuat,
sofa::core::objectmodel::HapticDeviceEvent::Button1StateMask);
simulation::Node *groot = dynamic_cast<simulation::Node *>(getContext()->getRootContext()); // access to current node
groot->propagateEvent(core::ExecParams::defaultInstance(), &event);
}
if (kpe->getKey()=='J' || kpe->getKey()=='j')
{
std::cout << "emulated button 2 pressed" << std::endl;
Vector3 dummyVector;
Quat dummyQuat;
sofa::core::objectmodel::HapticDeviceEvent event(currentToolIndex,dummyVector,dummyQuat,
sofa::core::objectmodel::HapticDeviceEvent::Button2StateMask);
simulation::Node *groot = dynamic_cast<simulation::Node *>(getContext()->getRootContext()); // access to current node
groot->propagateEvent(core::ExecParams::defaultInstance(), &event);
}
}
if (dynamic_cast<core::objectmodel::KeyreleasedEvent *>(event))
{
core::objectmodel::KeyreleasedEvent *kre = dynamic_cast<core::objectmodel::KeyreleasedEvent *>(event);
// emulated haptic buttons B=btn1, N=btn2
if (kre->getKey()=='H' || kre->getKey()=='h'
|| kre->getKey()=='J' || kre->getKey()=='j')
{
std::cout << "emulated button released" << std::endl;
Vector3 dummyVector;
Quat dummyQuat;
sofa::core::objectmodel::HapticDeviceEvent event(currentToolIndex,dummyVector,dummyQuat,0);
simulation::Node *groot = dynamic_cast<simulation::Node *>(getContext()->getRootContext()); // access to current node
groot->propagateEvent(core::ExecParams::defaultInstance(), &event);
}
}
}