Use mj_rayMesh for computing an inexact SDF for mesh-SDF collisions.
PiperOrigin-RevId: 794576123 Change-Id: I3c688056a5ed617a0787ccf1f4aa683f9409040f
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
cc52d0242b
commit
dcdf0a9c5c
@@ -0,0 +1,296 @@
|
|||||||
|
v 0.740000 0.740000 -1.000000
|
||||||
|
v 0.740000 1.000000 -0.740000
|
||||||
|
v 1.000000 0.740000 -0.740000
|
||||||
|
v 0.740000 0.807269 -0.991147
|
||||||
|
v 0.740000 0.870021 -0.965154
|
||||||
|
v 0.740000 0.923848 -0.923848
|
||||||
|
v 0.807269 0.740000 -0.991147
|
||||||
|
v 0.809387 0.809958 -0.980606
|
||||||
|
v 0.808254 0.870420 -0.954318
|
||||||
|
v 0.806858 0.917583 -0.917748
|
||||||
|
v 0.870021 0.740000 -0.965154
|
||||||
|
v 0.870490 0.808646 -0.954150
|
||||||
|
v 0.863847 0.863908 -0.932116
|
||||||
|
v 0.858399 0.903670 -0.903688
|
||||||
|
v 0.890111 0.890111 -0.890111
|
||||||
|
v 0.807269 0.991147 -0.740000
|
||||||
|
v 0.870021 0.965154 -0.740000
|
||||||
|
v 0.923848 0.923848 -0.740000
|
||||||
|
v 0.740000 0.991147 -0.807269
|
||||||
|
v 0.809958 0.980606 -0.809387
|
||||||
|
v 0.870420 0.954318 -0.808254
|
||||||
|
v 0.917583 0.917748 -0.806858
|
||||||
|
v 0.740000 0.965154 -0.870021
|
||||||
|
v 0.808646 0.954150 -0.870490
|
||||||
|
v 0.863908 0.932116 -0.863847
|
||||||
|
v 0.903670 0.903688 -0.858399
|
||||||
|
v 0.991147 0.740000 -0.807269
|
||||||
|
v 0.965154 0.740000 -0.870021
|
||||||
|
v 0.923848 0.740000 -0.923848
|
||||||
|
v 0.991147 0.807269 -0.740000
|
||||||
|
v 0.980606 0.809387 -0.809958
|
||||||
|
v 0.954318 0.808254 -0.870420
|
||||||
|
v 0.917748 0.806858 -0.917583
|
||||||
|
v 0.965154 0.870021 -0.740000
|
||||||
|
v 0.954150 0.870490 -0.808646
|
||||||
|
v 0.932116 0.863847 -0.863908
|
||||||
|
v 0.903688 0.858399 -0.903670
|
||||||
|
v 0.740000 -1.000000 -0.740000
|
||||||
|
v 0.740000 -0.740000 -1.000000
|
||||||
|
v 1.000000 -0.740000 -0.740000
|
||||||
|
v 0.740000 -0.991147 -0.807269
|
||||||
|
v 0.740000 -0.965154 -0.870021
|
||||||
|
v 0.740000 -0.923848 -0.923848
|
||||||
|
v 0.807269 -0.991147 -0.740000
|
||||||
|
v 0.809387 -0.980606 -0.809958
|
||||||
|
v 0.808254 -0.954318 -0.870420
|
||||||
|
v 0.806858 -0.917748 -0.917583
|
||||||
|
v 0.870021 -0.965154 -0.740000
|
||||||
|
v 0.870490 -0.954150 -0.808646
|
||||||
|
v 0.863847 -0.932116 -0.863908
|
||||||
|
v 0.858399 -0.903688 -0.903670
|
||||||
|
v 0.890111 -0.890111 -0.890111
|
||||||
|
v 0.807269 -0.740000 -0.991147
|
||||||
|
v 0.870021 -0.740000 -0.965154
|
||||||
|
v 0.923848 -0.740000 -0.923848
|
||||||
|
v 0.740000 -0.807269 -0.991147
|
||||||
|
v 0.809958 -0.809387 -0.980606
|
||||||
|
v 0.870420 -0.808254 -0.954318
|
||||||
|
v 0.917583 -0.806858 -0.917748
|
||||||
|
v 0.740000 -0.870021 -0.965154
|
||||||
|
v 0.808646 -0.870490 -0.954150
|
||||||
|
v 0.863908 -0.863847 -0.932116
|
||||||
|
v 0.903670 -0.858399 -0.903688
|
||||||
|
v 0.991147 -0.807269 -0.740000
|
||||||
|
v 0.965154 -0.870021 -0.740000
|
||||||
|
v 0.923848 -0.923848 -0.740000
|
||||||
|
v 0.991147 -0.740000 -0.807269
|
||||||
|
v 0.980606 -0.809958 -0.809387
|
||||||
|
v 0.954318 -0.870420 -0.808254
|
||||||
|
v 0.917748 -0.917583 -0.806858
|
||||||
|
v 0.965154 -0.740000 -0.870021
|
||||||
|
v 0.954150 -0.808646 -0.870490
|
||||||
|
v 0.932116 -0.863908 -0.863847
|
||||||
|
v 0.903688 -0.903670 -0.858399
|
||||||
|
v 1.000000 0.740000 0.740000
|
||||||
|
v 0.740000 1.000000 0.740000
|
||||||
|
v 0.740000 0.740000 1.000000
|
||||||
|
v 0.991147 0.807269 0.740000
|
||||||
|
v 0.965154 0.870021 0.740000
|
||||||
|
v 0.923848 0.923848 0.740000
|
||||||
|
v 0.991147 0.740000 0.807269
|
||||||
|
v 0.980606 0.809958 0.809387
|
||||||
|
v 0.954318 0.870420 0.808254
|
||||||
|
v 0.917748 0.917583 0.806858
|
||||||
|
v 0.965154 0.740000 0.870021
|
||||||
|
v 0.954150 0.808646 0.870490
|
||||||
|
v 0.932116 0.863908 0.863847
|
||||||
|
v 0.903688 0.903670 0.858399
|
||||||
|
v 0.890111 0.890111 0.890111
|
||||||
|
v 0.740000 0.991147 0.807269
|
||||||
|
v 0.740000 0.965154 0.870021
|
||||||
|
v 0.740000 0.923848 0.923848
|
||||||
|
v 0.807269 0.991147 0.740000
|
||||||
|
v 0.809387 0.980606 0.809958
|
||||||
|
v 0.808254 0.954318 0.870420
|
||||||
|
v 0.806858 0.917748 0.917583
|
||||||
|
v 0.870021 0.965154 0.740000
|
||||||
|
v 0.870490 0.954150 0.808646
|
||||||
|
v 0.863847 0.932116 0.863908
|
||||||
|
v 0.858399 0.903688 0.903670
|
||||||
|
v 0.807269 0.740000 0.991147
|
||||||
|
v 0.870021 0.740000 0.965154
|
||||||
|
v 0.923848 0.740000 0.923848
|
||||||
|
v 0.740000 0.807269 0.991147
|
||||||
|
v 0.809958 0.809387 0.980606
|
||||||
|
v 0.870420 0.808254 0.954318
|
||||||
|
v 0.917583 0.806858 0.917748
|
||||||
|
v 0.740000 0.870021 0.965154
|
||||||
|
v 0.808646 0.870490 0.954150
|
||||||
|
v 0.863908 0.863847 0.932116
|
||||||
|
v 0.903670 0.858399 0.903688
|
||||||
|
v 1.000000 -0.740000 0.740000
|
||||||
|
v 0.740000 -0.740000 1.000000
|
||||||
|
v 0.740000 -1.000000 0.740000
|
||||||
|
v 0.991147 -0.740000 0.807269
|
||||||
|
v 0.965154 -0.740000 0.870021
|
||||||
|
v 0.923848 -0.740000 0.923848
|
||||||
|
v 0.991147 -0.807269 0.740000
|
||||||
|
v 0.980606 -0.809387 0.809958
|
||||||
|
v 0.954318 -0.808254 0.870420
|
||||||
|
v 0.917748 -0.806858 0.917583
|
||||||
|
v 0.965154 -0.870021 0.740000
|
||||||
|
v 0.954150 -0.870490 0.808646
|
||||||
|
v 0.932116 -0.863847 0.863908
|
||||||
|
v 0.903688 -0.858399 0.903670
|
||||||
|
v 0.890111 -0.890111 0.890111
|
||||||
|
v 0.740000 -0.807269 0.991147
|
||||||
|
v 0.740000 -0.870021 0.965154
|
||||||
|
v 0.740000 -0.923848 0.923848
|
||||||
|
v 0.807269 -0.740000 0.991147
|
||||||
|
v 0.809387 -0.809958 0.980606
|
||||||
|
v 0.808254 -0.870420 0.954318
|
||||||
|
v 0.806858 -0.917583 0.917748
|
||||||
|
v 0.870021 -0.740000 0.965154
|
||||||
|
v 0.870490 -0.808646 0.954150
|
||||||
|
v 0.863847 -0.863908 0.932116
|
||||||
|
v 0.858399 -0.903670 0.903688
|
||||||
|
v 0.807269 -0.991147 0.740000
|
||||||
|
v 0.870021 -0.965154 0.740000
|
||||||
|
v 0.923848 -0.923848 0.740000
|
||||||
|
v 0.740000 -0.991147 0.807269
|
||||||
|
v 0.809958 -0.980606 0.809387
|
||||||
|
v 0.870420 -0.954318 0.808254
|
||||||
|
v 0.917583 -0.917748 0.806858
|
||||||
|
v 0.740000 -0.965154 0.870021
|
||||||
|
v 0.808646 -0.954150 0.870490
|
||||||
|
v 0.863908 -0.932116 0.863847
|
||||||
|
v 0.903670 -0.903688 0.858399
|
||||||
|
v -0.740000 0.740000 -1.000000
|
||||||
|
v -1.000000 0.740000 -0.740000
|
||||||
|
v -0.740000 1.000000 -0.740000
|
||||||
|
v -0.807269 0.740000 -0.991147
|
||||||
|
v -0.870021 0.740000 -0.965154
|
||||||
|
v -0.923848 0.740000 -0.923848
|
||||||
|
v -0.740000 0.807269 -0.991147
|
||||||
|
v -0.809958 0.809387 -0.980606
|
||||||
|
v -0.870420 0.808254 -0.954318
|
||||||
|
v -0.917583 0.806858 -0.917748
|
||||||
|
v -0.740000 0.870021 -0.965154
|
||||||
|
v -0.808646 0.870490 -0.954150
|
||||||
|
v -0.863908 0.863847 -0.932116
|
||||||
|
v -0.903670 0.858399 -0.903688
|
||||||
|
v -0.890111 0.890111 -0.890111
|
||||||
|
v -0.991147 0.807269 -0.740000
|
||||||
|
v -0.965154 0.870021 -0.740000
|
||||||
|
v -0.923848 0.923848 -0.740000
|
||||||
|
v -0.991147 0.740000 -0.807269
|
||||||
|
v -0.980606 0.809958 -0.809387
|
||||||
|
v -0.954318 0.870420 -0.808254
|
||||||
|
v -0.917748 0.917583 -0.806858
|
||||||
|
v -0.965154 0.740000 -0.870021
|
||||||
|
v -0.954150 0.808646 -0.870490
|
||||||
|
v -0.932116 0.863908 -0.863847
|
||||||
|
v -0.903688 0.903670 -0.858399
|
||||||
|
v -0.740000 0.991147 -0.807269
|
||||||
|
v -0.740000 0.965154 -0.870021
|
||||||
|
v -0.740000 0.923848 -0.923848
|
||||||
|
v -0.807269 0.991147 -0.740000
|
||||||
|
v -0.809387 0.980606 -0.809958
|
||||||
|
v -0.808254 0.954318 -0.870420
|
||||||
|
v -0.806858 0.917748 -0.917583
|
||||||
|
v -0.870021 0.965154 -0.740000
|
||||||
|
v -0.870490 0.954150 -0.808646
|
||||||
|
v -0.863847 0.932116 -0.863908
|
||||||
|
v -0.858399 0.903688 -0.903670
|
||||||
|
v -1.000000 -0.740000 -0.740000
|
||||||
|
v -0.740000 -0.740000 -1.000000
|
||||||
|
v -0.740000 -1.000000 -0.740000
|
||||||
|
v -0.991147 -0.740000 -0.807269
|
||||||
|
v -0.965154 -0.740000 -0.870021
|
||||||
|
v -0.923848 -0.740000 -0.923848
|
||||||
|
v -0.991147 -0.807269 -0.740000
|
||||||
|
v -0.980606 -0.809387 -0.809958
|
||||||
|
v -0.954318 -0.808254 -0.870420
|
||||||
|
v -0.917748 -0.806858 -0.917583
|
||||||
|
v -0.965154 -0.870021 -0.740000
|
||||||
|
v -0.954150 -0.870490 -0.808646
|
||||||
|
v -0.932116 -0.863847 -0.863908
|
||||||
|
v -0.903688 -0.858399 -0.903670
|
||||||
|
v -0.890111 -0.890111 -0.890111
|
||||||
|
v -0.740000 -0.807269 -0.991147
|
||||||
|
v -0.740000 -0.870021 -0.965154
|
||||||
|
v -0.740000 -0.923848 -0.923848
|
||||||
|
v -0.807269 -0.740000 -0.991147
|
||||||
|
v -0.809387 -0.809958 -0.980606
|
||||||
|
v -0.808254 -0.870420 -0.954318
|
||||||
|
v -0.806858 -0.917583 -0.917748
|
||||||
|
v -0.870021 -0.740000 -0.965154
|
||||||
|
v -0.870490 -0.808646 -0.954150
|
||||||
|
v -0.863847 -0.863908 -0.932116
|
||||||
|
v -0.858399 -0.903670 -0.903688
|
||||||
|
v -0.807269 -0.991147 -0.740000
|
||||||
|
v -0.870021 -0.965154 -0.740000
|
||||||
|
v -0.923848 -0.923848 -0.740000
|
||||||
|
v -0.740000 -0.991147 -0.807269
|
||||||
|
v -0.809958 -0.980606 -0.809387
|
||||||
|
v -0.870420 -0.954318 -0.808254
|
||||||
|
v -0.917583 -0.917748 -0.806858
|
||||||
|
v -0.740000 -0.965154 -0.870021
|
||||||
|
v -0.808646 -0.954150 -0.870490
|
||||||
|
v -0.863908 -0.932116 -0.863847
|
||||||
|
v -0.903670 -0.903688 -0.858399
|
||||||
|
v -1.000000 0.740000 0.740000
|
||||||
|
v -0.740000 0.740000 1.000000
|
||||||
|
v -0.740000 1.000000 0.740000
|
||||||
|
v -0.991147 0.740000 0.807269
|
||||||
|
v -0.965154 0.740000 0.870021
|
||||||
|
v -0.923848 0.740000 0.923848
|
||||||
|
v -0.991147 0.807269 0.740000
|
||||||
|
v -0.980606 0.809387 0.809958
|
||||||
|
v -0.954318 0.808254 0.870420
|
||||||
|
v -0.917748 0.806858 0.917583
|
||||||
|
v -0.965154 0.870021 0.740000
|
||||||
|
v -0.954150 0.870490 0.808646
|
||||||
|
v -0.932116 0.863847 0.863908
|
||||||
|
v -0.903688 0.858399 0.903670
|
||||||
|
v -0.890111 0.890111 0.890111
|
||||||
|
v -0.740000 0.807269 0.991147
|
||||||
|
v -0.740000 0.870021 0.965154
|
||||||
|
v -0.740000 0.923848 0.923848
|
||||||
|
v -0.807269 0.740000 0.991147
|
||||||
|
v -0.809387 0.809958 0.980606
|
||||||
|
v -0.808254 0.870420 0.954318
|
||||||
|
v -0.806858 0.917583 0.917748
|
||||||
|
v -0.870021 0.740000 0.965154
|
||||||
|
v -0.870490 0.808646 0.954150
|
||||||
|
v -0.863847 0.863908 0.932116
|
||||||
|
v -0.858399 0.903670 0.903688
|
||||||
|
v -0.807269 0.991147 0.740000
|
||||||
|
v -0.870021 0.965154 0.740000
|
||||||
|
v -0.923848 0.923848 0.740000
|
||||||
|
v -0.740000 0.991147 0.807269
|
||||||
|
v -0.809958 0.980606 0.809387
|
||||||
|
v -0.870420 0.954318 0.808254
|
||||||
|
v -0.917583 0.917748 0.806858
|
||||||
|
v -0.740000 0.965154 0.870021
|
||||||
|
v -0.808646 0.954150 0.870490
|
||||||
|
v -0.863908 0.932116 0.863847
|
||||||
|
v -0.903670 0.903688 0.858399
|
||||||
|
v -0.740000 -1.000000 0.740000
|
||||||
|
v -0.740000 -0.740000 1.000000
|
||||||
|
v -1.000000 -0.740000 0.740000
|
||||||
|
v -0.740000 -0.991147 0.807269
|
||||||
|
v -0.740000 -0.965154 0.870021
|
||||||
|
v -0.740000 -0.923848 0.923848
|
||||||
|
v -0.807269 -0.991147 0.740000
|
||||||
|
v -0.809387 -0.980606 0.809958
|
||||||
|
v -0.808254 -0.954318 0.870420
|
||||||
|
v -0.806858 -0.917748 0.917583
|
||||||
|
v -0.870021 -0.965154 0.740000
|
||||||
|
v -0.870490 -0.954150 0.808646
|
||||||
|
v -0.863847 -0.932116 0.863908
|
||||||
|
v -0.858399 -0.903688 0.903670
|
||||||
|
v -0.890111 -0.890111 0.890111
|
||||||
|
v -0.807269 -0.740000 0.991147
|
||||||
|
v -0.870021 -0.740000 0.965154
|
||||||
|
v -0.923848 -0.740000 0.923848
|
||||||
|
v -0.740000 -0.807269 0.991147
|
||||||
|
v -0.809958 -0.809387 0.980606
|
||||||
|
v -0.870420 -0.808254 0.954318
|
||||||
|
v -0.917583 -0.806858 0.917748
|
||||||
|
v -0.740000 -0.870021 0.965154
|
||||||
|
v -0.808646 -0.870490 0.954150
|
||||||
|
v -0.863908 -0.863847 0.932116
|
||||||
|
v -0.903670 -0.858399 0.903688
|
||||||
|
v -0.991147 -0.807269 0.740000
|
||||||
|
v -0.965154 -0.870021 0.740000
|
||||||
|
v -0.923848 -0.923848 0.740000
|
||||||
|
v -0.991147 -0.740000 0.807269
|
||||||
|
v -0.980606 -0.809958 0.809387
|
||||||
|
v -0.954318 -0.870420 0.808254
|
||||||
|
v -0.917748 -0.917583 0.806858
|
||||||
|
v -0.965154 -0.740000 0.870021
|
||||||
|
v -0.954150 -0.808646 0.870490
|
||||||
|
v -0.932116 -0.863908 0.863847
|
||||||
|
v -0.903688 -0.903670 0.858399
|
||||||
@@ -0,0 +1,34 @@
|
|||||||
|
<mujoco>
|
||||||
|
<extension>
|
||||||
|
<plugin plugin="mujoco.sdf.torus">
|
||||||
|
<instance name="torus">
|
||||||
|
<config key="radius1" value="0.35"/>
|
||||||
|
<config key="radius2" value="0.15"/>
|
||||||
|
</instance>
|
||||||
|
</plugin>
|
||||||
|
</extension>
|
||||||
|
|
||||||
|
<option gravity="0 0 -9.81" sdf_iterations="3" sdf_initpoints="10"/>
|
||||||
|
<asset>
|
||||||
|
<mesh name="torus">
|
||||||
|
<plugin instance="torus"/>
|
||||||
|
</mesh>
|
||||||
|
<mesh name="die" file="asset/die.obj" scale="1 1 1"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<include file="scene.xml"/>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<body pos="0 .05 2.5" euler="90 0 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="sdf" mesh="torus" rgba=".8 .17 .15 1" group="1">
|
||||||
|
<plugin instance="torus"/>
|
||||||
|
</geom>
|
||||||
|
</body>
|
||||||
|
<body>
|
||||||
|
<geom type="mesh" mesh="die" euler="90 0 0" rgba="0 0 1 .2"/>
|
||||||
|
</body>
|
||||||
|
<light pos="1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||||
|
<light pos="-1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -27,7 +27,7 @@
|
|||||||
</geom>
|
</geom>
|
||||||
</body>
|
</body>
|
||||||
<body>
|
<body>
|
||||||
<geom type="mesh" mesh="mug" euler="90 0 0" rgba="0 0 1 .2"/>
|
<geom type="sdf" mesh="mug" euler="90 0 0" rgba="0 0 1 .2"/>
|
||||||
</body>
|
</body>
|
||||||
<light pos="1 0 7" dir="0 0 -1" castshadow="false"/>
|
<light pos="1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||||
<light pos="-1 0 7" dir="0 0 -1" castshadow="false"/>
|
<light pos="-1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||||
|
|||||||
@@ -14,6 +14,7 @@
|
|||||||
|
|
||||||
#include "engine/engine_collision_sdf.h"
|
#include "engine/engine_collision_sdf.h"
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
#include <mujoco/mjdata.h>
|
#include <mujoco/mjdata.h>
|
||||||
@@ -23,17 +24,13 @@
|
|||||||
#include "engine/engine_collision_primitive.h"
|
#include "engine/engine_collision_primitive.h"
|
||||||
#include "engine/engine_io.h"
|
#include "engine/engine_io.h"
|
||||||
#include "engine/engine_plugin.h"
|
#include "engine/engine_plugin.h"
|
||||||
#include "engine/engine_sort.h"
|
#include "engine/engine_ray.h"
|
||||||
#include "engine/engine_util_blas.h"
|
#include "engine/engine_util_blas.h"
|
||||||
#include "engine/engine_util_errmem.h"
|
#include "engine/engine_util_errmem.h"
|
||||||
#include "engine/engine_util_misc.h"
|
#include "engine/engine_util_misc.h"
|
||||||
#include "engine/engine_util_spatial.h"
|
#include "engine/engine_util_spatial.h"
|
||||||
|
|
||||||
|
|
||||||
#define MAXSDFFACE 1300
|
|
||||||
#define MAXMESHPNT 500
|
|
||||||
|
|
||||||
|
|
||||||
//---------------------------- interpolated sdf -------------------------------------------
|
//---------------------------- interpolated sdf -------------------------------------------
|
||||||
|
|
||||||
mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) {
|
mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) {
|
||||||
@@ -274,7 +271,18 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
|
|||||||
return oct_distance(m, x, i);
|
return oct_distance(m, x, i);
|
||||||
}
|
}
|
||||||
case mjGEOM_MESH:
|
case mjGEOM_MESH:
|
||||||
return oct_distance(m, x, i);
|
if (m->mesh_octnum[i]) {
|
||||||
|
return oct_distance(m, x, i);
|
||||||
|
} else {
|
||||||
|
mjtNum dir[3] = {-x[0], -x[1], -x[2]};
|
||||||
|
mjtNum r = mju_normalize3(dir);
|
||||||
|
mjtNum dist = mj_rayMesh(m, d, i, x, dir);
|
||||||
|
if (dist > r) {
|
||||||
|
mju_scl3(dir, dir, -1);
|
||||||
|
return -mj_rayMesh(m, d, i, x, dir);
|
||||||
|
}
|
||||||
|
return dist;
|
||||||
|
}
|
||||||
default:
|
default:
|
||||||
mjERROR("sdf collisions not available for geom type %d", type);
|
mjERROR("sdf collisions not available for geom type %d", type);
|
||||||
return 0;
|
return 0;
|
||||||
@@ -380,7 +388,16 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
|
|||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case mjGEOM_MESH:
|
case mjGEOM_MESH:
|
||||||
oct_gradient(m, gradient, x, i);
|
if (m->mesh_octnum[i]) {
|
||||||
|
oct_gradient(m, gradient, x, i);
|
||||||
|
} else {
|
||||||
|
mjtNum dir[3] = {-x[0], -x[1], -x[2]};
|
||||||
|
mjtNum r = mju_normalize3(dir);
|
||||||
|
mjtNum dist = mj_rayMesh(m, d, i, x, dir);
|
||||||
|
gradient[0] = dist > r ? 1 : -1;
|
||||||
|
gradient[1] = dist > r ? 1 : -1;
|
||||||
|
gradient[2] = dist > r ? 1 : -1;
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
mjERROR("sdf collisions not available for geom type %d", type);
|
mjERROR("sdf collisions not available for geom type %d", type);
|
||||||
@@ -493,22 +510,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4],
|
|||||||
|
|
||||||
//---------------------------- narrow phase -----------------------------------------------
|
//---------------------------- narrow phase -----------------------------------------------
|
||||||
|
|
||||||
// comparison function for contact sorting
|
|
||||||
static inline int distcmp(int* i, int* j, void* context) {
|
|
||||||
mjtNum d1 = ((mjtNum*)context)[*i];
|
|
||||||
mjtNum d2 = ((mjtNum*)context)[*j];
|
|
||||||
if (d1 < d2) {
|
|
||||||
return -1;
|
|
||||||
} else if (d1 == d2) {
|
|
||||||
return 0;
|
|
||||||
} else {
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// define distSort function for contact sorting
|
|
||||||
mjSORT(distSort, int, distcmp)
|
|
||||||
|
|
||||||
// check if the collision point already exists
|
// check if the collision point already exists
|
||||||
static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
|
static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
|
||||||
for (int i = 0; i < cnt; i++) {
|
for (int i = 0; i < cnt; i++) {
|
||||||
@@ -548,36 +549,6 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
|
|||||||
return cnt+1;
|
return cnt+1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// finds minimum of Frank-Wolfe objective
|
|
||||||
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
|
|
||||||
const mjModel* m, const mjSDF* sdf, mjData* d) {
|
|
||||||
for (int step=0; step < m->opt.sdf_iterations; step++) {
|
|
||||||
mjtNum best = 1e10, fun, s[3], grad[3];
|
|
||||||
|
|
||||||
// evaluate gradient
|
|
||||||
mjc_gradient(m, d, sdf, grad, x);
|
|
||||||
|
|
||||||
// evaluate all corners
|
|
||||||
for (int i=0; i < ncorners; i++) {
|
|
||||||
// compute sdf
|
|
||||||
fun = mju_dot3(corners + 3*i, grad);
|
|
||||||
|
|
||||||
// save argmin
|
|
||||||
if (fun < best) {
|
|
||||||
best = fun;
|
|
||||||
mju_copy3(s, corners + 3*i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// update collision point
|
|
||||||
mju_subFrom3(s, x);
|
|
||||||
mju_addToScl3(x, s, 2. / (step+2.));
|
|
||||||
}
|
|
||||||
|
|
||||||
// compute distance
|
|
||||||
return mjc_distance(m, d, sdf, x);
|
|
||||||
}
|
|
||||||
|
|
||||||
// finds minimum using gradient descent
|
// finds minimum using gradient descent
|
||||||
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
|
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
|
||||||
mjData* d, int niter) {
|
mjData* d, int niter) {
|
||||||
@@ -626,129 +597,7 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
|
|||||||
return dist;
|
return dist;
|
||||||
}
|
}
|
||||||
|
|
||||||
//---------------------------- bounding box vs sdf -------------------------------------------------
|
|
||||||
|
|
||||||
// stricter triangle collision
|
|
||||||
static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
|
|
||||||
const mjSDF* sdf, mjData* d) {
|
|
||||||
mjtNum edges[6];
|
|
||||||
mjtNum normal[3], center[3];
|
|
||||||
mjtNum v[9], cross[9], p[3];
|
|
||||||
mjtNum kDistanceScl = 10.;
|
|
||||||
|
|
||||||
// triangle normal
|
|
||||||
mju_sub3(edges+0, triangle+3, triangle);
|
|
||||||
mju_sub3(edges+3, triangle+6, triangle);
|
|
||||||
mju_cross(normal, edges, edges+3);
|
|
||||||
mju_normalize3(normal);
|
|
||||||
|
|
||||||
// fourth point
|
|
||||||
mju_scl3(p, triangle, 1./3.);
|
|
||||||
mju_addToScl3(p, triangle+3, 1./3.);
|
|
||||||
mju_addToScl3(p, triangle+6, 1./3.);
|
|
||||||
mjtNum h = -mjc_distance(m, d, sdf, p)/kDistanceScl;
|
|
||||||
mju_addToScl3(p, normal, -h);
|
|
||||||
|
|
||||||
// circumsphere center
|
|
||||||
mju_sub3(v+0, triangle+0, p);
|
|
||||||
mju_sub3(v+3, triangle+3, p);
|
|
||||||
mju_sub3(v+6, triangle+6, p);
|
|
||||||
mju_cross(cross+0, v+3, v+6);
|
|
||||||
mju_cross(cross+3, v+6, v+0);
|
|
||||||
mju_cross(cross+6, v+0, v+3);
|
|
||||||
mju_scl3(center, cross, mju_dot3(v, v));
|
|
||||||
mju_addToScl3(center, cross+3, mju_dot3(v+3, v+3));
|
|
||||||
mju_addToScl3(center, cross+6, mju_dot3(v+6, v+6));
|
|
||||||
mju_scl3(center, center, 1./(2.*mju_dot3(v, cross)));
|
|
||||||
|
|
||||||
// circumsphere radius
|
|
||||||
mjtNum r = mju_sqrt(mju_dot3(center, center));
|
|
||||||
|
|
||||||
// coordinate change
|
|
||||||
mju_addTo3(center, p);
|
|
||||||
|
|
||||||
return mjc_distance(m, d, sdf, center) < r;
|
|
||||||
}
|
|
||||||
|
|
||||||
// intersect with circumsphere of bounding box
|
|
||||||
static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
|
|
||||||
const mjtNum rotation[9], const mjModel* m,
|
|
||||||
const mjSDF* s, mjData* d) {
|
|
||||||
mjtNum candidate[3];
|
|
||||||
mjtNum r = mju_norm3(bvh+3);
|
|
||||||
|
|
||||||
mju_mulMatVec3(candidate, rotation, bvh);
|
|
||||||
mju_addTo3(candidate, offset);
|
|
||||||
|
|
||||||
// check if inside the bounding box
|
|
||||||
return mjc_distance(m, d, s, candidate) < r;
|
|
||||||
}
|
|
||||||
|
|
||||||
//---------------------------- mesh vs sdf broad phase --------------------------------------------
|
|
||||||
|
|
||||||
// tree vs sdf binary search
|
|
||||||
static void collideBVH(const mjModel* m, mjData* d, int g,
|
|
||||||
const mjtNum offset[3], const mjtNum rotation[9],
|
|
||||||
int* faces, int* npoints, int* n0,
|
|
||||||
const mjSDF* sdf) {
|
|
||||||
const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]];
|
|
||||||
const int* faceid = m->bvh_nodeid + bvhadr;
|
|
||||||
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
|
|
||||||
const int* child = m->bvh_child + 2*bvhadr;
|
|
||||||
mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL;
|
|
||||||
|
|
||||||
mj_markStack(d);
|
|
||||||
// TODO(quaglino): Store bvh max depths to make this bound tighter.
|
|
||||||
int max_stack = m->mesh_bvhnum[m->geom_dataid[g]];
|
|
||||||
struct CollideTreeArgs_ {
|
|
||||||
int node;
|
|
||||||
};
|
|
||||||
typedef struct CollideTreeArgs_ CollideTreeArgs;
|
|
||||||
CollideTreeArgs* stack = mjSTACKALLOC(d, max_stack, CollideTreeArgs);
|
|
||||||
int nstack = 0;
|
|
||||||
stack[nstack].node = 0;
|
|
||||||
nstack++;
|
|
||||||
|
|
||||||
while (nstack) {
|
|
||||||
(*n0)++;
|
|
||||||
|
|
||||||
// pop from stack
|
|
||||||
nstack--;
|
|
||||||
int node = stack[nstack].node;
|
|
||||||
|
|
||||||
// node1 is a leaf
|
|
||||||
if (faceid[node] != -1) {
|
|
||||||
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
|
||||||
faces[*npoints] = faceid[node];
|
|
||||||
if (++(*npoints) == MAXSDFFACE) {
|
|
||||||
mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed");
|
|
||||||
mj_freeStack(d);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (bvh_active) bvh_active[node] = 1;
|
|
||||||
}
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// if no intersection at intermediate levels, stop
|
|
||||||
if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (bvh_active) bvh_active[node] = 1;
|
|
||||||
|
|
||||||
// recursive call
|
|
||||||
for (int i=0; i < 2; i++) {
|
|
||||||
if (child[2*node+i] != -1) {
|
|
||||||
if (nstack >= max_stack) mjERROR("BVH stack depth exceeded.");
|
|
||||||
stack[nstack].node = child[2*node+i];
|
|
||||||
nstack++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
mj_freeStack(d);
|
|
||||||
}
|
|
||||||
|
|
||||||
//------------------------------ collision functions -----------------------------------------------
|
//------------------------------ collision functions -----------------------------------------------
|
||||||
|
|
||||||
@@ -760,88 +609,7 @@ int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int
|
|||||||
|
|
||||||
// collision between a mesh and a signed distance field
|
// collision between a mesh and a signed distance field
|
||||||
int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||||
mjGETINFO;
|
return mjc_SDF(m, d, con, g1, g2, margin);
|
||||||
|
|
||||||
mjtNum offset[3], rotation[9], corners[9], x[3], depth;
|
|
||||||
mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT];
|
|
||||||
int vertadr = m->mesh_vertadr[m->geom_dataid[g1]];
|
|
||||||
int faceadr = m->mesh_faceadr[m->geom_dataid[g1]];
|
|
||||||
int cnt=0, npoints=0, ncandidate=0, n0=0, faces[MAXSDFFACE]={-1}, index[MAXMESHPNT];
|
|
||||||
|
|
||||||
// get sdf plugin
|
|
||||||
int instance = m->geom_plugin[g2];
|
|
||||||
const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g2);
|
|
||||||
instance = instance == -1 ? m->geom_dataid[g2] : instance;
|
|
||||||
mjtGeom geomtype = mjGEOM_SDF;
|
|
||||||
|
|
||||||
// copy into data
|
|
||||||
mjSDF sdf;
|
|
||||||
sdf.id = &instance;
|
|
||||||
sdf.type = mjSDFTYPE_SINGLE;
|
|
||||||
sdf.plugin = &sdf_ptr;
|
|
||||||
sdf.geomtype = &geomtype;
|
|
||||||
|
|
||||||
// compute transformation from g1 to g2
|
|
||||||
mjtNum sdf_quat[4], quat1[4];
|
|
||||||
mju_mat2Quat(quat1, mat1);
|
|
||||||
mju_mat2Quat(sdf_quat, mat2);
|
|
||||||
mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation);
|
|
||||||
|
|
||||||
// binary tree search
|
|
||||||
collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf);
|
|
||||||
|
|
||||||
// Frank-Wolfe algorithm
|
|
||||||
for (int i=0; i < npoints; i++) {
|
|
||||||
int face = faceadr + faces[i];
|
|
||||||
for (int v=0; v < 3; v++) {
|
|
||||||
mjtNum vec[3] = {
|
|
||||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0],
|
|
||||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1],
|
|
||||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+2],
|
|
||||||
};
|
|
||||||
|
|
||||||
// transform local 1 (mesh) to local 2 (sdf)
|
|
||||||
mju_mulMatVec3(corners+3*v, rotation, vec);
|
|
||||||
mju_addTo3(corners+3*v, offset);
|
|
||||||
}
|
|
||||||
|
|
||||||
// stricter culling
|
|
||||||
if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// starting point
|
|
||||||
x[0] = (corners[0]+corners[3]+corners[6])/3;
|
|
||||||
x[1] = (corners[1]+corners[4]+corners[7])/3;
|
|
||||||
x[2] = (corners[2]+corners[5]+corners[8])/3;
|
|
||||||
|
|
||||||
// SHOULD NOT OCCUR
|
|
||||||
if (ncandidate == MAXMESHPNT)mjERROR("too many contact points");
|
|
||||||
|
|
||||||
// Frank-Wolfe
|
|
||||||
depth = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d);
|
|
||||||
|
|
||||||
// store candidate if there is penetration
|
|
||||||
if (depth < 0) {
|
|
||||||
mju_copy3(candidate + 3*ncandidate, x);
|
|
||||||
index[ncandidate] = ncandidate;
|
|
||||||
dist[ncandidate++] = depth;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// sort contacts using depth
|
|
||||||
if (ncandidate > 1) {
|
|
||||||
int buf[MAXMESHPNT];
|
|
||||||
distSort(index, buf, ncandidate, dist);
|
|
||||||
}
|
|
||||||
|
|
||||||
// add only the first mjMAXCONPAIR pairs
|
|
||||||
for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) {
|
|
||||||
cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat,
|
|
||||||
dist[index[i]], cnt, m, &sdf, (mjData*)d);
|
|
||||||
}
|
|
||||||
|
|
||||||
return cnt;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// collision between two SDFs
|
// collision between two SDFs
|
||||||
|
|||||||
Reference in New Issue
Block a user