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