P_VRPNFeedback.C

00001 /***************************************************************************
00002 *cr 
00003 *cr (C) Copyright 1995-2019 The Board of Trustees of the 
00004 *cr University of Illinois 
00005 *cr All Rights Reserved 
00006 *cr 
00007 ***************************************************************************/
00008 
00009 /***************************************************************************
00010 * RCS INFORMATION:
00011 *
00012 * $RCSfile: P_VRPNFeedback.C,v $
00013 * $Author: johns $ $Locker: $ $State: Exp $
00014 * $Revision: 1.35 $ $Date: 2019年01月17日 21:21:01 $
00015 *
00016 ***************************************************************************
00017 * DESCRIPTION:
00018 * This is Paul's new Tracker code -- pgrayson@ks.uiuc.edu
00019 *
00020 *
00021 ***************************************************************************/
00022 
00023 #if defined(VMDVRPN)
00024 
00025 #include "P_VRPNFeedback.h"
00026 #include "utilities.h"
00027 
00028 #include <stdlib.h>
00029 #include <string.h>
00030 #include <math.h> // for sqrt()
00031 
00032 VRPNFeedback::VRPNFeedback() {
00033 fdv = NULL;
00034 }
00035 
00036 int VRPNFeedback::do_start(const SensorConfig *config) {
00037 if (fdv) return 0;
00038 if (!config->have_one_sensor()) return 0;
00039 char myUSL[100];
00040 config->make_vrpn_address(myUSL);
00041 fdv = new vrpn_ForceDevice_Remote(myUSL);
00042 set_maxforce(config->getmaxforce());
00043 
00044 // 
00045 // XXX we wish this worked, but apparently not implemented.
00046 // Have haptic device interpolate the force field between 
00047 // updates. This would help eliminate jerky updates.
00048 // fdv->setRecoveryTime(10); // make haptic constraint update smoother,
00049 // // by stepping N times between points...
00050 forceoff();
00051 return 1;
00052 }
00053 
00054 VRPNFeedback::~VRPNFeedback() {
00055 forceoff();
00056 // if(fdv) delete fdv; // XXX VRPN has broken destructors
00057 }
00058 
00059 void VRPNFeedback::update() {
00060 if(!alive()) return;
00061 // fdv->mainloop();
00062 }
00063 
00064 void VRPNFeedback::addconstraint(float k, const float *location) {
00065 int i;
00066 for(i=0;i<3;i++) {
00067 // the jacobian is k*I
00068 jac[i][i] -= k;
00069 
00070 // there is a force due to offset from the origin
00071 F[i] += k*location[i];
00072 }
00073 }
00074 
00075 void VRPNFeedback::addforcefield(const float *origin, const float *force,
00076 const float *jacobian) {
00077 int i;
00078 for(i=0;i<3;i++) {
00079 F[i] += force[i];
00080 
00081 // force due to the offset
00082 F[i] -= origin[0] * jacobian[0+3*i]
00083 + origin[1] * jacobian[1+3*i]
00084 + origin[2] * jacobian[2+3*i];
00085 }
00086 
00087 for(i=0;i<9;i++) jac[i/3][i%3] += jacobian[i];
00088 
00089 }
00090 
00091 void VRPNFeedback::sendforce(const float *initial_pos) {
00092 float disp[3],Finitial[3],Fmag;
00093 float origin[3]={0,0,0};
00094 
00095 if (!fdv) return;
00096 vec_sub(disp,initial_pos,origin);
00097 Finitial[0] = dot_prod(jac[0],disp);
00098 Finitial[1] = dot_prod(jac[1],disp);
00099 Finitial[2] = dot_prod(jac[2],disp);
00100 vec_add(Finitial,F,Finitial);
00101 
00102 Fmag=norm(Finitial);
00103 if(Fmag>=maxforce && maxforce>=0) { // send a constant force
00104 float newjac[3][3];
00105 float newFinitial[3];
00106 int i, j;
00107 
00108 vec_scale(newFinitial,maxforce/Fmag,Finitial); // scale the force
00109 for(i=0; i<3; i++) { // now fix up the jacobian
00110 for(j=0; j<3; j++) {
00111 float FJj = Finitial[0]*jac[0][j] +
00112 Finitial[1]*jac[1][j] +
00113 Finitial[2]*jac[2][j];
00114 newjac[i][j] = jac[i][j] - Finitial[i]*FJj/(Fmag*Fmag);
00115 }
00116 }
00117 for(i=0; i<3; i++) { // scale the jacobian
00118 vec_scale(newjac[i],maxforce/Fmag,newjac[i]);
00119 }
00120 
00121 vec_copy(origin,initial_pos); // use the point as an origin
00122 fdv->sendForceField(origin, newFinitial, newjac, 1000); // send the force
00123 }
00124 else {
00125 // send the requested force
00126 fdv->sendForceField(origin, F, jac, 1000);
00127 }
00128 }
00129 
00130 inline float sqr(float x) {
00131 return x*x;
00132 }
00133 
00134 void VRPNFeedback::addplaneconstraint(float k, const float *point,
00135 const float *thenormal) {
00136 int i,j;
00137 float jacobian[9], normal[3], force[3]={0,0,0}, len=0;
00138 
00139 for(i=0;i<3;i++) len += sqr(thenormal[i]); // normalize the normal
00140 len = sqrtf(len);
00141 for(i=0;i<3;i++) normal[i] = thenormal[i]/len;
00142 
00143 /* the part of a vector V that is normal to the plane is given by
00144 N*(V.N) (when V is a coordinate vector this is very simple)
00145 And (dFx/dx, dFy/dx, dFz/dx) = N*(i.N) for a plane! */
00146 for(i=0;i<3;i++) { // V = ith basis vector
00147 for(j=0;j<3;j++) // jth component of N
00148 jacobian[i+j*3] = -k * normal[j] * normal[i];
00149 }
00150 addforcefield(point,force,jacobian);
00151 }
00152 
00153 inline void VRPNFeedback::zeroforce() {
00154 int i,j;
00155 for(i=0;i<3;i++) {
00156 F[i]=0;
00157 for(j=0;j<3;j++) jac[i][j]=0;
00158 }
00159 }
00160 
00161 inline void VRPNFeedback::forceoff() {
00162 zeroforce();
00163 if(fdv) fdv->stopForceField();
00164 }
00165 
00166 #endif
00167