/Users/craigcornelius/Projects/SPRING Mac Release 0.2/haptic_v1.cpp

Go to the documentation of this file.

// $Id: haptic_v1.cpp,v 1.23 2006/05/24 16:52:39 sean Exp $
//
//  haptic_v1.cpp
//

#include "haptic_v1.h"
#include "glui.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#ifdef _WIN32
#else
#include <string.h>
#endif

const char* Haptic_v1::rcsid = "@(#) $Id: haptic_v1.cpp,v 1.23 2006/05/24 16:52:39 sean Exp $ $Copyright: (c)2001 National Biocomputation Center, Stanford University $";

// buffer size- well under the 1500 bytes ethernet limit, 
// so will fit in 1 packet
#define BUFSIZE 500

int Haptic_v1::debug = 0;

Haptic_v1::Haptic_v1(char* machine_name, int port_num, bool forcegridmode)
: Sensor(Sensor::haptic_v1_sensor)
{
    if (debug == 1) 
        cerr << "Haptic_v1::constructor(\"" << machine_name << "\", "
        << port_num << ")\n";
    
    // force initialization
    old_force.x = old_force.y = old_force.z =0.0;
    
    // forcegrid initialization
    old_grid[0].x = old_grid[0].y = old_grid[0].z =0.0;
    old_grid[1].x = old_grid[1].y = old_grid[1].z =0.0;

        // misc initialization
    twist_force = grip_force = 0.0;
    old_twist_force = old_grip_force = 0.0;
    force_scalefactor = 1.0;
    active_status = false;
    first_grab = true;
    //  grid_mode = forcegridmode;
        update_iterations = 0;
        force_iterations = 0;
    
    // linked haptics initialization
    haptic_link_mode = link_position;
    linked_device = NULL;
    
    num_active = 2;
    
    // set up socket: (need a separate, non global buffer so each instantiation
    // has it's own messages coming in
    buf = (char *)malloc(sizeof(char)*BUFSIZE);
    char server[80]; // = "maki.stanford.edu";
    strncpy(server, machine_name, 80);
    
    cerr << "making new socket to: " << server << endl;
#ifdef __APPLE__
        int sock_buf_size = 1024;
        hsock = new Socket(server,Socket::tcp,port_num, sock_buf_size);
#else
    hsock = new Socket(server,Socket::tcp,port_num);
#endif // __APPLE__
    cerr << "created socket" << endl;
    
    // Set grid_mode 
    SetGridMode(forcegridmode);
    
    // // send initial handshake
    if (!grid_mode) sprintf(buf, "haptic v1.0");
    else sprintf(buf, "haptic v1.1");
#ifdef BUFFERED
    hsock->BufferedSend(buf,strlen(buf));
#else
    hsock->Send(buf,BUFSIZE);
#endif
    cerr << " SEND HANDSHAKE BUF: [" << buf << "]\n";
    
    // Check to make sure haptic responded with an acknowledgement
    // (should ideally respond with what type of device it is, so we can
    // do device-specific things on this side)
#ifdef BUFFERED
    hsock->BufferedReceive(buf,BUFSIZE);
#else
    hsock->Receive(buf,BUFSIZE);
#endif
    cerr << " RECEIVE HANDSHAKE BUF: [" << buf << "]\n";
    
    // test their response- do they support this version of the protocol?
    if (strncmp(buf,"ACK",3) != 0) {
        // no ACK, so an error has occured and we cannot connect
        cerr << "ERROR - denied connection" << endl;
    } else {
        // success
        cerr << "Connection made" << endl;
    }
}

Haptic_v1::~Haptic_v1()
{
    if (debug == 1) cerr << "Haptic_v1::destructor()\n";
    
    sprintf(buf, "quit");
#ifdef BUFFERED
    hsock->BufferedSend(buf,strlen(buf));
#else
    hsock->Send(buf,BUFSIZE);
#endif
    
    delete hsock;
    free(buf);
    num_active = -1;
}

void Haptic_v1::SetActive(bool status)
{
    active_status = status;
} 

void  Haptic_v1::SetGridMode(bool mode)
{
    if ((grid_mode == false) && (mode == true))
        first_grab = true;
    grid_mode = mode;
}

bool  Haptic_v1::GetGridMode()
{
    return grid_mode;
}

void Haptic_v1::Update()
{
    grid[1] = force;

        // if interested in real haptic update rates
        update_iterations++;
        if (debug == 2) {
          const int granularity = 100;
          if (update_iterations % granularity == 0) {
                  double secs = timer.GetElapsedTime();
              cerr << "Haptic Update Rate = " << update_iterations/secs
                       << " updates per second over " << granularity << " samples\n";
                  cerr << "  Force update rate = " << force_iterations/secs << endl;
              timer.Reset();
                  update_iterations = 0;
          }
        }
    
    // handle linked haptic devices first
    if (linked_device) {
        updateLinked();                 // to calculate the force
        updateStreaming();              // to actually send the values + get new position
    }
    else if (grid_mode)   // handle forcegrid
        updateForcegrid();
    else                                        // is streaming protocol
        updateStreaming();
    
}

