MJX equality eq_active not working (?) #2149
-
IntroI'm a researcher using MJX for GPU-parallelized RL environments My setupMJX 3.2.3, brax 0.10.5 My questionIt seems that equality constraints (or at least the weld equality) ignores the eq_active field. When setting active="false", it's still active, and when I examine (from brax, but I assume this refers to an underlying MJX field that's tracked) pipeline_state.eq_active, it is 0 regardless of whether I set active="true" or active="false". When I manually modify it to 1 at runtime, it doesn't do anything. Primarily, I'm trying to have an equality that I can turn on or off, since adhesion grippers are not currently supported in MJX. Any hack will work for now as well. I tried also setting solimp[0] = solimp[1] = 0, and solref[0] to be really large, so the constraint is pretty much ignored, but it also seems I have no way of dynamically adjusting this at runtime. My questions are:
Could also be something with brax as well, though I'm not sure, especially as setting active="false" in the XML does not have any effect. I've looked around at the existing issues and discussions and have not found anything that seems to be related to the equality constraint not being dynamically enable-able or disable-able in MJX, so I hope I am not duplicating any issues. Minimal model and/or code that explain my questionIf it matters, my XML is below. I also have some brax environment code and the environment runner code, but pretty much it just modifies and/or queries state.pipeline_state.eq_active. <!-- Credit: adapted from Mujoco Menagerie -->
<mujoco model="panda">
<compiler angle="radian" meshdir="franka_emika_panda/assets" autolimits="true"/>
<option integrator="implicitfast" impratio="10"/>
<default>
<default class="panda">
<joint armature="0.1" damping="5" axis="0 0 1" range="-2.8973 2.8973"/>
<general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
<default class="finger">
<joint axis="0 1 0" type="slide" range="0 0.08"/>
</default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
</default>
<default class="collision">
<geom type="mesh" group="3"/>
<default class="finger_collision">
<geom type="box" size="0.015 0.005 0.04" pos="0 0.0055 0.035" friction="10 0.005 0.0001"/>
</default>
</default>
</default>
</default>
<asset>
<!-- Visual meshes -->
<mesh file="link0_0.obj"/>
<mesh file="link0_1.obj"/>
<mesh file="link0_2.obj"/>
<mesh file="link0_3.obj"/>
<mesh file="link0_4.obj"/>
<mesh file="link0_5.obj"/>
<mesh file="link0_7.obj"/>
<mesh file="link0_8.obj"/>
<mesh file="link0_9.obj"/>
<mesh file="link0_10.obj"/>
<mesh file="link0_11.obj"/>
<mesh file="link1.obj"/>
<mesh file="link2.obj"/>
<mesh file="link3_0.obj"/>
<mesh file="link3_1.obj"/>
<mesh file="link3_2.obj"/>
<mesh file="link3_3.obj"/>
<mesh file="link4_0.obj"/>
<mesh file="link4_1.obj"/>
<mesh file="link4_2.obj"/>
<mesh file="link4_3.obj"/>
<mesh file="link5_0.obj"/>
<mesh file="link5_1.obj"/>
<mesh file="link5_2.obj"/>
<mesh file="link6_0.obj"/>
<mesh file="link6_1.obj"/>
<mesh file="link6_2.obj"/>
<mesh file="link6_3.obj"/>
<mesh file="link6_4.obj"/>
<mesh file="link6_5.obj"/>
<mesh file="link6_6.obj"/>
<mesh file="link6_7.obj"/>
<mesh file="link6_8.obj"/>
<mesh file="link6_9.obj"/>
<mesh file="link6_10.obj"/>
<mesh file="link6_11.obj"/>
<mesh file="link6_12.obj"/>
<mesh file="link6_13.obj"/>
<mesh file="link6_14.obj"/>
<mesh file="link6_15.obj"/>
<mesh file="link6_16.obj"/>
<mesh file="link7_0.obj"/>
<mesh file="link7_1.obj"/>
<mesh file="link7_2.obj"/>
<mesh file="link7_3.obj"/>
<mesh file="link7_4.obj"/>
<mesh file="link7_5.obj"/>
<mesh file="link7_6.obj"/>
<mesh file="link7_7.obj"/>
<mesh file="hand_0.obj"/>
<mesh file="hand_1.obj"/>
<mesh file="hand_2.obj"/>
<mesh file="hand_3.obj"/>
<mesh file="hand_4.obj"/>
<mesh file="finger_0.obj"/>
<mesh file="finger_1.obj"/>
</asset>
<worldbody>
<light name="top" pos="0 0 2" mode="trackcom"/>
<body name="link0" childclass="panda">
<geom mesh="link0_0" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
<geom mesh="link0_1" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link0_2" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
<geom mesh="link0_3" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link0_4" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
<geom mesh="link0_5" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link0_7" rgba="1 1 1 1" class="visual"/>
<geom mesh="link0_8" rgba="1 1 1 1" class="visual"/>
<geom mesh="link0_9" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link0_10" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
<geom mesh="link0_11" rgba="1 1 1 1" class="visual"/>
<body name="link1" pos="0 0 0.333">
<inertial mass="4.970684" pos="0.003875 0.002081 -0.04762"
fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
<joint name="joint1"/>
<geom rgba="1 1 1 1" mesh="link1" class="visual"/>
<body name="link2" quat="1 -1 0 0">
<inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495"
fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
<joint name="joint2" range="-1.7628 1.7628"/>
<geom rgba="1 1 1 1" mesh="link2" class="visual"/>
<body name="link3" pos="0 -0.316 0" quat="1 1 0 0">
<joint name="joint3"/>
<inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2"
fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
<geom mesh="link3_0" rgba="1 1 1 1" class="visual"/>
<geom mesh="link3_1" rgba="1 1 1 1" class="visual"/>
<geom mesh="link3_2" rgba="1 1 1 1" class="visual"/>
<geom mesh="link3_3" rgba="0.25 0.25 0.25 1" class="visual"/>
<body name="link4" pos="0.0825 0 0" quat="1 1 0 0">
<inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2"
fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
<joint name="joint4" range="-3.0718 -0.0698"/>
<geom mesh="link4_0" rgba="1 1 1 1" class="visual"/>
<geom mesh="link4_1" rgba="1 1 1 1" class="visual"/>
<geom mesh="link4_2" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link4_3" rgba="1 1 1 1" class="visual"/>
<body name="link5" pos="-0.0825 0.384 0" quat="1 -1 0 0">
<inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2"
fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
<joint name="joint5"/>
<geom mesh="link5_0" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link5_1" rgba="1 1 1 1" class="visual"/>
<geom mesh="link5_2" rgba="1 1 1 1" class="visual"/>
<body name="link6" quat="1 1 0 0">
<inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2"
fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
<joint name="joint6" range="-0.0175 3.7525"/>
<geom mesh="link6_0" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
<geom mesh="link6_1" rgba="1 1 1 1" class="visual"/>
<geom mesh="link6_2" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link6_3" rgba="1 1 1 1" class="visual"/>
<geom mesh="link6_4" rgba="1 1 1 1" class="visual"/>
<geom mesh="link6_5" rgba="1 1 1 1" class="visual"/>
<geom mesh="link6_6" rgba="1 1 1 1" class="visual"/>
<geom mesh="link6_7" rgba="0.039216 0.541176 0.780392 1" class="visual"/>
<geom mesh="link6_8" rgba="0.039216 0.541176 0.780392 1" class="visual"/>
<geom mesh="link6_9" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link6_10" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link6_11" rgba="1 1 1 1" class="visual"/>
<geom mesh="link6_12" rgba="0 1 0 1" class="visual"/>
<geom mesh="link6_13" rgba="1 1 1 1" class="visual"/>
<geom mesh="link6_14" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link6_15" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link6_16" rgba="1 1 1 1" class="visual"/>
<body name="link7" pos="0.088 0 0" quat="1 1 0 0">
<inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2"
fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
<joint name="joint7"/>
<geom mesh="link7_0" rgba="1 1 1 1" class="visual"/>
<geom mesh="link7_1" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link7_2" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link7_3" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link7_4" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link7_5" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link7_6" rgba="0.25 0.25 0.25 1" class="visual"/>
<geom mesh="link7_7" rgba="1 1 1 1" class="visual"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<body name="binA" pos="-0.17 0.6 0">
<geom name="binA1" pos="0 -0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
<geom name="binA2" pos="0 0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
<geom name="binA3" pos="0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
<geom name="binA4" pos="-0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
</body>
<body name="binB" pos="0.17 0.6 0">
<geom name="binB1" pos="0 -0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
<geom name="binB2" pos="0 0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
<geom name="binB3" pos="0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
<geom name="binB4" pos="-0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
</body>
<body name="cube" pos="-0.15 0.6 0.07">
<freejoint/>
<geom name="cube_geom" size="0.03 0.03 0.03" type="box" rgba="0 0.4 0 1" contype="0" conaffinity="0"/>
</body>
<body name="goal_marker" pos="-0.15 0.6 0.07" gravcomp="1">
<freejoint/>
<geom name="goal_marker_geom" size="0.03 0.03 0.03" type="box" rgba="0.3 1 0.3 1" contype="0" conaffinity="0"/>
</body>
<body name="tablelink" pos="0 .2 0">
<geom name="table_geom" group="4" pos="0.0 0.0 0.0" size="2.8 2.4 0.1" type="plane" mass="10" friction="0.2 0.005 0.0001" contype="0" conaffinity="0"/>
</body>
</worldbody>
<equality>
<weld body1="link7" body2="cube" relpose="0 0 0.135 1 0 0 0.414" solimp="0.95 0.99 0.001" solref="0.005 1"/> <!-- Use these solver params when active -->
<!-- <weld body1="link7" body2="cube" relpose="0 0 0.135 1 0 0 0.414" solimp="0 0 0.001" solref="200 1"/>--> <!-- Use these solver params when inactive -->
</equality>
<contact>
<pair geom1="cube_geom" geom2="table_geom"/>
<pair geom1="cube_geom" geom2="binA1"/>
<pair geom1="cube_geom" geom2="binA2"/>
<pair geom1="cube_geom" geom2="binA3"/>
<pair geom1="cube_geom" geom2="binA4"/>
<pair geom1="cube_geom" geom2="binB1"/>
<pair geom1="cube_geom" geom2="binB2"/>
<pair geom1="cube_geom" geom2="binB3"/>
<pair geom1="cube_geom" geom2="binB4"/>
</contact>
<actuator>
<general class="panda" name="actuator1" joint="joint1" gainprm="4500" biasprm="0 -4500 -450"/>
<general class="panda" name="actuator2" joint="joint2" gainprm="4500" biasprm="0 -4500 -450"
ctrlrange="-1.7628 1.7628"/>
<general class="panda" name="actuator3" joint="joint3" gainprm="2000" biasprm="0 -3500 -350"/>
<general class="panda" name="actuator4" joint="joint4" gainprm="2000" biasprm="0 -3500 -350"
ctrlrange="-3.0718 -0.0698"/>
<general class="panda" name="actuator5" joint="joint5" gainprm="1000" biasprm="0 -2000 -200" forcerange="-12 12"/>
<general class="panda" name="actuator6" joint="joint6" gainprm="1000" biasprm="0 -2000 -200" forcerange="-12 12"
ctrlrange="-0.0175 3.7525"/>
<general class="panda" name="actuator7" joint="joint7" gainprm="1000" biasprm="0 -2000 -200" forcerange="-12 12"/>
</actuator>
</mujoco>
Confirmations
|
Beta Was this translation helpful? Give feedback.
Replies: 1 comment 2 replies
-
Beta Was this translation helpful? Give feedback.
Tracking in #2173