When an application opens a force device, such as the PHANToM, it also wants access to the position reports from the device and also any buttons that are on the device. The application gets access to these by opening the device with the same name as a vrpn_Tracker_Remote, a vrpn_Button_Remote, and a vrpn_ForceDevice_Remote. VRPN takes care of mapping all of these devices to use the same connection to communicate over, and the application can treat the three separate functions of the device independently.
The vrpn_ForceDevice_Remote is the force-update part of a force-feedback device. It specifies objects in the space of the force device that the end effector(s) will bump against.
#define MAXPLANE 4 //maximum number of plane in the scene
class vrpn_ForceDevice {
public:
void setSurfaceKspring(float k) {SurfaceKspring = k; }
void setSurfaceKdamping(float d) {SurfaceKdamping =d;}
void setSurfaceFstatic(float ks) {SurfaceFstatic = ks;}
void setSurfaceFdynamic(float kd) {SurfaceFdynamic =kd;}
void setFF_Origin(float x, float y, float z);
void setFF_Force(float fx, float fy, float fz); // dynes
void setFF_Jacobian(float dfxdx, float dfxdy, float dfxdz, // dynes/meter
float dfydx, float dfydy, float dfydz,
float dfzdx, float dfzdy, float dfzdz);
void setFF_Radius(float r);
};
typedef struct {
struct timeval msg_time; // Time of the report
float force[3]; // force value
} vrpn_FORCECB;
typedef void (*vrpn_FORCECHANGEHANDLER)(void *userdata,
const vrpn_FORCECB info);
typedef struct {
struct timeval msg_time; // Time of the report
double pos[3]; // position of SCP
double quat[4]; // orientation of SCP
} vrpn_FORCESCPCB;
typedef void (*vrpn_FORCESCPHANDLER) (void *userdata,
const vrpn_FORCESCPCB info);
class vrpn_ForceDevice_Remote: public vrpn_ForceDevice {
public:
// The name of the force device to connect to
vrpn_ForceDevice_Remote(char *name);
void set_plane(float *p);
void set_plane(float *p, float d);
void set_plane(float a, float b, float c,float d);
void sendSurface(void);
void startSurface(void);
void stopSurface(void);
// vertNum normNum and triNum start at 0
void setVertex(int vertNum,float x,float y,float z);
// NOTE: ghost dosen't take normals,
// and normals still aren't implemented for Hcollide
void setNormal(int normNum,float x,float y,float z);
void setTriangle(int triNum,int vert0,int vert1,int vert2,
int norm0=-1,int norm1=-1,int norm2=-1);
void removeTriangle(int triNum);
// should be called to incorporate the above changes into the displayed trimesh
void updateTrimeshChanges();
// set the trimesh's homogen transform matrix (in row major order)
void setTrimeshTransform(float homMatrix[16]);
void clearTrimesh(void);
// the next time we send a trimesh we will use the following type
void useHcollide();
void useGhost();
void sendConstraint(int enable, float x, float y, float z, float kSpr);
void sendForceField(float origin[3], float force[3],
float jacobian[3][3], float radius);
void sendForceField(void);
void stopForceField();
// This routine calls the mainloop of the connection it's own
virtual void mainloop(void);
// (un)Register a callback handler to handle a force change
// and plane equation change and trimesh change
virtual int register_change_handler(void *userdata,
vrpn_FORCECHANGEHANDLER handler);
virtual int unregister_change_handler(void *userdata,
vrpn_FORCECHANGEHANDLER handler);
virtual int register_scp_change_handler(void *userdata,
vrpn_FORCESCPHANDLER handler);
virtual int unregister_scp_change_handler(void *userdata,
vrpn_FORCESCPHANDLER handler);
virtual int register_error_handler(void *userdata,
vrpn_FORCEERRORHANDLER handler);
virtual int unregister_error_handler(void *userdata,
vrpn_FORCEERRORHANDLER handler);
};
A vrpn_ForceDevice_Remote object is used by passing the name of the force device you want to connect to as a parameter to the constructor, defining a callback handler that will respond to the force device state update messages, and then calling its mainloop() message each time through the program's main loop. Often, a callback handler is not required because you usually just want to tell a force device what forces to apply and don't need to hear back from the force device about what forces it is applying. While active, the force device is controlled by setting the current plane, trimesh, or force field.
The plane is sent using sendSurface(). The force device determines what forces need to be applied to simulate the plane. startSurface() and stopSurface() are used to activate or deactivate the force device.
You can send a triangle mesh by calls to setVertex() and setTriangle().Similarly you can modify the mesh with calls to setTriangle(), setVertex(), and removeTriangle(). Once you have finished a round of adjustments and are ready for them to take effect, make a call to updateTrimeshChanges(). clearTrimesh() behaves like stopSurface() for a plane. After a trimesh has been sent you can set its homogenous tranformation matrix with a call to setTrimeshTransform.
Several functions inherited from vrpn_ForceDevice are used to set various parameters used to simulate the plane surface such as the spring constant and friction. If you register a handler using register_scp_change_handler you can receive messages giving the location of the surface contact point (SCP) being used in the force model. While the hand tracker associated with force device may report that the hand position is actually below a virtual surface, the surface contact point will always stay on or above the virtual surface. It may be preferable to use the SCP rather than the hand position for graphical display of the hand position as doing so can have a significant effect on the perception of surface compliance.
A force field may be specified as a local approximation, analogous to a plane for surfaces. You must specify the position at which the approximation is valid, the force at that position and the jacobian matrix giving the derivatives of the force field in each direction. You must also specify a radius beyond which the approximation should not be used (forces will automatically go to zero). This is a safety feature. In general, you will want to use the latest position reported by the tracker for the approximation point. To update the force field you should call sendForceField() and to set the force field to 0 you can call stopForceField().
The following is an example that sets up the force device to simulate a plane defined by the equation, Y = 0.
#include
<stdlib.h> #include <stdio.h> #include
<vrpn_ForceDevice.h> #include <vrpn_Tracker.h> #include
<vrpn_Button.h>
#define PHANTOM_SERVER "Phantom@phantom3"
/*****************************************************************************
*
Callback handler
*
*****************************************************************************/
void handle_force_change(void *userdata, vrpn_FORCECB f)
{
static vrpn_FORCECB lr; // last report
static int first_report_done = 0;
if ((!first_report_done) ||
((f.force[0] != lr.force[0]) || (f.force[1] != lr.force[1])
|| (f.force[2] != lr.force[2])))
printf("force is (%f,%f,%f)\n", f.force[0], f.force[1], f.force[2]);
first_report_done = 1;
lr = f;
}
void handle_tracker_change(void *userdata, const vrpn_TRACKERCB t)
{
static vrpn_TRACKERCB lr; // last report
static float dist_interval_sq = 0.004;
if ((lr.pos[0] - t.pos[0])*(lr.pos[0] - t.pos[0]) +
(lr.pos[1] - t.pos[1])*(lr.pos[1] - t.pos[1]) +
(lr.pos[2] - t.pos[2])*(lr.pos[2] - t.pos[2]) > dist_interval_sq){
printf("Sensor %d is now at (%g,%g,%g)\n", t.sensor,
t.pos[0], t.pos[1], t.pos[2]);
lr = t;
}
}
void handle_button_change(void *userdata, vrpn_BUTTONCB b)
{
static int count=0;
static int buttonstate = 1;
int done = 0;
if (b.state != buttonstate) {
printf("button #%ld is in state %ld\n", b.button, b.state);
buttonstate = b.state;
count++;
}
if (count > 4)
done = 1;
*(int *)userdata = done;
}
int main(int argc, char *argv[])
{
int done = 0;
vrpn_ForceDevice_Remote *forceDevice;
vrpn_Tracker_Remote *tracker;
vrpn_Button_Remote *button;
/* initialize the force device */
forceDevice = new vrpn_ForceDevice_Remote(PHANTOM_SERVER);
forceDevice->register_change_handler(NULL, handle_force_change);
/* initialize the tracker */
tracker = new vrpn_Tracker_Remote(PHANTOM_SERVER);
tracker->register_change_handler(NULL, handle_tracker_change);
/* initialize the button */
button = new vrpn_Button_Remote(PHANTOM_SERVER);
button->register_change_handler(&done, handle_button_change);
// Set plane and surface parameters
forceDevice->set_plane(0.0, 1.0, 0.0, 0.0);
forceDevice->setSurfaceKspring(0.8); // spring constant - units of
// dynes/cm
forceDevice->setSurfaceKdamping(0.1); // damping constant -
// units of dynes*sec/cm
forceDevice->setSurfaceFstatic(0.7); // set static friction
forceDevice->setSurfaceFdynamic(0.3); // set dynamic friction
forceDevice->setRecoveryTime(10); // recovery occurs over 10
// force update cycles
// enable force device and send first surface
forceDevice->startSurface();
// main loop
while (! done ){
// Let the forceDevice send its planes to remote force device
forceDevice->mainloop();
// Let tracker receive position information from remote tracker
tracker->mainloop();
// Let button receive button status from remote button
button->mainloop();
// we may call forceDevice->set_plane(...) followed by
// forceDevice->sendSurface() here to change the plane
// for example: using position information from a tracker we can
// compute a plane to approximate a complex surface at that
// position and send that approximation 15-30 times per
// second to simulate the complex surface
}
// shut off force device
forceDevice->stopSurface();
} /* main */
------------------------------------------------------------------------------------
Here is an example that displays a tetrahedron with a 10 cm radius,
manipulates its mesh a little and transforms it:
#include <stdlib.h>
#include <stdio.h>
#include <vrpn_ForceDevice.h>
#include <vrpn_Tracker.h>
#include <vrpn_Button.h>
#define PHANTOM_SERVER "Phantom@phantom3"
/*****************************************************************************
*
Callback handler
*
*****************************************************************************/
void handle_force_change(void *userdata,const vrpn_FORCECB f)
{
static vrpn_FORCECB lr; // last report
static int first_report_done = 0;
if ((!first_report_done) ||
((f.force[0] != lr.force[0]) || (f.force[1] != lr.force[1])
|| (f.force[2] != lr.force[2])))
printf("force is (%f,%f,%f)\n", f.force[0], f.force[1], f.force[2]);
first_report_done = 1;
lr = f;
}
void handle_tracker_change(void *userdata, const vrpn_TRACKERCB t)
{
static vrpn_TRACKERCB lr; // last report
static float dist_interval_sq = 0.004;
if ((lr.pos[0] - t.pos[0])*(lr.pos[0] - t.pos[0]) +
(lr.pos[1] - t.pos[1])*(lr.pos[1] - t.pos[1]) +
(lr.pos[2] - t.pos[2])*(lr.pos[2] - t.pos[2]) > dist_interval_sq){
printf("Sensor %d is now at (%g,%g,%g)\n", t.sensor,
t.pos[0], t.pos[1], t.pos[2]);
lr = t;
}
}
void handle_button_change(void *userdata,const vrpn_BUTTONCB b)
{
static int count=0;
static int buttonstate = 1;
int done = 0;
if (b.state != buttonstate) {
printf("button #%ld is in state %ld\n", b.button, b.state);
buttonstate = b.state;
count++;
}
if (count > 4)
done = 1;
*(int *)userdata = done;
}
int main(int argc, char *argv[])
{
int done = 0;
vrpn_ForceDevice_Remote *forceDevice;
vrpn_Tracker_Remote *tracker;
vrpn_Button_Remote *button;
/* initialize the force device */
forceDevice = new vrpn_ForceDevice_Remote(PHANTOM_SERVER);
forceDevice->register_change_handler(NULL, handle_force_change);
/* initialize the tracker */
tracker = new vrpn_Tracker_Remote(PHANTOM_SERVER);
tracker->register_change_handler(NULL, handle_tracker_change);
/* initialize the button */
button = new vrpn_Button_Remote(PHANTOM_SERVER);
button->register_change_handler(&done, handle_button_change);
// before we make any triangle mesh setup calls we can choose which library
// to use, the default is ghost
//forceDevice->useGhost();
forceDevice->useHcollide();
// units are in meters
forceDevice->setVertex(0,0.0,0.1,0.0);
forceDevice->setVertex(1,0.1,-0.1,0.1);
forceDevice->setVertex(2,-0.1,-0.1,0.1);
forceDevice->setVertex(3,0.0,-0.1,-0.1);
forceDevice->setTriangle(0,0,2,1);
forceDevice->setTriangle(1,0,1,3);
forceDevice->setTriangle(2,0,3,2);
forceDevice->setTriangle(3,2,3,1);
forceDevice->setSurfaceKspring(0.8); // spring constant - units of
// dynes/cm
forceDevice->setSurfaceKdamping(0.001); // damping constant -
// units of dynes*sec/cm
forceDevice->setSurfaceFstatic(0.7); // set static friction
forceDevice->setSurfaceFdynamic(0.3); // set dynamic friction
// enable the trimesh and start displaying it
// (this function should be called to enable each set of changes
forceDevice->updateTrimeshChanges();
// ---just for fun lets call some of the mesh manipulation functions
// ---- (without really changing the tetrahedron much)
// setting the vertex automatically adjusts all its adjacent triangles
forceDevice->setVertex(0,0.0,0.11,0.0);
forceDevice->removeTriangle(3);
forceDevice->setTriangle(3,2,3,1);
forceDevice->updateTrimeshChanges();
// if you just set the triangle,
// it is effectively the same as removing it and then setting it
forceDevice->setTriangle(3,2,3,1);
forceDevice->updateTrimeshChanges();
// -------------------------------------------------------------------
// now move the trimesh a centermeter along each axis
/* setTrimeshTransform takes an array of 16 floates that describes
a homogenous transformation matrix in row major order,
NOTE that this resets the transform for the trimesh, instead
of multiplying it */
float xFormMatrix[16] = {1.0,0,0,.01,
0,1.0,0,.01,
0,0,1.0,-.01,
0,0,0.0,1.0};
forceDevice->setTrimeshTransform(xFormMatrix);
// main loop
while (! done ){
// Let the forceDevice send its data to remote force device
forceDevice->mainloop();
// Let tracker receive position information from remote tracker
tracker->mainloop();
// Let button receive button status from remote button
button->mainloop();
}
// turn off the haptic display of the trimesh
forceDevice->clearTrimesh();
} /* main */
----------------------------------------------------------------------------------------
Here is an example that makes a nice 3D sinusoidal force field:
(note: As a safety feature, forces are only enabled (by the client) when
the button is pressed)
#include <stdlib.h>
#include <stdio.h>
#include <vrpn_ForceDevice.h>
#include <vrpn_Tracker.h>
#include <vrpn_Button.h>
#define PHANTOM_SERVER "Phantom@phantom3"
/*****************************************************************************
*
Callback handler
*
*****************************************************************************/
void handle_force_change(void *userdata, const vrpn_FORCECB f)
{
static vrpn_FORCECB lr; // last report
static int first_report_done = 0;
if ((!first_report_done) ||
((f.force[0] != lr.force[0]) || (f.force[1] != lr.force[1])
|| (f.force[2] != lr.force[2]))) {
//printf("force is (%f,%f,%f)\n", f.force[0], f.force[1], f.force[2]);
}
first_report_done = 1;
lr = f;
}
void handle_tracker_change(void *userdata, const vrpn_TRACKERCB t)
{
static vrpn_TRACKERCB lr; // last report
static float dist_interval_sq = 0.004;
float *pos = (float *)userdata;
if ((lr.pos[0] - t.pos[0])*(lr.pos[0] - t.pos[0]) +
(lr.pos[1] - t.pos[1])*(lr.pos[1] - t.pos[1]) +
(lr.pos[2] - t.pos[2])*(lr.pos[2] - t.pos[2]) dist_interval_sq){
//printf("Sensor %d is now at (%g,%g,%g)\n", t.sensor,
// t.pos[0], t.pos[1], t.pos[2]);
lr = t;
}
pos[0] = t.pos[0];
pos[1] = t.pos[1];
pos[2] = t.pos[2];
}
void handle_button_change(void *userdata, const vrpn_BUTTONCB b)
{
static int buttonstate = 1;
if (b.state != buttonstate) {
printf("button #%ld is in state %ld\n", b.button, b.state);
buttonstate = b.state;
}
*(int *)userdata = buttonstate;
}
int main(int argc, char *argv[])
{
int done = 0;
float pos[3];
int forceInEffect = 0;
int forceEnabled = 0;
vrpn_ForceDevice_Remote *forceDevice;
vrpn_Tracker_Remote *tracker;
vrpn_Button_Remote *button;
/* initialize the force device */
forceDevice = new vrpn_ForceDevice_Remote(PHANTOM_SERVER);
forceDevice-register_change_handler(NULL, handle_force_change);
/* initialize the tracker */
tracker = new vrpn_Tracker_Remote(PHANTOM_SERVER);
tracker-register_change_handler((void *)pos, handle_tracker_change);
/* initialize the button */
button = new vrpn_Button_Remote(PHANTOM_SERVER);
button-register_change_handler((void *)&forceEnabled, handle_button_change);
// main loop
while (! done )
{
// Let the forceDevice send its messages to remote force device
forceDevice-mainloop();
// Let tracker receive position information from remote tracker
tracker-mainloop();
// Let button receive button status from remote button
button-mainloop();
if (forceEnabled) {
forceDevice-setFF_Origin(pos[0],pos[1],pos[2]);
// units = dynes
forceDevice-setFF_Force(cos(pos[0]*20.0*M_PI),
cos(pos[1]*20.0*M_PI),cos(pos[2]*20.0*M_PI));
// set derivatives of force field:
// units = dynes/meter
forceDevice-setFF_Jacobian(-20.0*M_PI*sin(pos[0]*20.0*M_PI), 0, 0,
0, -20.0*M_PI*sin(pos[1]*20.0*M_PI), 0,
0, 0, -20.0*M_PI*sin(pos[2]*20.0*M_PI));
forceDevice-setFF_Radius(0.02); // 2cm radius of validity
forceDevice-sendForceField();
forceInEffect = 1;
}
else if (forceInEffect){
forceDevice-stopForceField();
forceInEffect = 0;
}
}
} /* main */
The vrpn_ForceDevice_Remote class provides information on changes in the force it is applying by calling a user callback function. The application gives the function that should handle the data, and also a pointer to a block of user data that the callback routine needs in order to handle the data. If the callback handler was going to affect some object, a pointer to the object would be passed. In this case, the handler function would typecast the 'void *userdata' pointer to the correct type for the user data. If more than one data item is to be passed, they should be placed into a structure and a pointer to the structure passed as user data. For example, the force applied might be mapped to a vector displayed on the screen. In this case we would want to use the userdata pointer to pass the data that needs to be changed in order to change the appearance of the vector.
There is an overloaded member function of the vrpn_ForceDevice_Remote class that is used to set the coefficients of the plane equation describing the plane (Ax + By + Cz + D = 0); the function is set_plane().
There are three member functions from the base vrpn_ForceDevice class that are also used to set surface parameters; they set the plane & trimesh: stiffness setSurfaceKspring(), damping setSurfaceKdamping(), static friction setSurfaceFstatic() and kinetic friction setSurfaceFdynamic().
There are also functions in the base class for setting the force field parameters; they set the approximation point setFF_Origin(), the force at that point setFF_Force(), the jacobian of the force field setFF_Jacobian() and the bounding radius setFF_Radius().