// link to another haptic device
void Haptic_v1::updateLinked()
{
    if (!linked_device) return;
    
    switch (haptic_link_mode) {
    case link_position: {       // make a force to their position
        Point3D other_pos = linked_device->pos;
        force = (other_pos - pos) * force_scalefactor;
        break;
                        }
    case link_force: {  // feel their force
        Haptic_v1* other_haptic = (Haptic_v1*)linked_device;
        force = other_haptic->force;
        twist_force = other_haptic->twist_force;
        grip_force = other_haptic->grip_force;
        break;
                     }
    }  // end switch
}

void Haptic_v1::send_force()
{
    // remap the force vector to the axis of the device
    Point3D hforce(force.x,force.y,force.z);
        
    // scale it up
    hforce *= force_scalefactor;
    if (debug == 4) {
        fprintf(stderr,"force=(%.1f,%.1f,%.1f)=%.1f,twist=%.1f,grip=%.1f\n", 
            force.x, force.y, force.z, force.Length(), 
            twist_force, grip_force);
    }
    
    // create the buffer
    int numbytes = sprintf(buf,"%f %f %f %f %f",hforce.x, hforce.y, hforce.z, 
        twist_force, grip_force);
    
    // send the buffer
#ifdef BUFFERED
    hsock->BufferedSend(buf, numbytes);
#else
    hsock->Send(buf, BUFSIZE);
#endif
        
        // stats
        force_iterations++;

        // debugging
    if (debug == 4) {
        printf("Haptic_v1: Force sent: %f\t %f\t %f\t %f \t%f", 
            hforce.x, hforce.y, hforce.z, twist_force, grip_force);
    }
}

void Haptic_v1::receive_info()
{
    //
    // Get the position and rotation matrix
    //
        
    // locals
    Point3D hpos;
    double hm[3][3];
    float hactive[max_active];  // ideally, should support as many as need
        
    // parse the position info
    int num = sscanf(buf,"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
        &hpos.x, &hpos.y, &hpos.z,
        &hm[0][0], &hm[1][0], &hm[2][0], 
        &hm[0][1], &hm[1][1], &hm[2][1], 
        &hm[0][2], &hm[1][2], &hm[2][2], 
        &hactive[0], &hactive[1], &hactive[2], &hactive[3], &hactive[4],
        &hactive[5], &hactive[6], &hactive[7], &hactive[8], &hactive[9],
        &hactive[10], &hactive[11], &hactive[12], &hactive[13], &hactive[14],
        &hactive[15], &hactive[16], &hactive[17], &hactive[18], &hactive[19],
        &hactive[20], &hactive[21], &hactive[22], &hactive[23], &hactive[24]
        );
        
    // debugging
    if (debug == 5) cerr << "\tgot " << num << " args from device\n";
        
    // they map it to be correct: x to right, y up, z towards me
    pos = hpos;    
        
    // and handle the recentering of the world
    pos -= init_pos;
        
    // and scaling
    pos *= scalefactor;
        
    // copy the rotation matrix, too
    {
        for(int i=0; i<3; i++)
            for(int j=0; j<3; j++)
                m.m[i][j] = hm[i][j];
    }
        
    // save whether it's active
    num_active = num-12;    // calculate from number of arguments seen
    if (debug == 5) cerr << "\tmaking " << num_active << " active values\n";
    for (int i=0; i<num_active; i++)
        active[i] = hactive[i];
    
    if (debug == 4)
        cerr << "Haptic_v1: Received BUF: " << buf << endl;
}

void Haptic_v1::updateStreaming()
{
        // Send the force only if it changed
        // Note: we would probably like to send every time to help out 
        //   interpolation schemes, but that would be alot of bandwidth eaten.  
        //   Also, we'd end up having flow-control problems with a slow receiver 
        //   and this would eventually make us block, waiting for them to eat it 
        //   all
        // Note #2: We've moved this below in an attempt to ensure that we don't
        //   overwhelm a slow receiver.  Now we send the force only if we've had
        //   a position update- the idea being that if they can send that fast,
        //   then they can receive that fast, too.
        //   So, the paradigm is that we have a haptic server that can send
        //   positions very fast, a simulation that's a bit slower and, if for
        //   some reason this is reversed, then we'll max out at the senders rate
    //if (this->forceChanged())
                //send_force();
    
    //
    // Get the position and rotation matrix
    //
    
    // get the buffer
    int data_waiting = hsock->InputQueueSize();

        // if nothing to do, bail
        if (data_waiting <= 0) return;
    
    // eat all waiting data to get to last
    while (data_waiting > 0)
    {
#ifdef BUFFERED
        int nb = hsock->BufferedReceive(buf,BUFSIZE);
#else
        int nb = hsock->Receive(buf,BUFSIZE);
#endif
        data_waiting -= nb;
    }
    
    // there's data and we got it- process it
        receive_info();

        // and send the force update (see the note above)
        // (what the heck- send even if hasn't changed)
    //if (this->forceChanged())
                send_force();
}

