Vision/FEM-based dynamic model

<h1>Vision/FEM-based dynamic model</h1> - Reference: [Fonkoua et al., Deformation Control of a 3D Soft Object Using RGB-D Visual Servoing and FEM-Based Dynamic Model, IEEE Robotics and Automation Letters, 2024.](https://hal.science/hal-04622538) - Post author: [Jedd Yang](https://hackmd.io/@jeddiot) - Post written date: 2025-Oct-22 - Keywords: Deformation control, physics-based model, visual servoing <h2>Background</h2> <div class="section-box"> <h3>Background</h3> Autonomous manipulation of deformable objects draws attention by application in various fields, such as surgical robotics and manufacturing robotics. Deformable objects, contrast to rigid objects, exhibit infinite degrees of freedom and highly nonlinear dynamics, introducing significant challenges. There are two ways of deformation controld of soft bodies: (1) Model-free and (2) model-based approaches. <h4>Model-free deformation control</h4> Control problem is formulated in sensor measurement space; machine learning methods map the sensor measurements to the control velocities of the robot end-effectors. [MPC](https://www.mathworks.com/help/mpc/gs/what-is-mpc.html) then executes the deformation task. > [!Tip] Pros > Identification of the physical parameters of the manipulated object not needed. >[!Caution] Cons > 1. Heavily depends on the accuracy of initial estimates for the interaction. > 2. Potential issues of losing generality or overtraining in specific situations. > 3. Sensitive to measurement noise and lack the capability to guarantee beforehand the feasibility of the desired deformation. <h4>Model-based deformation control</h4> 1. Physics-based model > - Wada et al. ([IROS 1998](https://ieeexplore.ieee.org/document/727484), [ICRA 2001](https://ieeexplore.ieee.org/document/932534)): MSM (Mass Spring Model) + PID control, valid only for small deformations and planar motion. > - Makiyeh et al. ([ICRACV 2022](https://ieeexplore.ieee.org/document/10004285)): Indirectly position a single point on a soft object by controlling only the translations of the robot endeffector. > - Kinio et al. ([ROBIO 2012](https://scispace.com/papers/a-comparative-study-of-h-and-pid-control-for-indirect-o2yfzumfld)): $H_{\infty}$ controller + FEM model. > - Ficuciello et al. ([IROS 2018](https://ieeexplore.ieee.org/document/8593512)) & Duenser et al. ([ICRA 2021](https://ieeexplore.ieee.org/document/8594291)) : Formulated problem as optimization problem using FEM as well. Result being open-looped, reduced robustness. > - Koessler et al. ([ICRA 2021](https://ieeexplore.ieee.org/document/9560919)): Model-based controller with visual feedback, indirectly position points belonging to a soft linear object. The quasi-static hypothesis limits the motion speed. > - Fason et al. ([IROS 2018](https://ieeexplore.ieee.org/document/5650163)): [RKPM](https://www.sciencedirect.com/science/article/abs/pii/S0045782596010833) for 2D motion, limited to planar motion. 2. Geometric approaches: only take into account the shape of the object and can not capture complex behaviors related to the object properties. --- <h3>Preliminary Knowledge</h3> - Deformation Jacobian - Control --- <h3>Contribution</h3> *"A vision-based and model-based deformation control framework."* - New analytical form of the relation between robot end effector and motion of feature points based on FEM without quasi-static hypothesis. - New control law, dynamic model forms feedback and feedforward terms. </div> <h2>Scenario</h2> <div class="section-box"> <h3>Experimental Setup</h3> - A 7-DOF Franka Emika Panda robot with its gripper rigidly attached to the soft body during manipulation. - A static RGBD Intel RealSense-D435 camera for the visual tracking of the object shape and markers located on it that represent the feature points. - ViSP library to acquire/process the RGB-D frames from the camera and to control the robot. - A FEM simulator, [SOFA](https://www.sofa-framework.org/), to obtain essential terms for control law. - A laptop with Intel CoreTM i7 CPU @ 2.70GHz × 20, leading to a control rate at 60 Hz. <center> <img src="https://hackmd.io/_uploads/BkGEFfBAxg.png" width="500"> <figcaption>Fig. 1: Experimental Setup.</figcaption> <br> </center> --- <h3>System configuration</h3> The detailed description of the points is illustrated as shown, the red part is the manipulated points to which the gripper holds on, the blue is the static/free points, the green, the feature points. <center> <img src="https://hackmd.io/_uploads/Hk-gPfH0gx.png" width="400"> <figcaption>Fig. 2: System configuration.</figcaption> <br> </center> </div> <h2>Methodology</h2> <div class="section-box"> <h3>Modeling of the Deformable Object</h3> <h4>Strong form</h4> $$ \begin{align} \mathbf{M}(\mathbf{x}) \ddot{\mathbf{x}} &= \mathbf{f}_{ext}(t) - \mathbf{f}_{int}(\mathbf{x}, \dot{\mathbf{x}}), \quad \mathbf{x} \in \mathbb{R}^{3n} \\ &= [\mathbf{f}_{b}(t) + \mathbf{f}_{c}(t) + \mathbf{f}_{a}(t))] - \mathbf{f}_{int}(\mathbf{x}, \dot{\mathbf{x}}) \end{align} \tag{1} $$ where $\mathbf{f}_{b}(t)$ stands for known external force, $\mathbf{f}_{c}(t)$, the external force due to known constraints; $\mathbf{f}_{a}(t)$, the actuator force. <h4>Quasi-static case (In literature review)</h4> The zero net force condition, $$ \mathbf{f}_{ext} - \mathbf{f}_{int} = 0 \tag{2} $$ By linear Taylor expansion on internal force, then apply the quasi-static condition. We can deduce a linear relationship: $$ \begin{align} \dot{\mathbf{x}}_f &= -\mathbf{A}_{f}^{-1} \mathbf{A}_{m} \dot{\mathbf{x}}_{m} \\ &= -[\mathbf{A}_{ff} - \mathbf{A}_{fo}\mathbf{A}_{oo}^{-1}\mathbf{A}_{of}]^{-1} [\mathbf{A}_{fm} - \mathbf{A}_{fo}\mathbf{A}_{oo}^{-1}\mathbf{A}_{om}] \dot{\mathbf{x}}_{m} \end{align} \tag{3} $$ where $\mathbf{A}_{ij} = \mathbf{S}_i \mathbf{A}_s \mathbf{S}_j^T$ with $\mathbf{A}_s = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}$ and $i, j \in \{f,m,o\}$ that denotes the feature, manipulated, and other points. The $\mathbf{S}^T_i$ represents selection matrix. <h4>Dynamic case (Proposed)</h4> $$ \begin{align} d\dot{\mathbf{x}}_f &= -\mathbf{A}_{f}^{-1} \mathbf{A}_{m} d\dot{\mathbf{x}}_{m} + \mathbf{A}_{f}^{-1}(\mathbf{b}_{f} - \mathbf{A}_{fo}\mathbf{A}_{oo}^{-1}\mathbf{b}_{o}) \end{align} \tag{4} $$ Since $d\dot{\mathbf{x}}_f = \dot{\mathbf{x}}_f^{t+h} - \dot{\mathbf{x}}_f^{t}$ and $d\dot{\mathbf{x}}_m = \dot{\mathbf{x}}_m^{t+h} - \dot{\mathbf{x}}_m^{t}$, $$ \dot{\mathbf{x}}_f^{t+h} = -\mathbf{A}_{f}^{-1} \mathbf{A}_{m} \dot{\mathbf{x}}_{m}^{t+h} + \gamma \tag{5} $$ $\gamma$ is a term depending on the internal elastic forces applied on the object and on the velocities of the manipulated and feature points at the previous time-step. $$ \gamma = \mathbf{A}_{f}^{-1}(\mathbf{b}_{f} - \mathbf{A}_{fo}\mathbf{A}_{oo}^{-1}\mathbf{b}_{o} + \mathbf{A}_{m}\dot{\mathbf{x}}_{m}^{t}) + \dot{\mathbf{x}}_{f}^{t} \tag{6} $$ <h3>Robotics Control</h3> The velocity of a manipulated point $$ \dot{\mathbf{x}}_{P_i} = \mathbf{v}_G - (\mathbf{x}_{P_i} - \mathbf{x}_G) \times \omega_G \tag{7} $$ where $P_i \in \mathcal{A}_m$ is a point in the set of manipulated points, $G$ is the center of gravity of the part of the object rigidly attached to the robot gripper that is considered as a rigid body, $\mathbf{v}_G, \mathbf{\omega}_G \in \mathbb{R}^3$ are the linear and angular velocity of the robot gripper. $$ \dot{\mathbf{x}}_{m} = \begin{bmatrix} \dot{\mathbf{x}}_{P_1} \\ \dot{\mathbf{x}}_{P_2}\\ \vdots \\ \dot{\mathbf{x}}_{P_M} \end{bmatrix} = \mathbf{J} \begin{bmatrix} \mathbf{v}_{G} \\ \mathbf{\omega}_{G} \end{bmatrix} \tag{8} $$ Substituting Equation 7 to 5, we get: $$ \begin{align} \dot{\mathbf{x}}_{f} &= \mathbf{J}_D \begin{bmatrix} \mathbf{v}_{G} \\ \mathbf{\omega}_{G} \end{bmatrix} + \gamma\\ \Rightarrow \begin{bmatrix} \mathbf{v}_{G} \\ \mathbf{\omega}_{G} \end{bmatrix} &= -\mathbf{J}_D^{+} (\lambda (\mathbf{x}_{f} - \mathbf{x}_{f}^{*}) + \gamma) \end{align} \tag{9} $$ where $\mathbf{J}_D = -\mathbf{A}_{f}^{-1} \mathbf{A}_{m} \mathbf{J}$ is the deformation Jacobian, $\mathbf{J}_D^{+}$ is its pseudo-inverse, $\lambda$, a positive gain, $\mathbf{x}_f^{*}$, the desired 3D position of the feature points $\mathbf{x}_f$. >[!Tip] Combination of Computational Mechanics and Robotics! > With Equation 9, we can obtain the linear and angular velocity of the robot arm gripper by deformation Jacobian obtained from the simulator. <center> <img src="https://hackmd.io/_uploads/HkzEFGHAgl.png" width="4=600"> <figcaption>Fig. 3: Block diagram of the approach.</figcaption> <br> </center> </div> <h2>Result</h2> <div class="section-box"> <center>  <iframe width="560" height="315" src="https://www.youtube.com/embed/-6V4tlMInqo?si=dFo8Ztnmd4LoAu3l" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe> <br> <em>Vid. 1: Demo of the proposed framework by Fonkoua et al.</em> </center> </div>