ENH: Add apply forces API in controller and client

Base/Devices/imstkDeviceClient.cpp

@@ -151,10 +151,9 @@ DeviceClient::getForce() const
     return m_force;
 }
 
-const Vec3d&
-DeviceClient::getTorque() const
+void
+DeviceClient::setForce(Vec3d force)
 {
-    return m_torque;
+    m_force = force;
 }
-
 }

Base/Devices/imstkDeviceClient.h

@@ -84,14 +84,10 @@ public:
     bool getButton(size_t buttonId) const;
 
     ///
-    /// \brief Get the device force
+    /// \brief Get/Set the device force
     ///
     const Vec3d& getForce() const;
-
-    ///
-    /// \brief Get the device torque
-    ///
-    const Vec3d& getTorque() const;
+    void setForce(Vec3d force);
 
 protected:
 
@@ -112,7 +108,6 @@ protected:
     Quatd m_orientation = Quatd::Identity(); //!< Orientation of the end effector
     std::map<size_t, bool> m_buttons;        //!< Buttons: true = pressed/false = not pressed
     Vec3d m_force = Vec3d::Zero();           //!< Force vector
-    Vec3d m_torque = Vec3d::Zero();          //!< Torque vector
 };
 }
 

Base/Devices/imstkVRPNDeviceClient.cpp

@@ -42,6 +42,10 @@ VRPNDeviceClient::initModule()
     m_vrpnAnalog->register_change_handler(this, analogChangeHandler);
     m_vrpnButton->register_change_handler(this, buttonChangeHandler);
     m_vrpnForceDevice->register_force_change_handler(this, forceChangeHandler);
+
+    m_vrpnForceDevice->setFF_Origin(0,0,0);
+    m_vrpnForceDevice->setFF_Jacobian(0,0,0,0,0,0,0,0,0);
+    m_vrpnForceDevice->setFF_Radius(2);
 }
 
 void
@@ -61,6 +65,8 @@ VRPNDeviceClient::runModule()
     }
     if (this->getForceEnabled())
     {
+        m_vrpnForceDevice->setFF_Force(m_force[0], m_force[1], m_force[2]);
+        m_vrpnForceDevice->sendForceField();
         m_vrpnForceDevice->mainloop();
     }
 }
@@ -74,6 +80,8 @@ VRPNDeviceClient::cleanUpModule()
     m_vrpnButton->unregister_change_handler(this, buttonChangeHandler);
     m_vrpnForceDevice->unregister_force_change_handler(this, forceChangeHandler);
 
+    m_vrpnForceDevice->stopForceField();
+
     m_vrpnTracker.reset();
     m_vrpnAnalog.reset();
     m_vrpnButton.reset();
@@ -109,12 +117,12 @@ VRPNDeviceClient::analogChangeHandler(void *userData, const _vrpn_ANALOGCB a)
 {
     auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
 
-    if (a.num_channel > 0)
+    if (a.num_channel >= 3)
     {
         deviceClient->m_position << a.channel[0], a.channel[1], a.channel[2];
         //LOG(DEBUG) << "analog: position = " << deviceClient->m_position;
     }
-    if (a.num_channel > 3)
+    if (a.num_channel >= 6)
     {
         deviceClient->m_orientation =
                 Rotd(a.channel[3]*M_PI,Vec3d::UnitX())*
@@ -137,7 +145,7 @@ VRPNDeviceClient::buttonChangeHandler(void *userData, const _vrpn_BUTTONCB b)
 {
     auto deviceClient = reinterpret_cast<VRPNDeviceClient*>(userData);
     deviceClient->m_buttons[b.button] = (b.state == 1);
-    LOG(DEBUG) << "buttons: " << b.button << " = " << deviceClient->m_buttons[b.button];
+    //LOG(DEBUG) << "buttons: " << b.button << " = " << deviceClient->m_buttons[b.button];
 }
 
 void VRPN_CALLBACK

Base/Scene/Object/imstkVirtualCouplingObject.cpp

@@ -59,4 +59,22 @@ VirtualCouplingObject::updateFromDevice()
         m_collidingToVisualMap->apply();
     }
 }
+
+void
+VirtualCouplingObject::applyForces()
+{
+    m_deviceClient->setForce(m_force);
+}
+
+const Vec3d&
+VirtualCouplingObject::getForce() const
+{
+    return m_force;
+}
+
+void
+VirtualCouplingObject::setForce(Vec3d force)
+{
+    m_force = force;
+}
 }

Base/Scene/Object/imstkVirtualCouplingObject.h

@@ -55,6 +55,21 @@ public:
     ///
     void updateFromDevice();
 
+    ///
+    /// \brief Apply forces to the haptic device
+    ///
+    void applyForces();
+
+    ///
+    /// \brief Get/Set the force to apply to the device
+    ///
+    const Vec3d& getForce() const;
+    void setForce(Vec3d force);
+
+protected:
+
+    bool m_forceModified;
+    Vec3d m_force = Vec3d::Zero();
 };
 
 }

Base/SimulationManager/imstkSceneManager.cpp

@@ -62,6 +62,7 @@ SceneManager::runModule()
         if (auto virtualCoupling = std::dynamic_pointer_cast<VirtualCouplingObject>(obj))
         {
             virtualCoupling->updateFromDevice();
+            virtualCoupling->applyForces();
         }
     }
 }