How to track soft finger's tip? i want to have logs of soft finger's tip position(x,y,z). is there a way???

[mohammad79sss]

import SofaRuntime
SofaRuntime.importPlugin("Sofa.Component")
import Sofa.Core
import random
import math
import numpy as np
from scipy import signal
import os
path = os.path.dirname(os.path.abspath(__file__)) + '\\design\\'

class FingerController(Sofa.Core.Controller):

	def __init__(self, *args, **kwargs):
		Sofa.Core.Controller.__init__(self, *args, **kwargs)
		self.node = kwargs['node']
		self.displ_list = kwargs['displacements']
		displ1 = self.displ_list[0].value[0]
		displ2 = self.displ_list[1].value[0]
		displ3 = self.displ_list[2].value[0]
		self.Positions = kwargs['position']
		self.time = 0  # Accumulated time
		self.step_counter = 0  # Counter for tracking steps
		self.log_interval = int(0.2 / self.node.dt.value)  # Number of steps for 200ms

		# BASE SEARCH
		self.minimum = 0
		self.posmin = 0
		for i in range(len(self.Positions)):
			if (
					self.Positions[i][2] <= self.minimum
					and -0.1 <= self.Positions[i][0] <= 0.1
			):
				self.minimum = self.Positions[i][2]
				self.posmin = i

		# FINGERTIP SEARCH
		self.posmax = max(range(len(self.Positions)), key=lambda i: self.Positions[i][2])

		# Open file for logging
		self.log_file = open("./plot/Fingertip_Position.txt", "w")
		self.log_file.write("Time(s) X Y Z\n")  # Header
		self.log_file.flush()

		for i in range(len(self.Positions)):
			if (
					self.Positions[i][2] >= self.Positions[self.posmax][2]
					and -0.1 <= self.Positions[i][0] <= 0.1
			):
				self.posmax = i

		# Prevent division by zero
		delta_x = self.Positions[self.posmax][0] - self.Positions[self.posmin][0]
		self.coeff = (self.Positions[self.posmax][2] - self.Positions[self.posmin][
			2]) / delta_x if delta_x != 0 else float("inf")

		self.angle = (math.atan(self.coeff) * 180 / math.pi) - 90
		print(self.angle)

		with open("./plot/Position_endEffector.txt", "w") as file1:
			file1.write(
				f"0.0 {self.Positions[self.posmax][0]} {self.Positions[self.posmax][1]} {self.Positions[self.posmax][2]}\n")

		with open("./plot/Length-Angle.txt", "w") as file2:
			file2.write(f"0.0 0.0 {self.angle}\n")

	def onAnimateEndEvent(self, event):
		self.time += self.node.dt.value
		self.step_counter += 1

		# Initialize displacement value

		if 0 <= self.time < 0.5:
			self.displ_list[0].value[0] = min(self.displ_list[0].value[0] + 8.0, 9.0)  # cable1
			self.displ_list[1].value[0] = min(self.displ_list[1].value[0] + 3.0, 8.0)  # cable2
			self.displ_list[2].value[0] = min(self.displ_list[2].value[0] + 0.0, 0.0)  # cable3
		else:
			self.displ_list[0].value[0] = 0.0
			self.displ_list[1].value[0] = 0.0
			self.displ_list[2].value[0] = 0.0

		# print(f"Time: {self.time:.3f} s, Forces: {[d.value[0] for d in self.displ_list]}")

		# Log position every 200ms
		if self.step_counter >= self.log_interval:
			self.step_counter = 0  # Reset counter
			fingertip_position = self.Positions[self.posmax]
			print(f"Logging position: {fingertip_position}")

			self.log_file.write(
				f"{self.time:.3f} {fingertip_position[0]} {fingertip_position[1]} {fingertip_position[2]}\n"
			)
			self.log_file.flush()  # Ensure data is written immediately

		# Calculate new angle
		delta_x = self.Positions[self.posmax][0] - self.Positions[self.posmin][0]
		coeff = (self.Positions[self.posmax][2] - self.Positions[self.posmin][2]) / delta_x if delta_x != 0 else float(
			"inf")

		angle = (math.atan(coeff) * 180 / math.pi) - 90

		with open("./plot/Position_endEffector.txt", "a") as file1:
			file1.write(
				f"0.0 {self.Positions[self.posmax][0]} {self.Positions[self.posmax][1]} {self.Positions[self.posmax][2]}\n")

		with open("./plot/Length-Angle.txt", "a") as file2:
			file2.write(f"{self.time} {self.displ_list[0].value[0]} {angle}\n")

		# print(angle)

	def __del__(self):
		self.log_file.close()  # Close file when the controller is deleted


