17 #if defined(VRPN_USE_LIBNIFALCON) 18 #include "boost/array.hpp" 19 #include "boost/ptr_container/ptr_vector.hpp" 20 #include "boost/shared_ptr.hpp" 21 #include "falcon/core/FalconDevice.h" 22 #include "falcon/firmware/FalconFirmwareNovintSDK.h" 23 #include "falcon/grip/FalconGripFourButton.h" 24 #include "falcon/kinematic/FalconKinematicStamper.h" 25 #include "falcon/util/FalconFirmwareBinaryNvent.h" 29 #define FALCON_NUM_RETRIES 10 40 typedef boost::array<double, 3> d_vector;
43 static d_vector operator+(
const d_vector &a,
const d_vector &b)
53 static d_vector operator-(
const d_vector &a,
const d_vector &b)
63 static double d_length(
const d_vector &a)
74 static double timediff(
struct timeval t1,
struct timeval t2) {
75 return (t1.tv_usec - t2.tv_usec)*1.0 + 1000000.0 * (t1.tv_sec - t2.tv_sec);
80 class vrpn_NovintFalcon_Device
83 vrpn_NovintFalcon_Device(
int flags)
87 m_falconDevice = NULL;
90 m_falconDevice =
new libnifalcon::FalconDevice;
91 m_falconDevice->setFalconFirmware<libnifalcon::FalconFirmwareNovintSDK>();
94 if (m_flags & KINE_STAMPER) {
95 m_falconDevice->setFalconKinematic<libnifalcon::FalconKinematicStamper>();
97 delete m_falconDevice;
102 if (m_flags & GRIP_FOURBUTTON) {
103 m_falconDevice->setFalconGrip<libnifalcon::FalconGripFourButton>();
105 delete m_falconDevice;
111 ~vrpn_NovintFalcon_Device() {
113 fprintf(stderr,
"Closing Falcon device %d.\n", m_flags & MASK_DEVICEIDX);
115 if (m_falconDevice) {
116 m_falconDevice->close();
118 delete m_falconDevice;
129 m_falconDevice->getDeviceCount(count);
130 int devidx = m_flags & MASK_DEVICEIDX;
133 fprintf(stderr,
"Trying to open Falcon device %d/%d.\n", devidx, count);
135 if (devidx < count) {
136 if (!m_falconDevice->open(devidx)) {
137 fprintf(stderr,
"Cannot open falcon device %d - Lib Error Code: %d - Device Error Code: %d\n",
138 devidx, m_falconDevice->getErrorCode(), m_falconDevice->getFalconComm()->getDeviceErrorCode());
142 fprintf(stderr,
"Trying to open non-existing Novint Falcon device %d\n", devidx);
146 if (!m_falconDevice->isFirmwareLoaded()) {
148 fprintf(stderr,
"Loading Falcon Firmware\n");
152 for (i=0; i<FALCON_NUM_RETRIES; ++i) {
153 if(!m_falconDevice->getFalconFirmware()->loadFirmware(
false, libnifalcon::NOVINT_FALCON_NVENT_FIRMWARE_SIZE, const_cast<uint8_t*>(libnifalcon::NOVINT_FALCON_NVENT_FIRMWARE)))
155 fprintf(stderr,
"Firmware loading attempt %d failed.\n", i);
157 m_falconDevice->close();
158 if(!m_falconDevice->open(devidx))
160 fprintf(stderr,
"Cannot open falcon device %d - Lib Error Code: %d - Device Error Code: %d\n",
161 devidx, m_falconDevice->getErrorCode(), m_falconDevice->getFalconComm()->getDeviceErrorCode());
166 fprintf(stderr,
"Falcon firmware successfully loaded.\n");
173 fprintf(stderr,
"Falcon Firmware already loaded.\n");
178 bool message =
false;
179 boost::shared_ptr<libnifalcon::FalconFirmware> f;
180 f=m_falconDevice->getFalconFirmware();
181 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
183 f->setHomingMode(
true);
184 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
186 f->setLEDStatus(libnifalcon::FalconFirmware::RED_LED);
187 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
189 fprintf(stderr,
"Falcon not currently calibrated. Move control all the way out then push straight all the way in.\n");
193 f->setLEDStatus(libnifalcon::FalconFirmware::BLUE_LED);
194 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
196 fprintf(stderr,
"Falcon calibrated successfully.\n");
206 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
207 d_vector pos = m_falconDevice->getPosition();
212 fprintf(stderr,
"Move control all the way out until led turns green to activate device.\n");
215 if (pos[2] > 0.170) {
216 f->setLEDStatus(libnifalcon::FalconFirmware::GREEN_LED);
217 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
219 fprintf(stderr,
"Falcon activated successfully.\n");
228 bool get_status(vrpn_float64 *pos, vrpn_float64 *vel,
229 vrpn_float64 *quat, vrpn_float64 *vel_quat,
230 vrpn_float64 *vel_dt,
unsigned char *buttons) {
235 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
236 if ( i == FALCON_NUM_RETRIES )
244 switch (m_flags & MASK_DEVICEIDX) {
274 d_vector my_pos = m_falconDevice->getPosition();
275 const double convert_pos = 1.0;
276 pos[0] = convert_pos * my_pos[0];
277 pos[1] = convert_pos * my_pos[1];
278 pos[2] = convert_pos * (my_pos[2]-0.125);
280 fprintf(stderr,
"position %5.3f %5.3f %5.3f\n", pos[0],pos[1],pos[2]);
283 struct timeval current_time;
285 double deltat = timediff(current_time, m_oldtime);
288 vel[0] = convert_pos * (my_pos[0] - m_oldpos[0]) / deltat;
289 vel[1] = convert_pos * (my_pos[1] - m_oldpos[1]) / deltat;
290 vel[2] = convert_pos * (my_pos[2] - m_oldpos[2]) / deltat;
297 fprintf(stderr,
"velocity %5.3f %5.3f %5.3f\n", vel[0],vel[1],vel[2]);
300 m_oldtime.tv_sec = current_time.tv_sec;
301 m_oldtime.tv_usec = current_time.tv_usec;
305 unsigned int my_buttons = m_falconDevice->getFalconGrip()->getDigitalInputs();
306 if (m_flags & GRIP_FOURBUTTON) {
307 buttons[0] = (my_buttons & libnifalcon::FalconGripFourButton::CENTER_BUTTON) ? 1 : 0;
308 buttons[1] = (my_buttons & libnifalcon::FalconGripFourButton::PLUS_BUTTON) ? 1 : 0;
309 buttons[2] = (my_buttons & libnifalcon::FalconGripFourButton::MINUS_BUTTON) ? 1 : 0;
310 buttons[3] = (my_buttons & libnifalcon::FalconGripFourButton::FORWARD_BUTTON) ? 1 : 0;
316 bool set_force(
const d_vector &force) {
321 m_falconDevice->setForce(force);
322 if(!m_falconDevice->runIOLoop())
330 libnifalcon::FalconDevice *m_falconDevice;
332 struct timeval m_oldtime;
336 vrpn_NovintFalcon_Device() {};
338 vrpn_NovintFalcon_Device(
const vrpn_NovintFalcon_Device &dev) {};
344 MASK_DEVICEIDX = 0x000f,
345 KINE_STAMPER = 0x0010,
346 GRIP_FOURBUTTON = 0x0100
351 class ForceFieldEffect
355 ForceFieldEffect() : m_active(false), m_time(0), m_cutoff(0.0), m_damping(0.9)
358 for (i=0; i < 3; i++) {
361 for (j=0; j < 3; j++) {
362 m_jacobian[i][j] = 0.0;
368 ~ForceFieldEffect() {}
372 void setForce(vrpn_float32 ori[3], vrpn_float32 frc[3], vrpn_float32 jac[3][3], vrpn_float32 rad) {
374 for (i=0; i < 3; i++) {
375 m_neworig[i] = ori[i];
376 m_newadd[i] = frc[i];
377 for (j=0; j < 3; j++) {
378 m_newjacob[i][j] = jac[i][j];
385 void setDamping(
double damp) {
390 virtual bool start() {
400 virtual void stop() {
405 virtual bool isActive()
const {
return m_active; }
408 d_vector calcForce(
const d_vector &pos) {
409 d_vector force, offset;
413 const double mix = 1.0 - m_damping;
415 for (i=0; i < 3; i++) {
416 m_origin[i] = m_damping*m_origin[i] + mix*m_neworig[i];
417 m_addforce[i] = m_damping*m_addforce[i] + mix*m_newadd[i];
418 for (j=0; j < 3; j++) {
419 m_jacobian[i][j] = m_damping*m_jacobian[i][j] + mix*m_newjacob[i][j];
422 m_cutoff = m_damping*m_cutoff + mix*m_newcut;
424 offset = pos - m_origin;
426 if (d_length(offset) > m_cutoff) {
438 for (i=0; i<3; ++i) {
439 for (j=0; j<3; ++j) {
440 force[i] += offset[j]*m_jacobian[i][j];
453 double m_jacobian[3][3];
457 double m_newjacob[3][3];
464 vrpn_Tracker_NovintFalcon *dev = (vrpn_Tracker_NovintFalcon *)userdata;
465 dev->update_forcefield_effect(p);
470 class vrpn_NovintFalcon_ForceObjects {
472 boost::ptr_vector<ForceFieldEffect> m_FFEffects;
481 vrpn_NovintFalcon_ForceObjects() {
482 m_curforce.assign(0.0);
483 m_curpos.assign(0.0);
486 ~vrpn_NovintFalcon_ForceObjects() {};
489 d_vector getObjForce(vrpn_float64 *pos, vrpn_float64 *vel) {
492 for (i=0; i<3; ++i) {
499 int nobj = m_FFEffects.size();
500 for (i=0; i<nobj; ++i) {
501 m_curforce = m_curforce + m_FFEffects[i].calcForce (m_curpos);
508 vrpn_Tracker_NovintFalcon::vrpn_Tracker_NovintFalcon(
const char *name,
515 vrpn_ForceDevice(name, c), m_dev(NULL), m_obj(NULL), m_update_rate(1000.0), m_damp(0.9)
517 m_devflags=vrpn_NovintFalcon_Device::MASK_DEVICEIDX & devidx;
519 if (0 == strcmp(grip,
"4-button")) {
520 m_devflags |= vrpn_NovintFalcon_Device::GRIP_FOURBUTTON;
523 fprintf(stderr,
"WARNING: Unknown grip for Novint Falcon #%d: %s \n", devidx, grip);
530 if (0 == strcmp(kine,
"stamper")) {
531 m_devflags |= vrpn_NovintFalcon_Device::KINE_STAMPER;
533 fprintf(stderr,
"WARNING: Unknown kinematic model for Novint Falcon #%d: %s \n", devidx, kine);
540 vrpn_float64 val= atof(damp);
541 if (val >= 1.0 && val <= 10000.0) {
542 m_damp = 1.0 - 1.0/val;
544 fprintf(stderr,
"WARNING: Ignoring illegal force effect damping factor: %g \n", val);
551 fprintf(stderr,
"vrpn_Tracker_NovintFalcon:can't register force handler\n");
558 void vrpn_Tracker_NovintFalcon::clear_values()
561 if (m_devflags < 0)
return;
567 if (m_obj)
delete m_obj;
568 m_obj =
new vrpn_NovintFalcon_ForceObjects;
571 ForceFieldEffect *ffe =
new ForceFieldEffect;
572 ffe->setDamping(m_damp);
574 m_obj->m_FFEffects.push_back(ffe);
577 vrpn_Tracker_NovintFalcon::~vrpn_Tracker_NovintFalcon()
585 void vrpn_Tracker_NovintFalcon::reset()
589 if (m_devflags < 0)
return;
594 fprintf(stderr,
"Resetting the NovintFalcon #%d\n",
595 vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags);
600 m_dev =
new vrpn_NovintFalcon_Device(m_devflags);
603 fprintf(stderr,
"Device constructor failed!\n");
609 if (!m_dev->init()) {
611 fprintf(stderr,
"Device init failed!\n");
617 fprintf(stderr,
"Reset Completed.\n");
621 int vrpn_Tracker_NovintFalcon::get_report(
void)
627 if (m_dev->get_status(pos, vel, d_quat, vel_quat, &vel_quat_dt, buttons)) {
631 for (i=0; i < num_buttons; i++)
634 if (j == num_buttons) {
646 print_latest_report();
652 void vrpn_Tracker_NovintFalcon::send_report(
void)
657 if (d_connection->pack_message(len, m_timestamp, position_m_id, d_sender_id, msgbuf,
661 if (d_connection->pack_message(len, m_timestamp, velocity_m_id, d_sender_id, msgbuf,
667 void vrpn_Tracker_NovintFalcon::handle_forces(
void)
673 d_vector force= m_obj->getObjForce(pos,vel);
674 m_dev->set_force(force);
680 if (!m_obj)
return 1;
682 vrpn_float32 center[3];
683 vrpn_float32 force[3];
684 vrpn_float32 jacobian[3][3];
690 m_obj->m_FFEffects[0].start();
691 m_obj->m_FFEffects[0].setForce(center, force, jacobian, radius);
696 void vrpn_Tracker_NovintFalcon::mainloop()
698 struct timeval current_time;
703 if ( timediff(current_time, m_timestamp) >= 1000000.0/m_update_rate) {
706 m_timestamp.tv_sec = current_time.tv_sec;
707 m_timestamp.tv_usec = current_time.tv_usec;
724 fprintf(stderr,
"NovintFalcon #%d failed, trying to reset (Try power cycle if more than 4 attempts made)\n",
725 vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags);
730 fprintf(stderr,
"NovintFalcon #%d , unknown status message: %d)\n",
731 vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags, status);
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Generic connection class not specific to the transport mechanism.
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_PARTIAL
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
This structure is what is passed to a vrpn_Connection message callback.
const int vrpn_TRACKER_SYNCING
virtual int encode_to(char *buf)
const int vrpn_TRACKER_AWAITING_STATION
#define vrpn_gettimeofday
vrpn_int32 d_sender_id
Sender ID registered with the connection.
virtual int encode_vel_to(char *buf)