Solving the magnitude and direction of contact force during the compression process of rigid and elastic objects

[zhanglepy]

Solving the magnitude and direction of contact force during the compression process of rigid and elastic objects

code:

import Sofa.Core
import Sofa.SofaDeformable

import numpy as np
from typing import Optional, List

from components.tetrahedral import Topology, add_collision_models, add_loader, add_tetrahedral_forcefield, add_topology, add_visual_models

class Probe(Sofa.Core.Controller):
    """ Sofa Controller representing an ultrasound probe """
    def __init__(
        self,
        root_node: Sofa.Core.Node,
        visual_filename: Optional[str] = None,
        collision_filename: Optional[str] = None,
        node_name: str = "Probe",
        init_pose: list = [0,0,0,0,0,0,1],
        target_position: np.ndarray = np.zeros((1,3)),
        velocity: float = 0.01
        ):
        """ 
        Args:
            root_node (Sofa.Core.Node): root node of the simulation.
            visual_filename (str, Optional): name of the triangular surface file used for visualization.
            collision_filename (str): name of the triangular surface file used for collision computation.
            node_name (str): name of the created node.
            init_pose (list): initial position and orientation of the probe, expressed as [x, y, z, qx, qy, qz, qw]
            target_position (list): target position to reach in case predefined motion is performed (in probe reference frame), expressed as [x, y, z]
        """

        Sofa.Core.Controller.__init__(self)
        
        self.init_pose = init_pose
        self.current_pose = None
        self.motion_path = []
        self.velocity = velocity
        self.root_node = root_node

        self.probe_node = root_node.addChild(node_name)
        self.root_node.addObject('GenericConstraintSolver', name="GCS", maxIt=1000, tolerance=1e-6,
                           computeConstraintForces=True)
        self.state = self.probe_node.addObject('MechanicalObject',
                                            name='probe_state',
                                            position=init_pose,
                                            template="Rigid3d",
                                            showObject=1,
                                            showObjectScale=0.01,
                                            listening=1
                                            )

        # add_solver( parent_node=probe_node, add_constraint_correction=False )
        # probe_node.addObject('UniformMass',
                        # totalMass=0.1,
                        # name='probe_mass'
                        # )
        # probe_node.addObject('UncoupledConstraintCorrection')

        # Add collision models
        probe_collision = self.probe_node.addChild(f"{node_name}Collision")
        collision_loader = add_loader( parent_node=probe_collision,
                                         filename=collision_filename,
                                         name='probe_collision_loader',
                                        )

        add_topology( parent_node=probe_collision, 
                      mesh_loader=collision_loader,
                      topology=Topology.TRIANGLE
                    )

        self.collision_state = probe_collision.addObject('MechanicalObject',
                                name='probe_state',
                                template='Vec3d',
                                showObject=1,
                                showObjectScale=3,
                                listening=1
                                )
        
        add_collision_models( parent_node=probe_collision) #, spheres=True, radius=0.002 )
        probe_collision.addObject('RigidMapping', name='CollisionMapping')

        # Add visual models
        visual_node = self.probe_node.addChild(f"Visual{node_name}")
        visual_loader = add_loader( parent_node=visual_node,
                                    filename=visual_filename,
                                    name='probe_visual_loader',
                                    )

        add_topology( parent_node=visual_node, 
                      mesh_loader=visual_loader,
                      topology=Topology.TRIANGLE
                    )

        add_visual_models( parent_node = visual_node,
                            color = [0.957, 0.730, 0.582, 0.99]
                           )

        visual_node.addObject('RigidMapping', name='VisualMapping') #, input=, output=)

        # Create probe motion
        self.target_position = target_position
        self.num_deformations = target_position.shape[0]
        print(f"Number of deformations to apply: {self.num_deformations}")
        self.current_def = 1
        if self.num_deformations > 1:
            target_position = target_position[self.current_def-1,:]
        self.motion_path = self.create_linear_motion( target_position=target_position, dt=root_node.dt.value, velocity=self.velocity)


        print("0000")
        # probe_node.addObject(drawForces(name="PythonController", rootNode=probe_node))
        print("11111")
        # 创建一个新的子节点 'drawNode' 用于可视化
        draw_node = self.probe_node.addChild('drawNode')

        # 获取现有的 MechanicalObject 对象（在 Cube 节点中的 collision 子节点中）
        mech_object = self.collision_state

        # 获取自由度的总数
        nb_dofs = len(mech_object.position.value)  # 假设 position 是存储节点位置信息的数组
        print("Total number of DOFs:", nb_dofs)

        # 在 draw_node 中添加一个新的 MechanicalObject，用于显示位置
        draw_node.addObject('MechanicalObject', name="drawPositions",
                            position=mech_object.position, size=nb_dofs)

        # 添加 ConstantForceField 用于显示力的箭头
        draw_node.addObject('ConstantForceField', name="drawForceFF", force=[0, 0, 0], showArrowSize=1)

    #########################################################
	####### SOFA-RELATED
	#########################################################

    def onAnimateBeginEvent(self, __):
        if len(self.motion_path):  # if there is a motion path, execute one step
            self.current_def_ended = False
            new_pose = np.append(self.motion_path.pop(0), self.init_pose[3:])
            self.state.position.value = [new_pose]
        elif self.current_def <= self.num_deformations:
            self.current_def_ended = True
            print(f"End of deformation {self.current_def}")
            self.current_def += 1
            if self.current_def <= self.num_deformations:
                target_position = self.target_position[self.current_def-1,:]
                self.motion_path = self.create_linear_motion( target_position=target_position, dt=self.root_node.dt.value, velocity=self.velocity)


        constraint = self.collision_state.constraint.value
        dt = self.root_node.dt.value
        print(dt)
        constraintMatrixInline = np.fromstring(constraint, sep='  ')
        print('constraintMatrixInline: ', len(constraintMatrixInline))
        pointId = []
        constraintDirections = []
        index = 0
        i = 0
        forcesNorm = self.root_node.GCS.constraintForces.value
        print('forcesNorm: ', len(forcesNorm))
        contactforce_x = 0
        contactforce_y = 0
        contactforce_z = 0
        constraintDirections = np.zeros((1, 1))
        if len(constraintMatrixInline) > 0:
            constraintDirections = np.zeros((int(len(constraintMatrixInline) / 6), 3))
        while index < len(constraintMatrixInline):
            nbConstraint = constraintMatrixInline[index + 1]
            pointId = np.append(pointId, constraintMatrixInline[index + 2])
            constraintDirections[i][0] = constraintMatrixInline[index + 3]
            constraintDirections[i][1] = constraintMatrixInline[index + 4]
            constraintDirections[i][2] = constraintMatrixInline[index + 5]
            index = index + 6
            i = i + 1
        nbDofs = len(self.collision_state.position.value)
        print('nbDofs', nbDofs)
        forces = np.zeros((nbDofs, 3))
        print('pointid num', len(pointId))
        for i in range(int(len(constraintMatrixInline) / 6)):
            indice = int(pointId[i])
            forces[indice][0] = forces[indice][0] + constraintDirections[i][0] * forcesNorm[i] / dt
            forces[indice][1] = forces[indice][1] + constraintDirections[i][1] * forcesNorm[i] / dt
            forces[indice][2] = forces[indice][2] + constraintDirections[i][2] * forcesNorm[i] / dt
            print('indice', i, indice)

        for i in range(nbDofs):
            contactforce_x += forces[i][0]
            contactforce_y += forces[i][1]
            contactforce_z += forces[i][2]
        # print('nbDof', nbDofs)
        print('force', forces)
        print('contactforce', contactforce_x, contactforce_y, contactforce_z)
        # if len(constraintMatrixInline) > 0:
        #     self.rootNode.drawNode.drawForceFF.indices.value = list(range(0, nbDofs, 1))
        #     self.rootNode.drawNode.drawForceFF.forces.value = forces
        #     self.rootNode.drawNode.drawPositions.position.value = self.collision_state.position.value


    #########################################################
	####### CUSTOM
	#########################################################
    def create_linear_motion(self, target_position: np.ndarray, dt: float, velocity: float, single_step: bool = False, start_position: Optional[np.ndarray] = None) -> List[np.ndarray]:
        """
        Creates movement path to displace probe from its current position to the final position provided, at the given velocity.
        """

        if start_position is None:
            current_position = self.state.position.value[0]
            # only use xyz
            current_position = current_position[:3]
        else:
            assert len(start_position) == 3
            current_position = start_position

        displacement = target_position - current_position
        print(f"Now moving to {target_position}, from {current_position}")

        motion_path = []

        if single_step:
            motion_path = [target_position]
            motion_steps = 1
        else:
            displacement_per_step = velocity * dt
            motion_steps = int(np.ceil(np.linalg.norm(displacement) / displacement_per_step))

            progress = np.linspace(0.0, 1.0, motion_steps + 1)[1:]
            motion_path = current_position + displacement * progress[:, np.newaxis]

            motion_path[-1] = target_position

        return np.split(motion_path, motion_steps, axis=0)