def createScene(rootNode):
	rootNode.addObject('RequiredPlugin' ,pluginName='SofaPython3')
	rootNode.addObject('RequiredPlugin', pluginName="SoftRobots")
	rootNode.addObject('RequiredPlugin', pluginName="Sofa.Component.Collision.Detection.Algorithm")
	#rootNode.addObject('RequiredPlugin', pluginName="SofaOpenglVisual")
	rootNode.addObject('RequiredPlugin', pluginName="Sofa.GL.Component.Rendering3D")
	rootNode.addObject('RequiredPlugin', pluginName="Sofa.GL.Component.Rendering2D")
	rootNode.addObject('RequiredPlugin', pluginName="Sofa.GL.Component.Shader")
	#rootNode.addObject('RequiredPlugin', pluginName='SofaSparseSolver')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.LinearSolver.Iterative')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.LinearSolver.Direct')
	#rootNode.addObject('RequiredPlugin', pluginName='SofaSparseSolver')
	#rootNode.addObject('RequiredPlugin', pluginName='SofaPreconditioner ')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.LinearSolver.Iterative')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.LinearSolver.Preconditioner')
	#rootNode.addObject('RequiredPlugin', pluginName='SofaMiscCollision ')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.Collision.Geometry')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.Collision.Detection.Intersection')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.Collision.Response.Contact')
	rootNode.addObject('RequiredPlugin', pluginName='CollisionOBBCapsule')
	rootNode.addObject('RequiredPlugin', pluginName='Sofa.Component.IO.Mesh')
	rootNode.addObject('VisualStyle', displayFlags='showVisualModels showInteractionForceFields')

	rootNode.gravity=[ 0, 0, 0]
	rootNode.dt = 0.01
	rootNode.name = 'rootNode'

	rootNode.addObject('FreeMotionAnimationLoop')
	rootNode.addObject('GenericConstraintSolver',
		name = 'consolver',tolerance=1e-7,
		maxIterations=10000, computeConstraintForces = True )

	rootNode.addObject('CollisionPipeline')
	rootNode.addObject('BruteForceBroadPhase')
	rootNode.addObject('BVHNarrowPhase')
	rootNode.addObject('CollisionResponse',
		response='FrictionContactConstraint',
		responseParams='mu= 0.05')
	rootNode.addObject('LocalMinDistance',
		name='Proximity', alarmDistance=0.5,
		contactDistance=0.1, angleCone=0.0)

	rootNode.addObject('BackgroundSetting', color=[0.1, 0.11, 0.19999999])
	rootNode.addObject('OglSceneFrame', style='Arrows',
		alignment='TopRight')


## FINGER
	finger = rootNode.addChild('finger')
	finger.addObject('EulerImplicitSolver', name='odesolver',
		rayleighStiffness=0.1, rayleighMass=0.1)
	finger.addObject('SparseLDLSolver', name='directSolver')

	finger.addObject('MeshVTKLoader', name='loader',
		filename= path + 'multi_finger_tigh.vtu')


	finger.addObject('MechanicalObject',
	 name='tetras', template='Vec3d',
	 src = '@loader', position = "@loader.position")


	finger.addObject('TetrahedronSetTopologyContainer',
		name="topo", src ='@loader')

	# finger.addObject('TetrahedronSetTopologyModifier' ,
	# 	name="Modifier", listening =True)

	finger.addObject('TetrahedronSetGeometryAlgorithms',
		template="Vec3d" ,name="GeomAlgo", listening = True)

