-
|
Hello, I'm finally working on sending haptic forces computed in SofaPython to the Geomagic Touch haptic device through the Geomagic plugin.
My idea was to send the haptic forces to the "inputForceFeedback" attribute of the GeomagicDevice. It says from the GUI explanation that "Input force feedback in case no LCPForceFeedback is found. So I assumed that LCPForceFeedback must be temporarily disabled for this to work? However, I could not find any documentation on it, and I get this error while trying to run it:
The other option I was looking at was overwriting the "currentForce" vector in "GeomagicDriver.cpp" lines 97-139. Here is a condensed version of the code. Any ideas if this would work? import Sofa.Core
import Sofa
import SofaRuntime
from scipy import sparse
from scipy import linalg
from matplotlib import pyplot as plt
import numpy as np
from functools import wraps
from kalmanFilterForce1d import kalmanFilter
from getEmpiricalForces import getProbingForces
from principalDirectionFunction import calculatePrincipalComponents
addLateralMeniscus = True
sendToHapticDevice = True
extractContactForce = True
# Function called when the scene graph is being created
def createScene(rootNode):
rootNode.dt=0.01 #0.01, 0.05, 0.005
rootNode.gravity=[0, -9.81e3, 0]
confignode = rootNode.addChild("Config")
confignode.addObject('RequiredPlugin', name="SofaMiscCollision", printLog=False)
confignode.addObject('RequiredPlugin', name="SofaPython3", printLog=False)
confignode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight")
rootNode.addObject('DefaultVisualManagerLoop')
rootNode.addObject('DefaultPipeline', name="pipeline", depth="6", verbose="0")
rootNode.addObject('BruteForceBroadPhase')
rootNode.addObject('BVHNarrowPhase')
rootNode.addObject('DefaultContactManager', name="response", response="FrictionContactConstraint") #responseParams="mu=0.1") #Does this call on the penalty method, instead of Lagrange M?
rootNode.addObject('LocalMinDistance', alarmDistance="3.408", contactDistance="1.136", angleCone="0.0")
rootNode.addObject('FreeMotionAnimationLoop') #, parallelCollisionDetectionAndFreeMotion = True, parallelODESolving = True)
LCP = rootNode.addObject('LCPConstraintSolver', tolerance="0.001", maxIt="1000", computeConstraintForces="True", mu="0.000001")
rootNode.addObject('RequiredPlugin', name='Geomagic')
driver = rootNode.addObject('GeomagicDriver', name='GeomagicDevice', deviceName="Default Device", scale="30", drawDeviceFrame="1", positionBase="0 0 0", drawDevice="0", orientationBase="0 0.707 0 -0.707")
#LATERAL MENISCUS NODE BEGIN*************************************************************************************************************************************************************************
if addLateralMeniscus == True:
lateralMeniscus = rootNode.addChild('lateralMeniscus')
lateralMeniscus.gravity = [0, -9.81e3, 0]
lateralMeniscus.addObject('EulerImplicitSolver', name="cg_odesolver")
lateralMeniscus.addObject('CGLinearSolver', name="linear solver", iterations="25", tolerance="1e-09", threshold="1e-09")
lateralMeniscus.addObject('MeshGmshLoader', name="meshLoader", filename="mesh/lateralMeniscusComputationModelSTP.msh", rotation="90 270 -182", translation="0 -31.5 -26", scale3d="26 26 26")
lateralMeniscus.addObject('TetrahedronSetTopologyContainer', name="topo", position="-0.5 0 0 0.542 0.455 0.542 0.455", src="@meshLoader")
lateralMeniscusDofs = lateralMeniscus.addObject('MechanicalObject', name="lateralMeniscusDofs", src="@meshLoader")
lateralMeniscus.addObject('TetrahedronSetGeometryAlgorithms', template="Vec3d", name="GeomAlgo")
lateralMeniscus.addObject('DiagonalMass', name="mass", massDensity="1.09e-09", topology="@topo", geometryState="@lateralMeniscusDofs") # massDensity="10.9e-03"
lateralMeniscus.addObject('TetrahedralCorotationalFEMForceField', template="Vec3d", name="FEM", poissonRatio="0.3", youngModulus="15")
lateralMeniscus.addObject('PrecomputedConstraintCorrection', recompute="true")
#Add boundary conditions (missing collision between bone and meniscus)
lateralMeniscus.addObject('FixedConstraint', name="FixedConstraint", indices="1 2 5 6 7 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 73 198 199 201 203 204 206 207 209 210 212 213 215 216 217 219 220 221 222 223 224 225 226 229 230 231 232 233 234 235 236 237 238 239 242 248 249 251 252 253 254 255 257")
#Lateral meniscus visual model
lateralMeniscusVisual = lateralMeniscus.addChild('Lateral Meniscus Visual Model')
lateralMeniscusVisual.addObject('MeshOBJLoader', name="meshLoader_LM", filename="mesh/LATERAL_MENISCUS_VISUAL_MODEL.obj", rotation="-85 277 -185", translation="120 105 -80", scale3d="0.0545 0.0545 0.0545", handleSeams="1")
lateralMeniscusVisual.addObject('OglModel', name="VisualModel", src="@meshLoader_LM", color="1.0 0.2 0.2 1.0")
lateralMeniscusVisual.addObject('BarycentricMapping', name="visual mapping", input="@../lateralMeniscusDofs", output="@VisualModel")
#Lateral meniscus collision model (extracts triangular mesh from computation model)
lateralMeniscusTriangularSurface = lateralMeniscus.addChild('Lateral Meniscus Triangular Surface Model')
lateralMeniscusTriangularSurface.addObject('TriangleSetTopologyContainer', name="Container")
lateralMeniscusTriangularSurface.addObject('TriangleSetTopologyModifier', name="Modifier")
lateralMeniscusTriangularSurface.addObject('Tetra2TriangleTopologicalMapping', input="@../topo", output="@Container")
lateralMeniscusTriangularSurface.addObject('TriangleCollisionModel', name="CollisionModel", contactStiffness="0.13")
#LATERAL MENISCUS NODE END***************************************************************************************************************************************************************************
#OMNI NODE BEGIN*************************************************************************************************************************************************************************************
omni = rootNode.addChild('Omni')
omniDOFs = omni.addObject('MechanicalObject', name='DOFs', template='Rigid3d', position='@GeomagicDevice.positionDevice')
#OMNI NODE END*********************************************************************************************************************************************************************************************
#INSTRUMENT NODE BEGIN*************************************************************************************************************************************************************************************
instrument = rootNode.addChild('Instrument')
instrument.addObject('EulerImplicitSolver', name="ODE solver", rayleighStiffness="0.05", rayleighMass="1.0")
instrument.addObject('CGLinearSolver', name="linear solver", iterations="25", tolerance="1.0e-5", threshold="10.0e-5")
instrument.addObject('MechanicalObject', name='instrumentState', template='Rigid3d')
instrument.addObject('UniformMass', name="mass", totalMass="0.1")
instrument.addObject('LCPForceFeedback', activate="true", forceCoef="0.1")
instrument.addObject('UncoupledConstraintCorrection')
instrument.addObject('RestShapeSpringsForceField', stiffness="1000", angularStiffness="1e9", external_rest_shape="@/Omni", points="0")
#Instrument visual model
instrumentVisu = instrument.addChild('InstrumentVisualModel')
instrumentVisu.addObject('MeshOBJLoader', name="loader", filename="mesh/ARTHREX_AR_10000_HookProbe.obj", scale3d="1 1 1", handleSeams="1")
instrumentVisu.addObject('OglModel', name="InstrumentVisualmodel", src="@loader", color="gray", rx="270", ry="90", rz="90", dy="5", dz="150")
instrumentVisu.addObject('RigidMapping', name="MM->VM mapping", input="@instrumentState", output="@InstrumentVisualModel")
#Instrument collision model
instrumentCollision = instrument.addChild('instrumentCollision')
instrumentCollision.addObject('MeshOBJLoader', name="loader", filename="mesh/ARTHREX_AR_10000_HookProbe.obj", scale3d="1 1 1", handleSeams="1")
instrumentCollision.addObject('MeshTopology', src="@loader")
instrumentCollision.addObject('MechanicalObject', name="instrumentCollisionState1", src="@loader",rx="270", ry="90", rz="90", dy="5", dz="150")
instrumentCollision.addObject('LineCollisionModel', contactStiffness="0.44e-9")
instrumentCollision.addObject('PointCollisionModel', contactStiffness="0.44e-9")
instrumentCollision.addObject('RigidMapping', name="MM->CM mapping", input="@instrumentState", output="@instrumentCollisionState1")
#INSTRUMENT NODE END**********************************************************************************************************************************************************************************************
#Access data
if extractContactForce == True:
rootNode.addObject(MatrixAccessController('MatrixAccessor', name='matrixAccessor', target=lateralMeniscusDofs, target2=LCP, target3=omniDOFs, rootNode=rootNode))
return rootNode
class MatrixAccessController(Sofa.Core.Controller):
def __init__(self, *args, **kwargs):
Sofa.Core.Controller.__init__(self, *args, **kwargs)
self.target = kwargs.get("target", None)
self.target2 = kwargs.get("target2", None)
self.target3 = kwargs.get("target3", None)
self.rootNode = kwargs.get("rootNode", None)
self.contactOngoing = False
def onAnimateEndEvent(self, event):
jacobian = self.target.getData("constraint")
lambda_ = self.target2.getData("constraintForces")
instrumentPosition = self.target3.getData("position")
GeomagicDevice = self.rootNode.GeomagicDevice
lcpForceFeedback = self.rootNode.Instrument.getObject("LCPForceFeedback")
if contact == True: #Essentially only compute interaction forces if there is interaction between target object and instrument
#Calculate haptic force (lot's of stuff going on here normally)
hapticForce = np.array([1,2,3])
#SEND HAPTIC FORCE TO DEVICE*******************************************************************************************************
if sendToHapticDevice:
#Option 1: Overwrite LCPForceFeedback() (See GeomagicDriver.cpp in SOFA)
#GeomagicDevice.currentForce[0] = hapticForce[0]
#GeomagicDevice.currentForce[1] = hapticForce[1]
#GeomagicDevice.currentForce[2] = hapticForce[2]
#Option 2: Disable LCPForceFeedback(), then:
lcpForceFeedback.activate = "false"
GeomagicDevice.inputForceFeedback[0] = hapticForce[0] #Try also d_inputForceFeedback if not working
GeomagicDevice.inputForceFeedback[1] = hapticForce[1]
GeomagicDevice.inputForceFeedback[2] = hapticForce[2]
lcpForceFeedback.activate = "true"
# Function used only if this script is called from a python environment
if __name__ == '__main__':
main() |
Beta Was this translation helpful? Give feedback.
Replies: 1 comment 8 replies
-
|
Hi @oystebje Sorry for keeping you waiting, I had not much time these days. This check is only done at init. It therefore seems that it is impossible to use both forces feedback from the constraint resolution and manual input forces. Is that what you want to do ? |
Beta Was this translation helpful? Give feedback.
-
|
Thank you for the feedback, @hugtalbot! What I want to do is basically to overwrite the haptic force signal sent to the Geomagic device. Preferrably from SofaPython. From what I can read from the source code, this seems to be either the "inputForceFeedback" or the "currentForce" vectors, as shown in the GeomagicDriver.cpp, lines 97-137. See code below. Is there any way I can access these from SofaPython? Vector3 currentForce;
if (driver->m_forceFeedback)
{
Vector3 pos(driver->m_hapticData.transform[12+0]*0.1,driver->m_hapticData.transform[12+1]*0.1,driver->m_hapticData.transform[12+2]*0.1);
Vector3 pos_in_world = driver->d_positionBase.getValue() + driver->d_orientationBase.getValue().rotate(pos*driver->d_scale.getValue());
driver->m_forceFeedback->computeForce(pos_in_world[0],pos_in_world[1],pos_in_world[2], 0, 0, 0, 0, currentForce[0], currentForce[1], currentForce[2]);
driver->m_isInContact = false;
for (int i=0; i<3; i++)
if (currentForce[i] != 0.0)
{
driver->m_isInContact = true;
break;
}
}
else
{
Vector3 inputForceFeedback = driver->d_inputForceFeedback.getValue();
double normValue = inputForceFeedback.norm();
double maxInputForceFeedback = driver->d_maxInputForceFeedback.getValue();
if( maxInputForceFeedback > 0.0)
{
if( normValue > maxInputForceFeedback )
{
msg_warning(driver) << "Force given to applied inputForceFeedback (norm = "<< normValue <<") exceeds the maxInputForceFeedback ("<< maxInputForceFeedback <<")";
inputForceFeedback[0] *= maxInputForceFeedback/normValue;
inputForceFeedback[1] *= maxInputForceFeedback/normValue;
inputForceFeedback[2] *= maxInputForceFeedback/normValue;
driver->d_inputForceFeedback.setValue(inputForceFeedback);
}
currentForce = driver->d_inputForceFeedback.getValue();
}
else
{
msg_error(driver) << "maxInputForceFeedback value ("<< maxInputForceFeedback <<") is negative or 0, it should be strictly positive";
}
}
Vector3 force_in_omni = driver->d_orientationBase.getValue().inverseRotate(currentForce) * driver->d_forceScale.getValue(); |
Beta Was this translation helpful? Give feedback.
-
|
Hey @oystebje In that case, I would simply use a GeomagicDriver without LCPForceFeedback and using the data |
Beta Was this translation helpful? Give feedback.
-
|
@hugtalbot - thanks! My understanding was that as a consequence of disabling the LCPForceFeedback, the haptic forces (LCP) will not be computed at all - it's not just that they are not sent to the Geomagic driver. Is this right? If not, then this approach should work fine. Will try it out in the lab tomorrow. In my python script, I want to first read the contact forces between the instrument and lateral meniscus (similar to #3812), then perform my own computations (I have implemented up to here), and finally send these to the Geomagic driver. So I'm dependent on the contact forces being calculated. Thanks again for helping! |
Beta Was this translation helpful? Give feedback.
-
|
Hi @oystebje The absence of the LCPForceFeedback assumes that no forces will be translated from the simulation to the haptic device. However, as long as your scene contains a "ConstraintSolver" the constraint (contact) forces will be computed. So it should work. The issue you might face is that the forces computed by the ConstraintSolver (LCPConstraintSolver or GenericConstraintSolver) are computed at low rate and not at the haptic high rate. A PhD student is currently working on this topic (she's in deadline and I will point her this topic) Keep us posted 👍 |
Beta Was this translation helpful? Give feedback.
-
|
@hugtalbot - that makes sense. So I got to try it out now and the synchronization seems to be an issue as you have foreseen. The good news are that commenting out "LCPForceFeedback" in the instrument node transfers no forces to the haptic device, and the constraint forces are still computed. Reading the inputForceFeedback vector from the Geomagic driver also works well: print('inputForceFeedback: ' + str(GeomagicDevice.inputForceFeedback[0]) + str(GeomagicDevice.inputForceFeedback[1]) + str(GeomagicDevice.inputForceFeedback[2]))
However, writing to the haptic device gives a thread error, as shown in the video below. GeomagicDevice.inputForceFeedback[0] = hapticForce[0]
GeomagicDevice.inputForceFeedback[1] = hapticForce[1]
GeomagicDevice.inputForceFeedback[2] = hapticForce[2]SOFA_sync_issue_720p.mp4Perhaps it is better to transform the haptic forces back into constraint space and update lambda.. Thanks again! |
Beta Was this translation helpful? Give feedback.
-
|
The warning I see is the one warning that the input force exceeds the max force allowed (defined as a data in LCPForceFeedback) |
Beta Was this translation helpful? Give feedback.
Hey @oystebje
In that case, I would simply use a GeomagicDriver without LCPForceFeedback and using the data
inputForceFeedbackinput directly the forces you want to apply from you python script. Is that fitting what you want?