Among them, forcesNorm=self. root_node GCS-ConstraintForces.value cannot be obtained. I set self. root_node. addObject ('GenericConstraintSolver ', name="GCS") earlier, maxIt=1000, tolerance=1e-6,computeConstraintForces=True)。

output:
0.02
constraintMatrixInline: 950
forcesNorm: 0
[ERROR] [Probe(unnamed)] Unable to execute code.
Python exception:
IndexError: index 950 is out of bounds for axis 0 with size 950
probe-tissue-0904.zip

At:
G:\SOFA_codes\probe-tissue-0901\probe-tissue-simulation-sofapython3\objects\probe.py(166): onAnimateBeginEvent
G:\SOFA_codes\probe-tissue-0901\probe-tissue-simulation-sofapython3\simulation.py(166):
At:
G:\SOFA_codes\probe-tissue-0901\probe-tissue-simulation-sofapython3\simulation.py(166):

Looking forward to your reply, thank you！

[zhanglepy]

probe-tissue-0904.zip
This is the source code, which contains all the files.

[hugtalbot]

Hi @zhanglepy

could you elaborate what is the question exactly please?
Cheers

[zhanglepy]

Hello @hugtalbot
Thank you for your prompt reply!
The main issue is to use forcesForm=self. root_node GCS.constraintForces.value. The print ('forcesForm: ', len (forcesForm)) yields len (forcesForm) which is not equal to len (constraint Matrix Online)/6, for example len (constraint Matrix Online)=950
Len (forcesNorm)=0, which is clearly incorrect, but I don't know why?

[bakpaul]

Hello ! In the scene that you sent, you are not using the Probe but the BreastProbe (in the simulation.py file). Is that correct ?
If not, could you please help us help you by guiding us on what you are exactly launching in all the unsorted files that you sent us ?

[zhanglepy]

Hello,
You're right, you can run the simulation. py file directly in PyCharm to run the entire project. I think the main difficulty of this project is that the complex procedures involved in the contact between rigid and flexible objects are much larger than those involved in rigid contact. However, previous discussions on the contact between rigid and flexible objects seem to be incomplete, so I have not yet found the most direct and accurate answer. Thank you for your prompt reply!

[bakpaul]

If it is the simulation.py file that you are using, then I can see a problem in there : the GenericConstraintSolver si missing the data computeConstraintForces=True

If not, please be more clear about your question. You put a totally different script in your question than in the sources you sent. I just don't know anymore which file I should look at.

[github-actions[bot]]

⚠️ ⚠️ ⚠️
@zhanglepy
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