chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add primitive sphere-cylinder collider.

Browse Source 
PiperOrigin-RevId: 543586615
Change-Id: Icd47123e619f731f16dedfec25d825e44c24c036
This commit is contained in:
Yuval Tassa
2023-06-26 17:24:41 -07:00
committed by Copybara-Service
parent 1d6d6f33bf
commit a7021df63e
6 changed files with 185 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+1 -1
View File
@@ -10,7 +10,7 @@ General
- Added :ref:`mjv_connector` which has identical functionality to :ref:`mjv_makeConnector`, but with more convenient
"from-to" argument parametrization. :ref:`mjv_makeConnector` is now deprecated.
- Added primitive collider for sphere-cylinder contacts, previously this pair used the generic convex-convex collider.
Version 2.3.6 (June 20, 2023)
-----------------------------
src/engine/engine_collision_driver.c
+1 -1
View File
@@ -37,7 +37,7 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH */
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex},
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField},
/*SHPERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_Convex, mjc_SphereBox, mjc_Convex},
/*SHPERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex},
/*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex},
/*ELLIPSOID */ {0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_Convex},
/*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex},
src/engine/engine_collision_primitive.c
+74
View File
@@ -314,6 +314,80 @@ int mjc_SphereCapsule(const mjModel* m, const mjData* d,
// sphere : cylinder
int mjc_SphereCylinder(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
// get cylinder sizes and axis
mjtNum radius = size2[0];
mjtNum height = size2[1];
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
// find sphere projection onto cylinder axis and plane
mjtNum vec[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
mjtNum x = mju_dot3(axis, vec);
mjtNum a_proj[3], p_proj[3];
mju_scl3(a_proj, axis, x);
mju_sub3(p_proj, vec, a_proj);
mjtNum p_proj_sqr = mju_dot3(p_proj, p_proj);
// get collision type
int collide_side = mju_abs(x) < height;
int collide_cap = p_proj_sqr < radius*radius;
if (collide_side && collide_cap) { // deep penetration (sphere origin inside cylinder)
mjtNum dist_cap = height - mju_abs(x);
mjtNum dist_radius = radius - mju_sqrt(p_proj_sqr);
if (dist_cap < dist_radius) { // disable one collision type
collide_side = 0;
} else {
collide_cap = 0;
}
}
// side collision: use sphere-sphere
if (collide_side) {
mju_addTo3(a_proj, pos2);
return _SphereSphere(con, margin, pos1, mat1, size1, a_proj, mat2, size2);
}
// cap collision: use plane-sphere
if (collide_cap) {
mjtNum flipmat[9] = {
-mat2[0], mat2[1], -mat2[2],
-mat2[3], mat2[4], -mat2[5],
-mat2[6], mat2[7], -mat2[8]
};
mjtNum* mat_cap;
mjtNum pos_cap[3];
if (x > 0) { // top cap
mju_addScl3(pos_cap, pos2, axis, height);
mat_cap = mat2;
} else { // bottom cap
mju_addScl3(pos_cap, pos2, axis, -height);
mat_cap = flipmat;
}
int ncon = _PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1);
if (ncon) {
// flip frame normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER)
mju_scl3(con->frame, con->frame, -1);
}
return ncon;
}
// otherwise corner collision: use sphere-sphere
mju_scl3(p_proj, p_proj, size2[0] / mju_sqrt(p_proj_sqr)); // denominator cannot be 0
mju_scl3(vec, axis, x > 0 ? height : -height);
mju_addTo3(vec, p_proj);
mju_addTo3(vec, pos2);
// sphere-sphere with point sphere at the corner
mjtNum size_zero[1] = {0};
return _SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size_zero);
}
// capsule : capsule
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
src/engine/engine_collision_primitive.h
+2
View File
@@ -47,6 +47,8 @@ int mjc_SphereSphere (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
int mjc_SphereCapsule (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
int mjc_SphereCylinder (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
int mjc_CapsuleCapsule (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
test/engine/testdata/collision_primitive/sphere_cylinder.xml
Vendored
+44
View File
@@ -0,0 +1,44 @@
<mujoco>
<statistic meansize="0.15"/>
<visual>
<map force="0.02"/>
<scale contactheight=".05" contactwidth=".2"/>
<headlight ambient=".4 .4 .4"/>
</visual>
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<light pos="0 0 1"/>
<body pos="0 0 .3">
<freejoint/>
<geom type="sphere" size=".05" rgba=".5 .5 .5 .5" margin="0.1" gap="0.1"/>
</body>
<body pos="-.5 0 .3">
<freejoint/>
<geom type="sphere" size=".1" rgba=".5 .5 .5 .5"/>
</body>
<body mocap="true" pos="-1 0 0">
<geom type="cylinder" size=".2 .01" rgba=".5 .5 .5 .3" margin=".04"/>
</body>
<body mocap="true" pos="-.5 0 0">
<geom type="cylinder" size=".2 .04" rgba=".5 .5 .5 .3"/>
</body>
<body mocap="true">
<geom type="cylinder" size=".2 .2" rgba=".5 .5 .5 .3"/>
</body>
<body mocap="true" pos=".5 0 0">
<geom type="cylinder" size=".02 .1" rgba=".5 .5 .5 .3"/>
</body>
</worldbody>
</mujoco>
test/engine/testdata/collision_primitive/spheres_cylinders.xml
Vendored
+63
View File
@@ -0,0 +1,63 @@
<mujoco>
<statistic extent="1.5" meansize=".05"/>
<option solver="CG" tolerance="1e-6"/>
<size memory="5M"/>
<visual>
<rgba haze="0.15 0.25 0.35 1"/>
<map stiffness="700" shadowscale="0.5" fogstart="10" fogend="15" zfar="40" haze="0.3"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
</asset>
<default>
<geom solref=".005 1"/>
<default class="wall">
<geom type="plane" size=".5 .5 .05"/>
</default>
</default>
<worldbody>
<light directional="true" diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
<light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
<geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
<body mocap="true" pos="-.1 .05 0" zaxis=".5 0 1">
<geom type="capsule" size=".1 .1" group="1" condim="1"/>
</body>
<geom name="+x" class="wall" zaxis="1 0 0" pos="-.5 0 -.25"/>
<geom name="-x" class="wall" zaxis="-1 0 0" pos=".5 0 -.25"/>
<geom name="+y" class="wall" zaxis="0 1 0" pos="0 -.5 -.25"/>
<geom name="-y" class="wall" zaxis="0 -1 0" pos="0 .5 -.25"/>
<composite type="particle" count="3 3 2" spacing="0.2" offset="0 0 1">
<geom type="sphere" size=".09" rgba=".8 .2 .1 1" condim="3"/>
<joint kind="particle" type="free"/>
</composite>
<composite type="particle" count="3 3 2" spacing="0.2" offset="0 0 1.6" prefix="A">
<geom type="cylinder" size=".09 .02" rgba=".8 .2 .1 1" condim="3"/>
<joint kind="particle" type="free"/>
</composite>
<composite type="particle" count="3 3 2" spacing="0.2" offset="0 0 2.2" prefix="B">
<geom type="cylinder" size=".02 .09" rgba=".8 .2 .1 1" condim="3"/>
<joint kind="particle" type="free"/>
</composite>
<composite type="particle" count="3 3 2" spacing="0.2" offset="0 0 2.8" prefix="C">
<geom type="sphere" size=".06" rgba=".8 .2 .1 1" condim="3"/>
<joint kind="particle" type="free"/>
</composite>
</worldbody>
</mujoco>