// The haptic uses a forcegrid, and thus needs a little more info...
void Haptic_v1::updateForcegrid()
{
    // DATA TO SEND
    if (active_status == true) {
        
        Point3D to_send[2];
        
        // grid[0] is the position received from the haptic device 
        // (a priori different fom the position of the tool in spring, 
        // due to scalefactor, offset...
        to_send[0] = grid[0];
        
        // grid[1] is the corresponding force
        // forcefactor: Phantom wants 0.1, LapIE wants 40
        to_send[1] = grid[1] * force_scalefactor;
        
        // create the buffer
        if (first_grab == true) {
            
            first_grab = false;
            int numbytes = sprintf(buf,"build");
            hsock->BufferedSend(buf, numbytes);
        }
        
        else {
            int nb = sprintf(buf,"fill %f %f %f %f %f %f",
                to_send[0].x, to_send[0].y, to_send[0].z,
                to_send[1].x, to_send[1].y, to_send[1].z);
            hsock->BufferedSend(buf, nb);
        }
    }
    else {
        // else, notify the haptics that we are not active here, in spring
        if (first_grab == false) {
            int numbytes = sprintf(buf,"destruct");
            hsock->BufferedSend(buf, numbytes);
            //cerr << "Sending buf[" << buf << "]\n";
        }
        first_grab = true;
    }
    
    // DATA TO RECEIVE
    
    int data_waiting = hsock->InputQueueSize() / BUFSIZE;
    
    if(data_waiting == 0)
        return;
    
    // Necessary pre-flush, to avoid congestion
    while(data_waiting > 0)
    {
        hsock->BufferedReceive(buf,BUFSIZE);
        data_waiting--;
        
        // if it's not a position buffer, don't flush it
        if (strncmp(buf, "pos", 3) != 0)
            break;
    }
    
    // parse the buffer info
    
    // if it's a position buffer
    if (strncmp(buf, "pos", 3) == 0) {
        Point3D hpos;
        double hm[3][3];
        float hactive;
        sscanf(buf + 3," %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %f",
            &hpos.x, &hpos.y, &hpos.z,
            &hm[0][0], &hm[0][1], &hm[0][2],
            &hm[1][0], &hm[1][1], &hm[1][2], 
            &hm[2][0], &hm[2][1], &hm[2][2], &hactive);
        
        grid[0] = hpos;
        
        // then map it to be correct: x to right, y up, z towards me
        pos = hpos;     
        
        // and handle the recentering of the world
        pos -= init_pos;
        
        // and scaling
        pos *= scalefactor;
        
        // copy the rotation matrix, too
        for(int i=0; i<3; i++)
            for(int j=0; j<3; j++)
                m.m[i][j] = hm[i][j];
            
            // save whether it's active
            active[0] = hactive;
    }
    
    else if (strcmp(buf, "stop") == 0)
    {
        // Flush the entire buffer
        while(data_waiting > 0)
        {
            hsock->Receive(buf,BUFSIZE);
            data_waiting--;
        }
        return;
    }
    
    // Anything else is an error buffer -> discard and flush.
    else {
        //cerr << "Irrelevant data received from haptics, Flushing...";
        //cerr << "\nBuffer received: [" << buf << "]";
        data_waiting = ((hsock->InputQueueSize() / BUFSIZE) -1);
        while (data_waiting >= 0) {
            hsock->BufferedReceive(buf, BUFSIZE);
            data_waiting --;
        }
    }
}

int Haptic_v1::forceChanged()
{
    int res;
    
    res = ((force.x != old_force.x) || (force.y != old_force.y) 
        || (force.z != old_force.z) || (twist_force != old_twist_force)
        || (grip_force != old_grip_force));
    
    // update old force
    if (res) { 
        old_force = force; 
        old_twist_force = twist_force; 
        old_grip_force = grip_force; 
    }
    
    // printf("%d", res);
    return res;
}

int Haptic_v1::gridChanged()
{
    int res;
    
    res = ((grid[0].x != old_grid[0].x) || (grid[0].y != old_grid[0].y) 
        || (grid[0].z != old_grid[0].z) || (grid[1].x != old_grid[1].x) 
        || (grid[0].y != old_grid[1].y) || (grid[1].z != old_grid[1].z));
    if(res)
    {
        old_grid[0] = grid[0];
        old_grid[1] = grid[1];
    }
    return res;
}

void Haptic_v1::HapticLink(Sensor* sensor)
{
    linked_device = sensor;
    
    // turn off any leftover forces
    if (linked_device == NULL) 
        force = Point3D(0,0,0);
}

---