#previous value = massDensity='1.020e-9', src = '@topo')
	finger.addObject('MeshMatrixMass',
		massDensity='0.120e-9', src = '@topo')

	mu1 =12 * 10^10 # (2 * 10 ** 8)  Reduce stiffness
	k0 = 12 * 10^10 # 2 / 1.5e-3  Adjust bulk modulus

	# finger.addObject('TetrahedronHyperelasticityFEMForceField',
	# 	template='Vec3d', name='FEM', src ='@topo',
	# 	ParameterSet=str(mu1)+' '+str(k0),materialName="NeoHookean")

	finger.addObject('BoxROI', name='boxROI',
		box=[-12,0, -80,  -2, 10, -90], drawBoxes=True)

	finger.createObject('TetrahedronFEMForceField', template='Vec3d', name='FEM',
						method='large', poissonRatio='0.3', youngModulus='18000')

	finger.createObject('UniformMass', totalMass='0.5')

	finger.addObject('RestShapeSpringsForceField',
					 points='@boxROI.indices', stiffness=1e7,
					 angularStiffness=1e7)

	## ------finger.addObject('LinearSolverConstraintCorrection', solverName='directSolver', listening =True )
	finger.addObject('LinearSolverConstraintCorrection', listening=True)

## CABLE
	# First cable (unchanged)--------------------------------------------------------------------------------------------------
	cable1 = finger.addChild('cable1')
	cable1.addObject('EulerImplicitSolver', name='odesolver',
					 rayleighStiffness=0.1, rayleighMass=0.1)

	cable1.addObject('SparseLDLSolver', name='directSolver')

	pullPointLocation = [-10.0, 5.0, -100]
	initialValue = 0.0
	valueType = 'displacement'

	cable1.addObject('MechanicalObject', name='mech',
					 position='-11 6 -73.113' + '\n' + '-11 5 -61.387' +
							  '\n' + '-11 5.5 -53.613' + '\n' + '-11 5 -41.887' +
							  '\n' + '-11 5 -34.113' + '\n' + '-11 5 -22.387' +
							  '\n' + '-11 5 -14.611' + '\n' + '-11 5 -2.885',
					 showObject=True, showObjectScale=10)

	cable1.addObject('CableConstraint', indices=[0, 1, 2, 3, 4, 5, 6, 7],
					 pullPoint=pullPointLocation, value=initialValue,
					 valueType=valueType, hasPullPoint=True,
					 listening=True, group=1)

	cable1.addObject('BarycentricMapping', name="Mapping",
					 mapForces=False, mapMasses=False)

	# CABLE 2 - Increased y by 1.0 (from -0.50 to 0.50)
	cable2 = finger.addChild('cable2')
	cable2.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.1, rayleighMass=0.1)
	cable2.addObject('SparseLDLSolver', name='directSolver')

	cable2.addObject('MechanicalObject', name='mech',
					 position='-5.33 1.50 -73.113\n-5.33 1.50 -61.387\n-5.33 3.17 -53.613\n-5.33 1.50 -41.887\n-5.33 1.50 -34.113\n-5.33 1.50 -22.387\n-5.33 1.50 -14.611\n-5.33 1.50 -2.885',
					 showObject=True, showObjectScale=10)

	cable2.addObject('CableConstraint', indices=[0, 1, 2, 3, 4, 5, 6, 7],
					 pullPoint=[-5.33, 1.50, -100], value=0.0, valueType='displacement',
					 hasPullPoint=True, listening=True, group=2)
	cable2.addObject('BarycentricMapping', name="Mapping", mapForces=False, mapMasses=False)

	# CABLE 3 - Decreased x by 0.40 (from -5 to -5.4) and increased y by 0.40 (from 9 to 9.4)
	cable3 = finger.addChild('cable3')
	cable3.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.1, rayleighMass=0.1)
	cable3.addObject('SparseLDLSolver', name='directSolver')

	cable3.addObject('MechanicalObject', name='mech',
					 position='-5.4 8.4 -73.113\n-5.4 8.4 -61.387\n-5.4 8.7 -53.613\n-5.4 8.4 -41.887\n-5.4 8.4 -34.113\n-5.4 8.4 -22.387\n-5.4 8.4 -14.611\n-5.4 8.4 -2.885',
					 showObject=True, showObjectScale=10)

	cable3.addObject('CableConstraint', indices=[0, 1, 2, 3, 4, 5, 6, 7],
					 pullPoint=[-5.4, 8.4, -100], value=0.0, valueType='displacement',
					 hasPullPoint=True, listening=True, group=3)

	cable3.addObject('BarycentricMapping', name="Mapping", mapForces=False, mapMasses=False)

	rootNode.addObject(FingerController(
		node=rootNode,
		position=rootNode.finger.tetras.position.value,
		displacements=[
			rootNode.finger.cable1.CableConstraint,
			rootNode.finger.cable2.CableConstraint,
			rootNode.finger.cable3.CableConstraint
		]
	))

	## VISUAL MODEL FINGER
	modelVisu1 = finger.addChild('visu')
	modelVisu1.addObject('MeshOBJLoader', name='loader', filename=path + 'multi_finger_tigh.obj')
	modelVisu1.addObject('OglModel', src='@loader', color=[0.98, 0.70, 0.70, 100])
	modelVisu1.addObject('BarycentricMapping')

	## COLLISION MODEL FINGER
	collisionFinger = finger.addChild('collisionFinger')
	collisionFinger.addObject('MeshOBJLoader', name='loader',
							  filename=path + 'Tendon_Driven_Actuator.obj')
	collisionFinger.addObject('MeshTopology', src='@loader', name='topo')
	collisionFinger.addObject('MechanicalObject', name='collisMech',
							  position='@loader.position')
	collisionFinger.addObject('TriangleCollisionModel', selfCollision=False)
	collisionFinger.addObject('LineCollisionModel', selfCollision=False)
	collisionFinger.addObject('PointCollisionModel', selfCollision=False)
	collisionFinger.addObject('BarycentricMapping')

	## FLOOR
	planeNode = rootNode.addChild('Plane')

	planeNode.addObject('MeshOBJLoader', name='loader',
						filename=path + 'floorFlat.obj',
						rotation=[-90, -90, 0],
						scale=5, translation=[-30, 0, 5])
	planeNode.addObject('MeshTopology', src='@loader')
	planeNode.addObject('MechanicalObject', src='@loader')
	planeNode.addObject('TriangleCollisionModel')
	planeNode.addObject('LineCollisionModel')
	planeNode.addObject('PointCollisionModel')
	planeNode.addObject('OglModel', name='Visual',
						src='@loader', color=[0.73, 0.04, 0.02, 1])


	return rootNode

[bakpaul]

Hello,

There are plently of ways, but you are throwing a random code with your question without explanation of what it does and what doesn't suits you in your code. Could you please explain so I can advice you the best way possible ?

[mohammad79sss]

Hi mate. you right. i just wanted to know is there a way for tracking an object which moves in SOFA or not. since you know some ways, if you could, provide me email or whatsapp or anything else and i will provide you full version of code, document and pictures of running program. if it is not possible tell me to continue our discussion here.

[hugtalbot]

Hi @mohammad79sss

Going through your script, an easy way to track the position (degrees of freedom) of your object would be to print the value of self.Positions[idX] where idX is the index of the point you are aiming at tracking.

Let me know if this works
Cheers