From c2ff60e630ede80e593ff4a00b6a679d49837545 Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Mon, 18 Aug 2025 08:59:20 -0700
Subject: [PATCH] Decrease time step in shell examples.

Also add bending damping.

PiperOrigin-RevId: 796446515
Change-Id: I4744003caf6146e5e00614a6016d1a6dfc42cca7
---
 model/flex/basket.xml       |  6 +++---
 model/flex/poncho.xml       |  2 +-
 src/engine/engine_passive.c | 36 +++++++++++++++++++++++-------------
 3 files changed, 27 insertions(+), 17 deletions(-)

diff --git a/model/flex/basket.xml b/model/flex/basket.xml
index 8dc15fe3..56af2b1c 100644
--- a/model/flex/basket.xml
+++ b/model/flex/basket.xml
@@ -18,7 +18,7 @@
 
   <compiler autolimits="true"/>
 
-  <option solver="CG" tolerance="1e-6"/>
+  <option solver="CG" tolerance="1e-6" timestep="0.001"/>
 
   <size memory="10M"/>
 
@@ -30,8 +30,8 @@
     <flexcomp type="box" count="8 8 8" spacing=".1 .1 .1" pos="0 0 1"
               radius=".01" rgba=".68 .53 .38 1" name="box" dim="2" mass="0.25">
       <contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
-      <edge equality="true" damping="10"/>
-      <elasticity young="6e6" poisson="0.2" thickness="8e-3" elastic2d="bend"/>
+      <edge equality="true" damping="1"/>
+      <elasticity young="6e6" poisson="0.2" thickness="8e-3" elastic2d="bend" damping="1e-5"/>
     </flexcomp>
 
     <replicate count="3" offset=".2 0 0">
diff --git a/model/flex/poncho.xml b/model/flex/poncho.xml
index c2462e56..8e6e2e80 100644
--- a/model/flex/poncho.xml
+++ b/model/flex/poncho.xml
@@ -15,7 +15,7 @@
 
 <mujoco model="Poncho">
   <include file="mannequin.xml"/>
-  <option timestep="0.01" integrator="implicitfast" viscosity=".3" solver="CG" tolerance="1e-6">
+  <option timestep="0.005" integrator="implicitfast" viscosity=".3" solver="CG" tolerance="1e-6">
     <flag energy="enable"/>
   </option>
 
diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c
index ae5287a7..87e22774 100644
--- a/src/engine/engine_passive.c
+++ b/src/engine/engine_passive.c
@@ -152,20 +152,29 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
         frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]);
         frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]);
 
-        // force
-        mjtNum force[12] = {0};
-        for (int x = 0; x < 3; x++) {
-          for (int i = 0; i < 4; i++) {
-            for (int j = 0; j < 4; j++) {
-              // thin plate bending force
-              force[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
-            }
-            // curved reference contribution
-            force[3*i+x] += b[17*e+16] * frc[i][x];
-          }
+        // velocities
+        mjtNum* vel[4];
+        for (int i = 0; i < 4; i++) {
+          vel[i] = d->qvel + m->body_dofadr[bodyid[v[i]]];
         }
 
-        // TODO: add damping
+        // force
+        mjtNum spring[12] = {0};
+        mjtNum damper[12] = {0};
+        for (int i = 0; i < 4; i++) {
+          for (int x = 0; x < 3; x++) {
+            for (int j = 0; j < 4; j++) {
+              // thin plate bending force
+              spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
+
+              // thin plate damping force
+              // TODO: do not assume DOFs are in the world frame
+              damper[3*i+x] += b[17*e+4*i+j] * vel[j][x];
+            }
+            // curved reference contribution
+            spring[3*i+x] += b[17*e+16] * frc[i][x];
+          }
+        }
 
         // insert into global force
         for (int i = 0; i < 4; i++) {
@@ -173,7 +182,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
           int body_dofnum = m->body_dofnum[bid];
           int body_dofadr = m->body_dofadr[bid];
           for (int x = 0; x < body_dofnum; x++) {
-            d->qfrc_spring[body_dofadr+x] -= force[3*i+x];
+            d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
+            d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
           }
         }
       }