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