From 0ce78cd3b5a20c86db487cae35dc017177349d48 Mon Sep 17 00:00:00 2001 From: epernod Date: Tue, 6 Oct 2026 16:42:20 +0200 Subject: [PATCH] [src] Add extension input/output int Data. Add customDevice behavior --- .../Haply_Inverse3Controller.cpp | 22 +++++++++++++++++-- .../Haply_Inverse3Controller.h | 5 +++++ 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/SofaHaplyRobotics/Haply_Inverse3Controller.cpp b/src/SofaHaplyRobotics/Haply_Inverse3Controller.cpp index fd8a113..44a5109 100644 --- a/src/SofaHaplyRobotics/Haply_Inverse3Controller.cpp +++ b/src/SofaHaplyRobotics/Haply_Inverse3Controller.cpp @@ -68,12 +68,15 @@ Haply_Inverse3Controller::Haply_Inverse3Controller() , 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_extensionButton(initData(&d_extensionButton, 0, "extensionButton", "Int value returning the extension value of the device [0; 255]. For future use.")) , d_posDevice(initData(&d_posDevice, "positionDevice", "position of the device end-effector in SOFA frame")) , d_oriDevice(initData(&d_oriDevice, "originDevice", "position (Rigid) of the base of the device (origin of the device 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")) , d_maxForce(initData(&d_maxForce, 5.0, "maxForce", "Maximum absolute value of each force component sent to the device")) - + , d_inputExtensionInfo(initData(&d_inputExtensionInfo, 0, "inputExtensionInfo", "Int value to send extension information to the device [0; 255]. For future use.")) + , d_mimicCustomDevice(initData(&d_mimicCustomDevice, true, "mimicCustomDevice", "Temporary bool to activate mimic of a custom device If no available.")) + , d_drawDebug(initData(&d_drawDebug, false, "drawDebug", "Parameter to draw debug information")) , l_forceFeedback(initLink("forceFeedBack", "link to the forceFeedBack component, if not set will search through graph and take first one encountered.")) { @@ -86,6 +89,7 @@ Haply_Inverse3Controller::Haply_Inverse3Controller() d_handleButtonA.setReadOnly(true); d_handleButtonB.setReadOnly(true); d_handleButtonC.setReadOnly(true); + d_extensionButton.setReadOnly(true); this->addAlias(&this->d_handleButtonA, "handleButton"); d_rawForceDevice.setReadOnly(true); @@ -93,7 +97,7 @@ Haply_Inverse3Controller::Haply_Inverse3Controller() d_handleButtonA.setGroup("Device Status"); d_handleButtonB.setGroup("Device Status"); d_handleButtonC.setGroup("Device Status"); - + d_extensionButton.setGroup("Device Status"); d_rawForceDevice.setGroup("Device Status"); } @@ -471,6 +475,20 @@ void Haply_Inverse3Controller::simulation_updatePosition() d_handleButtonB.setValue(m_simuData.buttonB); d_handleButtonC.setValue(m_simuData.buttonC); + if (d_mimicCustomDevice.getValue()) + { + int value = d_extensionButton.getValue(); + if (m_simuData.buttonA) + value += 1; + else if (m_simuData.buttonB) + value -= 1; + + value = std::clamp(value, 0, 255); + d_extensionButton.setValue(value); + } + else + d_extensionButton.setValue(m_simuData.extensionValue); + Quat ori = { m_simuData.orientation[0], m_simuData.orientation[1], m_simuData.orientation[2], m_simuData.orientation[3] }; Coord& posDevice = sofa::helper::getWriteOnlyAccessor(d_posDevice); diff --git a/src/SofaHaplyRobotics/Haply_Inverse3Controller.h b/src/SofaHaplyRobotics/Haply_Inverse3Controller.h index 257b3c7..dfb96e9 100644 --- a/src/SofaHaplyRobotics/Haply_Inverse3Controller.h +++ b/src/SofaHaplyRobotics/Haply_Inverse3Controller.h @@ -110,11 +110,15 @@ class SOFA_HAPLYROBOTICS_API Haply_Inverse3Controller : public Controller Data d_handleButtonA; ///< Bool value showing if First button is pressed Data d_handleButtonB; ///< Bool value showing if Second button is pressed Data d_handleButtonC; ///< Bool value showing if Third button is pressed + Data d_extensionButton; ///< Int value showing the extension value of the device [0; 255]. For future use. Data 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 d_oriDevice; ///< position (Rigid) of the base of the device (origin of the device in SOFA frame). Computed using @sa d_positionBase, @sa d_orientationBase and @sa d_scale Data d_rawForceDevice; ///< For debug: raw values sent to the device in the device frame Data d_dampingForce; ///< Damping value, it is a factor applied to the velocity and substracted to force feedback to avoid oscillations. Data d_maxForce; ///< Maximum absolute value of each force component sent to the device + + Data d_inputExtensionInfo; /// Int value to send extension information to the device [0; 255]. For future use. + Data d_mimicCustomDevice; /// Temporary bool to activate mimic of a custom device If no available. /// Data parameter to draw debug information Data d_drawDebug; @@ -136,6 +140,7 @@ class SOFA_HAPLYROBOTICS_API Haply_Inverse3Controller : public Controller bool buttonA; // button A press status bool buttonB; // button B press status bool buttonC; // button C press status + int extensionValue; // extension value for future use }; /// Data belonging to the haptic thread only