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

[github-actions[bot]]

⚠️ ⚠️ ⚠️
@mohammad79sss
Feedback has been given to you by the project reviewers, however we have not received a response from you. Without further news in the coming weeks, this discussion will be automatically closed in order to keep this forum clean and fresh 🌱 Thank you for your understanding

[github-actions[bot]]

⚠️ ⚠️ ⚠️
@mohammad79sss
In accordance with our forum management policy, the last reply is more than 4 months old and is therefore closed. Our objective is to keep the forum up to date and offer the best support experience.

Please feel free to reopen it if the topic is still active by providing us with an update. Please feel free to open a new thread at any time - we'll be happy to help where and when we can.