[src] Add option to support the wireless grip version with 3 buttons (#28)

NEEDLE-11 #done #time 2h

---------

* [src] Add option to support the wireless grip version with 3 buttons

* [src] Fix several bugs in the integration of the wireless stylus

---------

Co-authored-by: epernod <erik.pernod@gmail.com>

Author: th-skam

src/SofaHaplyRobotics/Haply_Inverse3Controller.cpp

@@ -42,7 +42,8 @@ namespace sofa::HaplyRobotics
 // Static constants
 const std::string Haply_Inverse3Controller::inverseKey_ = "inverse3";
 const std::string Haply_Inverse3Controller::deviceIdKey_ = "device_id";
-const std::string Haply_Inverse3Controller::gripIdKey_ = "verse_grip";
+const std::string Haply_Inverse3Controller::gripIdKey_ = "verse_grip"; // Refered as "VerseGrip Quill" on Haply developer doc.
+const std::string Haply_Inverse3Controller::wirelessGripIdKey_ = "wireless_verse_grip"; // Refered as "VerseGrip Stylus" on Haply developer doc.
 
 
 using namespace sofa::helper::system::thread;
@@ -59,7 +60,9 @@ Haply_Inverse3Controller::Haply_Inverse3Controller()
     , d_orientationBase(initData(&d_orientationBase, Quat(0, 0, 0, 1), "orientationBase", "Orientation of the device base in the SOFA scene world coordinates"))    
     , d_scale(initData(&d_scale, 1.0, "scale", "Default scale applied to the Device coordinates"))    
 
-    , d_handleButton(initData(&d_handleButton, false, "handleButton", "Bool value showing if handle button is pressed"))
+    , d_handleButtonA(initData(&d_handleButtonA, false, "handleButtonA", "Bool value returning if VerseGrip Stylus first button (resp. VerseGrip Quill single button) is pressed."))
+    , d_handleButtonB(initData(&d_handleButtonB, false, "handleButtonB", "Bool value returning if VerseGrip Stylus second button is pressed"))
+    , d_handleButtonC(initData(&d_handleButtonC, false, "handleButtonC", "Bool value returning if VerseGrip Stylus calibrate button is pressed"))
     , d_posDevice(initData(&d_posDevice, "positionDevice", "position of the device end-effector in SOFA frame"))
     , d_rawForceDevice(initData(&d_rawForceDevice, "rawForceDevice", "For debug: raw values sent to the device in the device frame"))
     , d_dampingForce(initData(&d_dampingForce, 0.0001, "damping", "Default damping applied to the force feedback"))
@@ -73,10 +76,17 @@ Haply_Inverse3Controller::Haply_Inverse3Controller()
     d_hapticIdentity.setGroup("Infos");
 
     d_posDevice.setReadOnly(true);
-    d_handleButton.setReadOnly(true);
+    d_handleButtonA.setReadOnly(true);
+    d_handleButtonB.setReadOnly(true);
+    d_handleButtonC.setReadOnly(true);
+    this->addAlias(&this->d_handleButtonA, "handleButton");
     d_rawForceDevice.setReadOnly(true);
+
     d_posDevice.setGroup("Device Status");
-    d_handleButton.setGroup("Device Status");
+    d_handleButtonA.setGroup("Device Status");
+    d_handleButtonB.setGroup("Device Status");
+    d_handleButtonC.setGroup("Device Status");
+
     d_rawForceDevice.setGroup("Device Status");
 }
 
@@ -317,7 +327,7 @@ void Haply_Inverse3Controller::HapticsHandling(const std::string& msg)
         }
     }
 
-    if (data.contains(gripIdKey_)) 
+    if (data.contains(gripIdKey_) && !data[gripIdKey_].empty())
     {
         // example of grip data
         // grip_id: 61576 -> {"button":false, "hall" : 1, "orientation" : {"w":0.71655273, "x" : 0.19647217, "y" : 0.6290283, "z" : -0.22833252}, 
@@ -336,9 +346,34 @@ void Haply_Inverse3Controller::HapticsHandling(const std::string& msg)
             m_hapticData.orientation[1] = qy;
             m_hapticData.orientation[2] = qz;
             m_hapticData.orientation[3] = qw;
-            m_hapticData.buttonStatus = state["button"].get<bool>();
+            m_hapticData.buttonA = state["button"].get<bool>();
+            m_hapticData.buttonB = false;
+            m_hapticData.buttonC = false;
         }
     }
