Dynamic Applied force extraction and visualization #5354
Replies: 6 comments 3 replies
-
|
@Zentetsu When I modify it to apply my case, I found that nb_constraints can be larger than 3. (nb_constraints > 3) above for loop occurs runtime error due to index out of problem because forces dimension is nb_dofs by 3. Could you please provide information about what constraint_list and forces_norm represent? It would really help me with the implementing. |
Beta Was this translation helpful? Give feedback.
-
|
@Zentetsu Through modification, I success to get contact force! Thank you! |
Beta Was this translation helpful? Give feedback.
-
|
@SangbaekYoo So, if I understood correctly and it seems you solved it, it's because you can have different collision models simultaneously defined on your object. |
Beta Was this translation helpful? Give feedback.
-
Regarding my two first points: Constraint ID
|
Beta Was this translation helpful? Give feedback.
-
|
Hello ! Regarding your "UPDATE - 13/06": How did you identify the constraint id of all collision model pairs ? |
Beta Was this translation helpful? Give feedback.
-
|
To identify the constraint ID, I use the ContactListener. Then, I check the intersection between points and triangles to determine which points need to be considered in the force calculation. Here is another snippet of code that contains snippetdef updateForceMatrix(
self,
forces_matrix: list,
forces_dir: list,
constraints: list,
f_norm: float,
dt: float,
dof_id: int,
) -> None:
"""Update the force matrix to fit the number of degrees of freedom.
Args:
forces_matrix: The force matrix to update.
forces_dir: The force direction matrix to update.
constraints: The constraints applied to the force matrix.
f_norm: The norm of the force.
dt: The time step for the update.
dof_id: The degree of freedom ID.
"""
forces_matrix[dof_id][0] += constraints[0] * f_norm / dt
forces_matrix[dof_id][1] += constraints[1] * f_norm / dt
forces_matrix[dof_id][2] += constraints[2] * f_norm / dt
vector = np.array(forces_matrix[dof_id])
magnitude = np.linalg.norm(vector)
if magnitude == 0:
f_dir = np.zeros_like(vector)
else:
f_dir = vector / magnitude
forces_dir[dof_id] = [5.0 * v for v in f_dir.tolist()]
return forces_matrix, forces_dir
def getForceMatrix(
self, constraint: sparray, forces_norm: list, indice_contact: list, dt: float
) -> tuple:
"""Get the force matrix from the constraints.
Args:
constraint: The constraint object.
forces_norm: The forces norm.
indice_contact: The indices of the contact points.
dt: The time step.
"""
constraint_list = list(constraint.todok().keys())
grouped = defaultdict(list)
for x, y in constraint_list:
grouped[x].append((x, y))
constraint_list = list(grouped.values())
forces_matrix = np.zeros((0, 3))
forces_dir = np.zeros((0, 3))
dofs = set([])
for i in range(len(constraint_list)):
c = constraint_list[i]
# print("constraint ID", c[0][0], "nb constraints", len(c), end=" ")
if len(c) == 3:
dof = int(c[0][1] / 3)
if dof not in indice_contact:
# print(" PCM " + str(dof) + " ")
continue
# print("pointCollisionModel; DOFs", dof)
dofs.add(dof)
forces_matrix, forces_dir = self.resizeForceMatrix(
forces_matrix, forces_dir, dof
)
forces_matrix, forces_dir = self.updateForceMatrix(
forces_matrix,
forces_dir,
[
float(constraint.todok()[c[0]]),
float(constraint.todok()[c[1]]),
float(constraint.todok()[c[2]]),
],
forces_norm[c[0][0]],
dt,
dof,
)
elif len(c) == 6:
# continue
if (
int(c[0][1] / 3) not in indice_contact
or int(c[3][1] / 3) not in indice_contact
):
# print(
# " LCM "
# + str(int(c[0][1] / 3))
# + " "
# + str(int(c[3][1] / 3))
# + " "
# )
continue
# print("lineCollisionModel; DOFs", int(c[0][1] / 3), int(c[3][1] / 3))
dofs.add(int(c[0][1] / 3))
dofs.add(int(c[3][1] / 3))
dof = [int(c[0][1] / 3), int(c[3][1] / 3)]
forces_matrix, forces_dir = self.resizeForceMatrix(
forces_matrix, forces_dir, max((dof))
)
for d in dof:
forces_matrix, forces_dir = self.updateForceMatrix(
forces_matrix,
forces_dir,
[
float(constraint.todok()[c[0]]),
float(constraint.todok()[c[1]]),
float(constraint.todok()[c[2]]),
],
forces_norm[c[0][0]],
dt,
d,
)
elif len(c) == 9:
# continue
if (
int(c[0][1] / 3) not in indice_contact
or int(c[3][1] / 3) not in indice_contact
or int(c[6][1] / 3) not in indice_contact
):
# print(
# " TCM : "
# + str(int(c[0][1] / 3))
# + " "
# + str(int(c[3][1] / 3))
# + " "
# + str(int(c[6][1] / 3))
# )
continue
# print(
# "triangleCollisionModel; DOFs",
# int(c[0][1] / 3),
# int(c[3][1] / 3),
# int(c[6][1] / 3),
# )
dofs.add(int(c[0][1] / 3))
dofs.add(int(c[3][1] / 3))
dofs.add(int(c[6][1] / 3))
dof = [
int(c[0][1] / 3),
int(c[3][1] / 3),
int(c[6][1] / 3),
]
forces_matrix, forces_dir = self.resizeForceMatrix(
forces_matrix, forces_dir, max((dof))
)
for d in dof:
forces_matrix, forces_dir = self.updateForceMatrix(
forces_matrix,
forces_dir,
[
float(constraint.todok()[c[0]]),
float(constraint.todok()[c[1]]),
float(constraint.todok()[c[2]]),
],
forces_norm[c[0][0]],
dt,
d,
)
# print("forces_matrix", forces_matrix)
return dofs, forces_matrix, forces_dirHere is a result from a new test I made. The simulation is similar to the one before. However, only rigid cubes are used, the weight of the big cube is
Is my thinking correct? |
Beta Was this translation helpful? Give feedback.
-
|
For your information, I just created an example as a PR in the SofaPython3 repository describing how to easily access (and visualize) contact forces, when using the response method "FrictionContactConstraint". See ➡️ https://github.com/sofa-framework/SofaPython3/pull/514/files |
Beta Was this translation helpful? Give feedback.
-
|
|
Beta Was this translation helpful? Give feedback.
-
|
|
Beta Was this translation helpful? Give feedback.
Uh oh!
There was an error while loading. Please reload this page.
Uh oh!
There was an error while loading. Please reload this page.
-
Hello!
I'm working on a simulation complementary to this one.
My goal here is to be able to track and visualize the forces applied to objects (solid and deformable) and the end effector of a robotic arm.
I used this example to track the force. However, I still have some questions left.
The simulation for the moment contains two cubes (rigid and deformable) and the floor.
Constraint ID
PointCollisionModelIn the example, the constraints are display in this format with and contain 2 dofs:
In my simulation, the constraints appear in this format and contain 1 dof:
If I understood, there are 3 Constraint IDs for each object's points due to the use of
TriangleCollisionModel.However, I don't know how to deal with it. Should I sum them up or ignore some of them?
Here is the snippet of code that I made to display force:
The one for managing the collision:
And a screenshot from the simulation (one cube):
Constraint ID multi
CollisionModelI'm also concerned when the objects use multiple
CollisionModel(Line, Point, and Triangle)To be sure that the two cubes are able to interact with each other I add
TriangleCollisionModelandLineCollisionModel.In this case, the constraint format changes, but I'm not sure how to manage them.
Is there any specific way to deal the multiple type of constraints with two cubes when they enter in collision?
PointCollisionModelonlySimulation
Constraint data
TriangleCollisionModelLineCollisionModelPointCollisionModelSimulation
Constraint data
Deformable object
When I want to monitor the force applied to the deformable cube, multiple arrows appear with a drastically bigger scale than the one with the rigid cube. Is that normal?
PS: the total force for the rigid and deformable cube is the same
UPDATE - 06/05
CollisionModel
Concerning the management of the
collision modeland theconstraint ID, I was able to understand it a bit further.It needs to be managed in parallel with the
ContactListener, which will give information about whichcollision modeland the vertices involved in force management. All the constraint IDs should not be summed for the same object.In my use case, multiple objects can interact with each other, and it's essential to be careful when managing the
Constraint ID.Regarding my other concern about the total force
The problem came from the
UncoupledConstraintCorrection. I defined this way:The compliance made this behavior. However, I can track the force correctly by removing the
complianceargument.Still, I'm unsure how to use the
UncoupledConstraintCorrectionparameter.Deformable object
Even after solving my problem with a rigid object, I'm still concerned when it involves a deformable one.
From this example and this one, I'm still not able to figure out their differences and how to correctly extract the force applied on a deformable object.
UPDATE - 13/06
Force Measurement of deformable object
After reviewing my code and comparing it with other code examples, I was able to fix my problem when measuring the force applied to a deformable object by correcting the definition of the object:
GenericConstraintCorrectionI also changed the collision model on which I'm tracking the force applied. Before, I used the Collision model of the STL file(
MeshTopology), and now I'm using the collision model of the mesh file(TetrahedronSetTopologyContainer).In this way, I can follow the force applied to each point of my model.
Force measurement during resting phase
The setting of this test is one big deformable cube(125g) resting on the floor and a second cube(5kg) resting on the top of the first one.
The force I'm tracking seems correct, however, there is a part I'm not sure how to interpret.
The force detected on the cube's face touching the ground seems correct:
As well as the face of the second cube resting on the biggest one:
But I don't know how to interpret the force measured by the upper face of the big cube:
Simulation
Measured force output
PointCollisionModel; floorTriangleCollisionModelTriangleCollisionModel; small cubePointCollisionModelPointCollisionModel; small cubeTriangleCollisionModelTriangleCollisionModel; big cubePointCollisionModelPointCollisionModel; big cubeTriangleCollisionModelSnippet of code - UPDATED VERSION -
Beta Was this translation helpful? Give feedback.
All reactions