17#ifdef VRPN_USE_LIBNIFALCON
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
40typedef std::array<double, 3> d_vector;
43static d_vector operator+(
const d_vector &a,
const d_vector &b)
53static d_vector operator-(
const d_vector &a,
const d_vector &b)
63static double d_length(
const d_vector &a)
74static 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);
80class 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>();
98 delete m_falconDevice;
100 fprintf(stderr,
"vrpn_NovintFalcon_Device::vrpn_NovintFalcon_Device(): delete failed\n");
107 if (m_flags & GRIP_FOURBUTTON) {
108 m_falconDevice->setFalconGrip<libnifalcon::FalconGripFourButton>();
111 delete m_falconDevice;
113 fprintf(stderr,
"vrpn_NovintFalcon_Device::vrpn_NovintFalcon_Device(): delete failed\n");
121 ~vrpn_NovintFalcon_Device() {
123 fprintf(stderr,
"Closing Falcon device %d.\n", m_flags & MASK_DEVICEIDX);
125 if (m_falconDevice) {
126 std::shared_ptr<libnifalcon::FalconFirmware> f;
127 f=m_falconDevice->getFalconFirmware();
129 f->setLEDStatus(libnifalcon::FalconFirmware::RED_LED |
130 libnifalcon::FalconFirmware::BLUE_LED |
131 libnifalcon::FalconFirmware::GREEN_LED);
132 for (
int i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
134 m_falconDevice->close();
137 delete m_falconDevice;
139 fprintf(stderr,
"vrpn_NovintFalcon_Device::~vrpn_NovintFalcon_Device(): delete failed\n");
152 m_falconDevice->getDeviceCount(count);
153 int devidx = m_flags & MASK_DEVICEIDX;
156 fprintf(stderr,
"Trying to open Falcon device %d/%d.\n", devidx, count);
158 if (devidx < count) {
159 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());
165 fprintf(stderr,
"Trying to open non-existing Novint Falcon device %d\n", devidx);
169 if (!m_falconDevice->isFirmwareLoaded()) {
171 fprintf(stderr,
"Loading Falcon Firmware\n");
175 for (i=0; i<FALCON_NUM_RETRIES; ++i) {
176 if(!m_falconDevice->getFalconFirmware()->loadFirmware(
true, libnifalcon::NOVINT_FALCON_NVENT_FIRMWARE_SIZE,
const_cast<uint8_t*
>(libnifalcon::NOVINT_FALCON_NVENT_FIRMWARE)))
178 fprintf(stderr,
"Firmware loading attempt %d failed.\n", i);
179 if(i==FALCON_NUM_RETRIES-1){
180 fprintf(stderr,
"Cannot load falcon device %d firmware - Device Error Code: %d - Comm Lib Error Code: %d\n",
181 devidx, m_falconDevice->getErrorCode(), m_falconDevice->getFalconComm()->getDeviceErrorCode());
186 fprintf(stderr,
"Falcon firmware successfully loaded.\n");
194 fprintf(stderr,
"Falcon Firmware already loaded.\n");
199 bool message =
false;
200 std::shared_ptr<libnifalcon::FalconFirmware> f;
201 f=m_falconDevice->getFalconFirmware();
202 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
204 f->setHomingMode(
true);
205 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
207 f->setLEDStatus(libnifalcon::FalconFirmware::RED_LED);
208 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
210 fprintf(stderr,
"Falcon not currently calibrated. Move control all the way out then push straight all the way in.\n");
214 f->setLEDStatus(libnifalcon::FalconFirmware::BLUE_LED);
215 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
217 fprintf(stderr,
"Falcon calibrated successfully.\n");
227 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
228 d_vector pos = m_falconDevice->getPosition();
233 fprintf(stderr,
"Move control all the way out until led turns green to activate device.\n");
236 if (pos[2] > 0.170) {
237 f->setLEDStatus(libnifalcon::FalconFirmware::GREEN_LED);
238 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
240 fprintf(stderr,
"Falcon activated successfully.\n");
249 bool get_status(vrpn_float64 *pos, vrpn_float64 *vel,
250 vrpn_float64 *quat, vrpn_float64 *vel_quat,
251 vrpn_float64 *vel_dt,
unsigned char *buttons) {
256 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i)
continue;
257 if ( i == FALCON_NUM_RETRIES )
277 d_vector my_pos = m_falconDevice->getPosition();
278 const double convert_pos = 1.0;
279 pos[0] = convert_pos * my_pos[0];
280 pos[1] = convert_pos * my_pos[1];
281 pos[2] = convert_pos * (my_pos[2]-0.125);
283 fprintf(stderr,
"position %5.3f %5.3f %5.3f\n", pos[0],pos[1],pos[2]);
286 struct timeval current_time;
288 double deltat = timediff(current_time, m_oldtime);
291 vel[0] = convert_pos * (my_pos[0] - m_oldpos[0]) / deltat;
292 vel[1] = convert_pos * (my_pos[1] - m_oldpos[1]) / deltat;
293 vel[2] = convert_pos * (my_pos[2] - m_oldpos[2]) / deltat;
300 fprintf(stderr,
"velocity %5.3f %5.3f %5.3f\n", vel[0],vel[1],vel[2]);
303 m_oldtime.tv_sec = current_time.tv_sec;
304 m_oldtime.tv_usec = current_time.tv_usec;
308 unsigned int my_buttons = m_falconDevice->getFalconGrip()->getDigitalInputs();
309 if (m_flags & GRIP_FOURBUTTON) {
310 buttons[0] = (my_buttons & libnifalcon::FalconGripFourButton::CENTER_BUTTON) ? 1 : 0;
311 buttons[1] = (my_buttons & libnifalcon::FalconGripFourButton::PLUS_BUTTON) ? 1 : 0;
312 buttons[2] = (my_buttons & libnifalcon::FalconGripFourButton::MINUS_BUTTON) ? 1 : 0;
313 buttons[3] = (my_buttons & libnifalcon::FalconGripFourButton::FORWARD_BUTTON) ? 1 : 0;
319 bool set_force(
const d_vector &force) {
324 m_falconDevice->setForce(force);
325 if(!m_falconDevice->runIOLoop())
333 libnifalcon::FalconDevice *m_falconDevice;
335 struct timeval m_oldtime;
339 vrpn_NovintFalcon_Device() {};
341 vrpn_NovintFalcon_Device(
const vrpn_NovintFalcon_Device &dev) {};
347 MASK_DEVICEIDX = 0x000f,
348 KINE_STAMPER = 0x0010,
349 GRIP_FOURBUTTON = 0x0100
354class ForceFieldEffect
358 ForceFieldEffect() : m_active(false), m_time(0), m_cutoff(0.0), m_damping(0.9)
361 for (i=0; i < 3; i++) {
364 for (j=0; j < 3; j++) {
365 m_jacobian[i][j] = 0.0;
371 ~ForceFieldEffect() {}
375 void setForce(vrpn_float32 ori[3], vrpn_float32 frc[3], vrpn_float32 jac[3][3], vrpn_float32 rad) {
377 for (i=0; i < 3; i++) {
378 m_neworig[i] = ori[i];
379 m_newadd[i] = frc[i];
380 for (j=0; j < 3; j++) {
381 m_newjacob[i][j] = jac[i][j];
388 void setDamping(
double damp) {
393 virtual bool start() {
403 virtual void stop() {
408 virtual bool isActive()
const {
return m_active; }
411 d_vector calcForce(
const d_vector &pos) {
412 d_vector force, offset;
416 const double mix = 1.0 - m_damping;
418 for (i=0; i < 3; i++) {
419 m_origin[i] = m_damping*m_origin[i] + mix*m_neworig[i];
420 m_addforce[i] = m_damping*m_addforce[i] + mix*m_newadd[i];
421 for (j=0; j < 3; j++) {
422 m_jacobian[i][j] = m_damping*m_jacobian[i][j] + mix*m_newjacob[i][j];
425 m_cutoff = m_damping*m_cutoff + mix*m_newcut;
427 offset = pos - m_origin;
429 if (d_length(offset) > m_cutoff) {
441 for (i=0; i<3; ++i) {
442 for (j=0; j<3; ++j) {
443 force[i] += offset[j]*m_jacobian[i][j];
456 double m_jacobian[3][3];
460 double m_newjacob[3][3];
467 vrpn_Tracker_NovintFalcon *dev = (vrpn_Tracker_NovintFalcon *)userdata;
468 dev->update_forcefield_effect(p);
473class vrpn_NovintFalcon_ForceObjects {
475 std::vector<ForceFieldEffect*> m_FFEffects;
484 vrpn_NovintFalcon_ForceObjects() {
485 m_curforce.fill(0.0);
489 ~vrpn_NovintFalcon_ForceObjects() {};
492 d_vector getObjForce(vrpn_float64 *pos, vrpn_float64 *vel) {
495 for (i=0; i<3; ++i) {
502 int nobj = m_FFEffects.size();
503 for (i=0; i<nobj; ++i) {
504 m_curforce = m_curforce + m_FFEffects[i]->calcForce (m_curpos);
511vrpn_Tracker_NovintFalcon::vrpn_Tracker_NovintFalcon(
const char *name,
518 vrpn_ForceDevice(name, c), m_dev(NULL), m_obj(NULL), m_update_rate(1000.0), m_damp(0.9)
520 m_devflags=vrpn_NovintFalcon_Device::MASK_DEVICEIDX & devidx;
522 if (0 == strcmp(grip,
"4-button")) {
523 m_devflags |= vrpn_NovintFalcon_Device::GRIP_FOURBUTTON;
526 fprintf(stderr,
"WARNING: Unknown grip for Novint Falcon #%d: %s \n", devidx, grip);
533 if (0 == strcmp(kine,
"stamper")) {
534 m_devflags |= vrpn_NovintFalcon_Device::KINE_STAMPER;
536 fprintf(stderr,
"WARNING: Unknown kinematic model for Novint Falcon #%d: %s \n", devidx, kine);
543 vrpn_float64 val= atof(damp);
544 if (val >= 1.0 && val <= 10000.0) {
545 m_damp = 1.0 - 1.0/val;
547 fprintf(stderr,
"WARNING: Ignoring illegal force effect damping factor: %g \n", val);
552 if (register_autodeleted_handler(forcefield_message_id,
554 fprintf(stderr,
"vrpn_Tracker_NovintFalcon:can't register force handler\n");
561void vrpn_Tracker_NovintFalcon::clear_values()
564 if (m_devflags < 0)
return;
574 fprintf(stderr,
"vrpn_Tracker_NovintFalcon::clear_values(): delete failed\n");
578 try { m_obj =
new vrpn_NovintFalcon_ForceObjects; }
579 catch (...) { m_obj = NULL;
return; }
582 ForceFieldEffect *ffe;
583 try { ffe =
new ForceFieldEffect; }
584 catch (...) {
return; }
585 ffe->setDamping(m_damp);
587 m_obj->m_FFEffects.push_back(ffe);
590vrpn_Tracker_NovintFalcon::~vrpn_Tracker_NovintFalcon()
593 if (m_dev)
delete m_dev;
594 if (m_obj)
delete m_obj;
596 fprintf(stderr,
"vrpn_Tracker_NovintFalcon::~vrpn_Tracker_NovintFalcon(): delete failed\n");
601void vrpn_Tracker_NovintFalcon::reset()
605 if (m_devflags < 0)
return;
610 fprintf(stderr,
"Resetting the NovintFalcon #%d\n",
611 vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags);
617 fprintf(stderr,
"vrpn_Tracker_NovintFalcon::reset(): delete failed\n");
622 try { m_dev =
new vrpn_NovintFalcon_Device(m_devflags); }
625 fprintf(stderr,
"Device constructor failed!\n");
632 if (!m_dev->init()) {
634 fprintf(stderr,
"Device init failed!\n");
640 fprintf(stderr,
"Reset Completed.\n");
644int vrpn_Tracker_NovintFalcon::get_report(
void)
650 if (m_dev->get_status(pos, vel, d_quat, vel_quat, &vel_quat_dt, buttons)) {
654 for (i=0; i < num_buttons; i++)
657 if (j == num_buttons) {
669 print_latest_report();
675void vrpn_Tracker_NovintFalcon::send_report(
void)
680 if (d_connection->pack_message(len, m_timestamp, position_m_id, d_sender_id, msgbuf,
684 if (d_connection->pack_message(len, m_timestamp, velocity_m_id, d_sender_id, msgbuf,
690void vrpn_Tracker_NovintFalcon::handle_forces(
void)
696 d_vector force= m_obj->getObjForce(pos,vel);
697 m_dev->set_force(force);
703 if (!m_obj)
return 1;
705 vrpn_float32 center[3];
706 vrpn_float32 force[3];
707 vrpn_float32 jacobian[3][3];
713 m_obj->m_FFEffects[0]->start();
714 m_obj->m_FFEffects[0]->setForce(center, force, jacobian, radius);
719void vrpn_Tracker_NovintFalcon::mainloop()
721 struct timeval current_time;
726 if ( timediff(current_time, m_timestamp) >= 1000000.0/m_update_rate) {
729 m_timestamp.tv_sec = current_time.tv_sec;
730 m_timestamp.tv_usec = current_time.tv_usec;
750 fprintf(stderr,
"NovintFalcon #%d , unknown status message: %d)\n",
751 vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags, status);
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Generic connection class not specific to the transport mechanism.
virtual int encode_to(char *buf)
virtual int encode_vel_to(char *buf)
This structure is what is passed to a vrpn_Connection message callback.
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
#define vrpn_gettimeofday
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_AWAITING_STATION