+	else if (data.contains(wirelessGripIdKey_) && !data[wirelessGripIdKey_].empty())
+	{
+		// example of wireless grip data
+        // {"device_id":"1534","state":{"battery_level":0.55833346,"battery_voltage":3.935,"buttons":{"a":false,"b":false,"c":false},"hall":18,"orientation":{"w":-0.02947998, "x":-0.22390747, "y" : 0.65689087, "z" : 0.71691895},
+        // "transform":{"position":{"x":0, "y" : 0, "z" : 0}, "rotation" : {"w":1, "x" : 0, "y" : 0, "z" : 0}, "scale" : {"x":1, "y" : 1, "z":1}}},"status":{"awake":true,"connected":true,"ready":true}}
+        for (auto& el : data[wirelessGripIdKey_])
+        {
+            const json& state = el["state"];
+
+            float qx = state["orientation"]["x"].get<float>();
+            float qy = state["orientation"]["y"].get<float>();
+            float qz = state["orientation"]["z"].get<float>();
+            float qw = state["orientation"]["w"].get<float>();
+
+            m_hapticData.orientation[0] = qx;
+            m_hapticData.orientation[1] = qy;
+            m_hapticData.orientation[2] = qz;
+            m_hapticData.orientation[3] = qw;
+            m_hapticData.buttonA = state["buttons"]["a"].get<bool>();
+            m_hapticData.buttonB = state["buttons"]["b"].get<bool>();
+            m_hapticData.buttonC = state["buttons"]["c"].get<bool>();
+        }
+	}
 
     m_ws->send(request.dump());
 
@@ -405,7 +440,9 @@ void Haply_Inverse3Controller::simulation_updatePosition()
     Vec3 position = { m_simuData.position[0], m_simuData.position[1], m_simuData.position[2] };
 
     // Update Data on button status
-    d_handleButton.setValue(m_simuData.buttonStatus);
+    d_handleButtonA.setValue(m_simuData.buttonA);
+    d_handleButtonB.setValue(m_simuData.buttonB);
+    d_handleButtonC.setValue(m_simuData.buttonC);
 
     Quat ori = { m_simuData.orientation[0], m_simuData.orientation[1], m_simuData.orientation[2], m_simuData.orientation[3] };
 
@@ -454,7 +491,7 @@ void Haply_Inverse3Controller::draw(const sofa::core::visual::VisualParams* vpar
 
     // Debug: Draw force feedback vector
     sofa::type::RGBAColor color4(1.0f, 0.0, 0.0f, 1.0);
-    if (d_handleButton.getValue())
+    if (d_handleButtonA.getValue())
     {
         color4 = sofa::type::RGBAColor(0.0f, 1.0, 0.0f, 1.0);
     }

src/SofaHaplyRobotics/Haply_Inverse3Controller.h

@@ -105,7 +105,9 @@ class SOFA_HAPLYROBOTICS_API Haply_Inverse3Controller : public Controller
     Data<SReal> d_scale; ///< Default scale applied to the device Coordinates
 
     /// Output Data
-    Data<bool> d_handleButton; ///< Bool value showing if handle button is pressed
+    Data<bool> d_handleButtonA; ///< Bool value showing if First button is pressed
+    Data<bool> d_handleButtonB; ///< Bool value showing if Second button is pressed
+    Data<bool> d_handleButtonC; ///< Bool value showing if Third button is pressed
     Data<Coord> d_posDevice; ///< position of the device end-effector in SOFA Frame. Take into account @sa d_positionBase, @sa d_orientationBase and @sa d_scale
     Data<Vec3> d_rawForceDevice; ///< For debug: raw values sent to the device in the device frame
 	Data<SReal> d_dampingForce; ///< Damping value, it is a factor applied to the velocity and substracted to force feedback to avoid oscillations. 
@@ -127,7 +129,9 @@ class SOFA_HAPLYROBOTICS_API Haply_Inverse3Controller : public Controller
         float position[3]; // raw position of the end-effector
         float orientation[4]; // raw quaternion of the handle
         float force[3]; // debug raw force vector sent to the device
-        bool buttonStatus; // button press status
+        bool buttonA; // button A press status
+        bool buttonB; // button B press status
+        bool buttonC; // button C press status
     };
 
     /// Data belonging to the haptic thread only
@@ -163,6 +167,7 @@ class SOFA_HAPLYROBOTICS_API Haply_Inverse3Controller : public Controller
     static const std::string inverseKey_;
     static const std::string deviceIdKey_;
     static const std::string gripIdKey_;
+    static const std::string wirelessGripIdKey_;
 };
 
 } // namespace sofa::HaplyRobotics