Best Practices for Real-Time State Synchronization of a Deformable Object with Vision Data？

[sodasoda520]

Hello SOFA Community,

I am currently working on a research project involving the robotic manipulation of a flexible object. My goal is to create a "physics-in-the-loop" control system where a real-world robot's actions are guided by a SOFA simulation.

My setup consists of:

1. A real-world robotic manipulator.
2. A 3D vision system that can track the real-time coordinates of a few key feature points on the surface of the flexible object.
3. A parallel SOFA simulation with an FEM model of the same object.

The intended control loop is as follows:

• Step 1 (Perception): The vision system captures the current 3D positions of the feature points on the real object.
• Step 2 (Synchronization): The state of the simulated object in SOFA is updated to accurately match the observed shape of the real object.
• Step 3 (Planning): A control action is computed within the SOFA simulation based on this corrected state.
• Step 4 (Actuation): The computed action is sent to the real robot, which then manipulates the object.

In my simulation, the flexible plate is fixed at one end using a FixedConstraint. The manipulation happens at the opposite end. To achieve a physically realistic grasp, I have defined a small region of nodes at the manipulated end and "rigidified" them.

The control input is applied directly to this 6-DOF rigid body. Specifically, in each control step, I compute a small pose increment (a 3D translation vector Δx and a 3D rotation vector Δθ). I then update the rigid body's position and orientation (as a quaternion) in its MechanicalObject.

My main challenge lies in Step 2: The Synchronization. I have the sparse 3D coordinates of the feature points from my vision system, but I need a robust and efficient method to update the entire state (i.e., the positions of all nodes) of my FEM model in SOFA to reflect this real-world measurement. The updated state must be physically plausible.

I am reaching out to the community to ask for advice on the best practices or recommended strategies for tackling this kind of problem within the SOFA framework. Specifically:

• Are there standard SOFA components or design patterns that are well-suited for this type of "data assimilation" or real-time state estimation for deformable objects?
• What is the generally accepted approach?

Any guidance, pointers to relevant examples, documentation would be immensely helpful.

Thank you for your time and expertise.

[bakpaul]

HEllo ! Sorry for the late reply.

I think that what you are trying to do is very close to what has been done in the following paper : https://hal.science/hal-01745625v1
It uses two simulation, one for direct simulation (this is the one doing the fusion of the sensor and the simulation) and the inverse one, using the result of the direct one to initialize the robot configuration then compute the control.

To do this it solves two QP Problems, one in the direct simulation to find the right actuator positioning to match the input data, and then a second one to compute actuator displacement to reduce the interposition error of the EEF w/r to a goal. Everything that is used here is open source and available in SofaRobots and SoftRobots.Inverse plugins.

If you have any question regarding the paper, don't hesitate to contact the authors.

Hope that helps !

[github-actions[bot]]

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