void CServerDriver_Hydra::ScanForNewControllers( bool bNotifyServer )
{
for ( int base = 0; base < sixenseGetMaxBases(); ++base )
{
if ( sixenseIsBaseConnected( base ) )
{
sixenseSetActiveBase( base );
for ( int i = 0; i < sixenseGetMaxControllers(); ++i )
{
if ( sixenseIsControllerEnabled( i ) )
{
char buf[256];
GenerateSerialNumber( buf, sizeof( buf ), base, i );
scope_lock lock( m_Mutex );
if ( !FindTrackedDeviceDriver( buf, vr::ITrackedDeviceServerDriver_Version ) )
{
DriverLog( "added new device %s\n", buf );
m_vecControllers.push_back( new CHydraHmdLatest( m_pDriverHost, base, i ) );
if ( bNotifyServer && m_pDriverHost )
{
m_pDriverHost->TrackedDeviceAdded( m_vecControllers.back()->GetSerialNumber() );
}
}
}
}
}
}
}
void HydraManager::Update(float dt)
{
int left_index = sixenseUtils::getTheControllerManager()->getIndex( sixenseUtils::ControllerManager::P1L );
int right_index = sixenseUtils::getTheControllerManager()->getIndex( sixenseUtils::ControllerManager::P1R );
sixenseSetActiveBase(0);
sixenseGetAllNewestData(&mAcd);
}
void FlyingMouse::updateHydraData()
{
#ifdef USE_SIXENSE
//int left_index = sixenseUtils::getTheControllerManager()->getIndex(sixenseUtils::ControllerManager::P1L);
//int right_index = sixenseUtils::getTheControllerManager()->getIndex(sixenseUtils::ControllerManager::P1R);
m_active = false;
const int maxBases = sixenseGetMaxBases();
for (int base = 0; base < maxBases; ++base)
{
sixenseSetActiveBase(base);
if (!sixenseIsBaseConnected(base))
continue;
sixenseAllControllerData acd;
sixenseGetAllNewestData(&acd);
const int maxControllers = sixenseGetMaxControllers();
for (int cont = 0; cont < maxControllers; cont++)
{
if (!sixenseIsControllerEnabled(cont))
continue;
m_active = true;
const sixenseControllerData& cd = acd.controllers[cont];
float* mtx = mtxL;
if (cd.which_hand == 2)
mtx = mtxR;
mtx[0] = cd.rot_mat[0][0];
mtx[1] = cd.rot_mat[0][1];
mtx[2] = cd.rot_mat[0][2];
mtx[3] = 0.0f;
mtx[4] = cd.rot_mat[1][0];
mtx[5] = cd.rot_mat[1][1];
mtx[6] = cd.rot_mat[1][2];
mtx[7] = 0.0f;
mtx[ 8] = cd.rot_mat[2][0];
mtx[ 9] = cd.rot_mat[2][1];
mtx[10] = cd.rot_mat[2][2];
mtx[11] = 0.0f;
const float posS = 0.001f; // Try to match world space
mtx[12] = cd.pos[0] * posS;
mtx[13] = cd.pos[1] * posS;
mtx[14] = cd.pos[2] * posS;
mtx[15] = 1.0f;
}
g_lastAcd = g_curAcd;
g_curAcd = acd;
}
//sixenseUtils::getTheControllerManager()->update(&acd);
#endif // USE_SIXENSE
}
bool Hydra_Tracker::GiveHeadPoseData(double *data)
{
sixenseSetActiveBase(0);
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
//sixenseUtils::getTheControllerManager()->update( &acd );
//sixenseControllerData cd;
//Rotation quat = Rotation(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
sixenseMath::Matrix4 mat = sixenseMath::Matrix4(acd.controllers[0].rot_mat);// sixenseMath::Quat(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
double yaw = 0.0f;
double pitch = 0.0f;
double roll = 0.0f;
float ypr[3];
mat.getEulerAngles().fill(ypr);
newHeadPose[Yaw] = ypr[0];
newHeadPose[Pitch] = ypr[1];
newHeadPose[Roll] = ypr[2];
newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
newHeadPose[TZ] = acd.controllers[0].pos[2]/50.0f;
if (bEnableX) {
data[TX] = newHeadPose[TX];
}
if (bEnableY) {
data[TY] = newHeadPose[TY];
}
if (bEnableY) {
data[TZ] = newHeadPose[TZ];
}
if (bEnableYaw) {
data[Yaw] = newHeadPose[Yaw] * 57.295781f;
}
if (bEnablePitch) {
data[Pitch] = newHeadPose[Pitch] * 57.295781f;
}
if (bEnableRoll) {
data[Roll] = newHeadPose[Roll] * 57.295781f;
}
return true;
}
dev::sixense::sixense() :
status(false),
flying(false)
{
if ((status = (sixenseInit() == SIXENSE_SUCCESS &&
sixenseSetActiveBase(0) == SIXENSE_SUCCESS)))
{
float nr = ::conf->get_f("sixense_filter_near_range", 500.00);
float nv = ::conf->get_f("sixense_filter_near_value", 0.95);
float fr = ::conf->get_f("sixense_filter_far_range", 1600.00);
float fv = ::conf->get_f("sixense_filter_far_value", 0.99);
sixenseSetFilterParams(nr, nv, fr, fv);
sixenseSetFilterEnabled(1);
}
move_rate = ::conf->get_f("sixense_move_rate", 30.0);
turn_rate = ::conf->get_f("sixense_turn_rate", 120.0);
hand_controller = ::conf->get_i("sixense_hand_controller", 0);
fly_button = ::conf->get_i("sixense_fly_button ", 1);
}
void Hydra_Tracker::GetHeadPoseData(double *data)
{
sixenseSetActiveBase(0);
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
sixenseMath::Matrix4 mat = sixenseMath::Matrix4(acd.controllers[0].rot_mat);
float ypr[3];
mat.getEulerAngles().fill(ypr);
newHeadPose[Yaw] = ypr[0];
newHeadPose[Pitch] = ypr[1];
newHeadPose[Roll] = ypr[2];
newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
newHeadPose[TZ] = acd.controllers[0].pos[2]/50.0f;
if (s.bEnableX) {
data[TX] = newHeadPose[TX];
}
if (s.bEnableY) {
data[TY] = newHeadPose[TY];
}
if (s.bEnableY) {
data[TZ] = newHeadPose[TZ];
}
if (s.bEnableYaw) {
data[Yaw] = newHeadPose[Yaw] * 57.295781f;
}
if (s.bEnablePitch) {
data[Pitch] = newHeadPose[Pitch] * 57.295781f;
}
if (s.bEnableRoll) {
data[Roll] = newHeadPose[Roll] * 57.295781f;
}
}
void CServerDriver_Hydra::ThreadFunc()
{
// We know the sixense SDK thread is running at "60 FPS", but we don't know when
// those frames are. To minimize latency, we sleep for slightly less than the
// target rate, and detect when the frame has not advanced to wait a bit longer.
auto longInterval = std::chrono::milliseconds( 16 );
auto retryInterval = std::chrono::milliseconds( 2 );
auto scanInterval = std::chrono::seconds( 1 );
auto pollDeadline = std::chrono::steady_clock::now();
auto scanDeadline = std::chrono::steady_clock::now() + scanInterval;
#ifdef _WIN32
// Request at least 2ms timing granularity for the life of this process
timeBeginPeriod( 2 );
#endif
while ( !m_bStopRequested )
{
// Check for new controllers here because sixense API is modal
// (e.g. sixenseSetActiveBase()) so it can't happen in parallel with pose updates
if ( pollDeadline > scanDeadline )
{
ScanForNewControllers( true );
scanDeadline += scanInterval;
}
bool bAnyActivated = false;
bool bAllUpdated = true;
for ( int base = 0; base < sixenseGetMaxBases(); ++base )
{
if ( !sixenseIsBaseConnected( base ) )
continue;
sixenseAllControllerData acd;
sixenseSetActiveBase( base );
if ( sixenseGetAllNewestData( &acd ) != SIXENSE_SUCCESS )
continue;
for ( int id = 0; id < sixenseGetMaxControllers(); ++id )
{
for ( auto it = m_vecControllers.begin(); it != m_vecControllers.end(); ++it )
{
CHydraHmdLatest *pHydra = *it;
if ( pHydra->IsActivated() && pHydra->HasControllerId( base, id ) )
{
bAnyActivated = true;
// Returns true if this is new data (so we can sleep for long interval)
if ( !pHydra->Update( acd.controllers[id] ) )
{
bAllUpdated = false;
}
break;
}
}
}
}
CheckForChordedSystemButtons();
// If everyone just got new data, we can wait about 1/60s, else try again soon
pollDeadline += !bAnyActivated ? scanInterval :
bAllUpdated ? longInterval : retryInterval;
std::this_thread::sleep_until( pollDeadline );
}
#ifdef _WIN32
timeEndPeriod( 2 );
#endif
}
int main(int argc, char **argv)
{
ArgsParser arg(argc, argv);
// get help
if (argc < 2
|| 0 == strcasecmp(argv[1], "-h")
|| 0 == strcasecmp(argv[1], "--help"))
{
displayHelp(argv[0]);
exit(-1);
}
const char *target = arg.getOpt("-t", "--target", "localhost:7777");
const char *rateStr = arg.getOpt("-r", "--rate", "20");
const char *sendRate = arg.getOpt("-s", "--sendrate", "16");
// get port on which the dtrack sends the data
float rate = atof(rateStr);
// verbose max. once per second
int sendsPerVerbose;
if (rate < 1)
sendsPerVerbose = 1;
else
sendsPerVerbose = (int)rate;
// select() delay record
rate = 1.0 / rate;
struct timeval delay;
delay.tv_sec = (int)rate;
delay.tv_usec = (int)(1e6 * (rate - delay.tv_sec));
// +++++++++++++++ prepare mapping +++++++++++++++
int artID[MAXSENSORS], coverID[MAXSENSORS]; // COVER IDs for sensor[i]
if (arg.numArgs() > MAXSENSORS)
{
std::cerr << "Only " << MAXSENSORS << " sensors allowed" << std::endl;
}
int i;
int numSensors = 0;
for (i = 0; i < arg.numArgs(); i++)
{
coverID[numSensors] = i;
bool ok = splitOpt(arg[i], artID[numSensors], coverID[numSensors]);
if (ok)
{
numSensors++;
}
else
{
std::cerr << "Illegal device selection: " << arg[i] << std::endl;
exit(0);
}
}
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Show what we will do
printf("\n");
printf(" +-----------------------------------------------------+\n");
printf(" + VRC HydraServer %-10s (C) 2005 HLRS +\n", HydraServerVersion);
printf(" +-----------------------------------------------------+\n");
printf(" + Settings: +\n");
printf(" + UDP Target: %-30s +\n", target);
printf(" + Send Rate: %-3.1f Packets/s +\n", 1.0 / rate);
printf(" +-----------------------------------------------------+\n");
printf(" + Mapping: +\n");
for (i = 0; i < numSensors; i++)
{
printf(" + Target %c%-2d --> COVER ID %-2d +\n",
((artID[i] >= 10) ? 'F' : 'B'),
(artID[i] % 10) + 1,
coverID[i]);
}
printf(" +-----------------------------------------------------+\n\n");
/// ++++++++++++++++++++++++++ All parameters set - start work +++++++++++++++++++
signal(SIGINT, sigHandler);
#ifndef WIN32
signal(SIGPIPE, sigHandler);
signal(SIGCHLD, sigHandler);
#endif
signal(SIGTERM, sigHandler);
// create udp socket
UDP_Sender sender(target);
if (sender.isBad())
{
std::cerr << "Could not start UDP server to "
<< target << " : "
<< sender.errorMessage() << std::endl;
return -1;
}
int init = sixenseInit();
int activebase = sixenseSetActiveBase(0);
int basecolor = sixenseSetBaseColor(255, 0, 0);
sixenseAllControllerData acd;
int reshigh = sixenseSetHighPriorityBindingEnabled(1);
int autoenable = sixenseAutoEnableHemisphereTracking(1);
int frame = 0;
while (1)
{
sixenseGetAllNewestData(&acd);
Sleep((int)*sendRate);
/*fprintf(stderr,"left pos: x = %f\ty = %f\tz = %f\nright pos: x = %f\ty = %f\t z = %f\n\n",
acd.controllers[0].pos[0],acd.controllers[0].pos[1],acd.controllers[0].pos[2],
acd.controllers[1].pos[0],acd.controllers[1].pos[1],acd.controllers[1].pos[2]); */
for (i = 0; i < numSensors; i++)
{
char sendbuffer[2048];
sprintf(sendbuffer, "VRC %d %3d [%5.1f %5.1f %5.1f] - [%6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f] - [ %d %d %s]",
coverID[i], acd.controllers[i].buttons, acd.controllers[i].pos[0], acd.controllers[i].pos[1], acd.controllers[i].pos[2],
acd.controllers[i].rot_mat[0][0], acd.controllers[i].rot_mat[0][1], acd.controllers[i].rot_mat[0][2],
acd.controllers[i].rot_mat[1][0], acd.controllers[i].rot_mat[1][1], acd.controllers[i].rot_mat[1][2],
acd.controllers[i].rot_mat[2][0], acd.controllers[i].rot_mat[2][1], acd.controllers[i].rot_mat[2][2],
acd.controllers[i].joystick_x, acd.controllers[i].joystick_y, compatibilityString);
sender.send(sendbuffer);
if (frame % sendsPerVerbose == 0)
{
fprintf(stderr, "%s\n", sendbuffer);
}
}
if (frame % sendsPerVerbose == 0)
{
fprintf(stderr, "---\n");
}
frame++;
}
sixenseExit();
return 0;
}
//===========================================================================
cHydraDevice::cHydraDevice(unsigned int a_deviceNumber):
a_deviceNumber(a_deviceNumber)
{
// the connection to your device has not yet been established.
m_deviceReady = false;
/************************************************************************
STEP 1:
Here you should complete the specifications of your device.
These values only need to be estimates. Since haptic devices often perform
differently depending of their configuration withing their workspace,
simply use average values.
*************************************************************************/
//-----------------------------------------------------------------------
// NAME:
//-----------------------------------------------------------------------
// haptic device model (see file "CGenericHapticDevice.h")
m_specifications.m_model = C_HAPTIC_DEVICE_HYDRA;
// name of the device manufacturer, research lab, university.
m_specifications.m_manufacturerName = "Razor";
// name of your device
m_specifications.m_modelName = "Hydra";
//-----------------------------------------------------------------------
// CHARACTERISTICS: (The following values must be positif or equal to zero)
//-----------------------------------------------------------------------
// the maximum force [N] the device can produce along the x,y,z axis.
m_specifications.m_maxLinearForce = 0.0; // [N]
// the maximum amount of torque your device can provide arround its
// rotation degrees of freedom.
m_specifications.m_maxAngularTorque = 0.0; // [N*m]
// the maximum amount of torque which can be provided by your gripper
m_specifications.m_maxGripperForce = 0.0; // [N]
// the maximum closed loop linear stiffness in [N/m] along the x,y,z axis
m_specifications.m_maxLinearStiffness = 0.0; // [N/m]
// the maximum amount of angular stiffness
m_specifications.m_maxAngularStiffness = 0.0; // [N*m/Rad]
// the maximum amount of stiffness supported by the gripper
m_specifications.m_maxGripperLinearStiffness = 0; // [N*m]
// the radius of the physiqual workspace of the device (x,y,z axis)
m_specifications.m_workspaceRadius = 0.3; // [m]
// DAMPING PROPERTIES:
// Start with small values as damping terms can be high;y sensitive to
// the quality of your velocity signal and the spatial resolution of your
// device. Try gradually increasing the values by using example "01-devices" and by
// enabling viscosity with key command "2".
// Maximum recommended linear damping factor Kv
m_specifications.m_maxLinearDamping = 0.0; // [N/(m/s)]
//! Maximum recommended angular damping factor Kv (if actuated torques are available)
m_specifications.m_maxAngularDamping = 0.0; // [N*m/(Rad/s)]
//! Maximum recommended angular damping factor Kv for the force gripper. (if actuated gripper is available)
m_specifications.m_maxGripperAngularDamping = 0.0; // [N*m/(Rad/s)]
//-----------------------------------------------------------------------
// CHARACTERISTICS: (The following are of boolean type: (true or false)
//-----------------------------------------------------------------------
// does your device provide sensed position (x,y,z axis)?
m_specifications.m_sensedPosition = true;
// does your device provide sensed rotations (i.e stylus)?
m_specifications.m_sensedRotation = true;
// does your device provide a gripper which can be sensed?
m_specifications.m_sensedGripper = true;
// is you device actuated on the translation degrees of freedom?
m_specifications.m_actuatedPosition = false;
// is your device actuated on the rotation degrees of freedom?
m_specifications.m_actuatedRotation = false;
// is the gripper of your device actuated?
m_specifications.m_actuatedGripper = false;
// can the device be used with the left hand?
m_specifications.m_leftHand = true;
// can the device be used with the right hand?
m_specifications.m_rightHand = true;
/************************************************************************
STEP 2:
Here, you should implement code which tells the application if your
device is actually connected to your computer and can be accessed.
In practice this may be consist in checking if your I/O board
is active or if your drivers are available.
If your device can be accessed, set:
m_systemAvailable = true;
Otherwise set:
m_systemAvailable = false;
Your actual code may look like:
bool result = checkIfMyDeviceIsAvailable()
m_systemAvailable = result;
*************************************************************************/
// *** INSERT YOUR CODE HERE ***
/* Initiate sixsense */
int r = sixenseInit();
std::cout << "a_deviceNumber " << this->a_deviceNumber
<< ": sixenseInit() returns " << r << std::endl;
sixenseSetHemisphereTrackingMode(0,1);
sixenseSetHemisphereTrackingMode(1,1);
sixenseSetActiveBase(0);
m_deviceAvailable = true; // default value.
}
