Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 20 additions & 2 deletions src/SofaHaplyRobotics/Haply_Inverse3Controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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."))
{
Expand All @@ -86,14 +89,15 @@ 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);

d_posDevice.setGroup("Device Status");
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");
}

Expand Down Expand Up @@ -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);
Expand Down
5 changes: 5 additions & 0 deletions src/SofaHaplyRobotics/Haply_Inverse3Controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -110,11 +110,15 @@ class SOFA_HAPLYROBOTICS_API Haply_Inverse3Controller : public Controller
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<int> d_extensionButton; ///< Int value showing the extension value of the device [0; 255]. For future use.
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<Coord> 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<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.
Data<SReal> d_maxForce; ///< Maximum absolute value of each force component sent to the device

Data<int> d_inputExtensionInfo; /// Int value to send extension information to the device [0; 255]. For future use.
Data<bool> d_mimicCustomDevice; /// Temporary bool to activate mimic of a custom device If no available.

/// Data parameter to draw debug information
Data<bool> d_drawDebug;
Expand All @@ -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
Expand Down
Loading