Request of the guidance of the proper XML simulation setting for NIST gear meshing #19
Description
Hi, @yunshengtian. First thank you for your great share on the assembly plan.
I am now using your implementation for the assembly plan on the example of gear meshing in the NIST challenge (https://www.nist.gov/el/intelligent-systems-division-73500/robotic-grasping-and-manipulation-assembly/assembly). In this specific example, I want to generate the disassembly plan for the gears under the simultaneous effect of both translational force and torque (in the same direction). The gear assembly looks as below.
However, when I changed the XML simulation settings correspondingly to make the disassembly process work based on your defined physics model, weird things happen. As you can see in the gifs below, (the 1st gif) the medium-sized and small-sized gears don't revolute around their respective axis, or at least move in a physically expected way. Instead, they keep still with severe mesh penetration between the large and medium gears. Similarly, (the 2nd gif) the small-sized gear moves in an anomaly way along with severe mesh penetration.
And here are the corresponding XML strings for both gifs.
For the 1st gif, 'part2' refers to the large gear, 'part0' the small one, 'part1' the medium one, and 'part3' the base holding the gears.
<redmax model="assemble">
<option integrator="BDF1" timestep="1e-3" gravity="0. 0. 1e-12"/>
<render bg_color="255 255 255 255"/>
<default>
<general_SDF_contact kn="1000000.0" kt="1e-12" mu="0.0" damping="0.0"/>
</default>
<robot>
<link name="part2">
<joint name="part2" type="free3d-exp" axis="0.0 0.0 0.0" pos="-2.459021304856198 0.002167244860656758 -0.006609842408113079" quat="1 0 0 0" frame="WORLD" damping="0.0"/>
<body name="part2" type="SDF" filename="/home/rkx/Telekinesis/code_base/autoplan/resources/preprocessed_obj/00010/2.obj" load_sdf="true" save_sdf="true" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.05" col_th="0.013587286900234731" mu="0.0" rgba="0.23529411764705882 0.6549019607843137 0.8666666666666667 1.0"/>
</link>
</robot>
<robot>
<link name="part0">
<joint name="part0" type="free3d-exp" axis="0.0 0.0 0.0" pos="4.098382401387394 -0.0003343830775857804 0.08086139665247331" quat="1 0 0 0" frame="WORLD" damping="0.0"/>
<body name="part0" type="SDF" filename="/home/rkx/Telekinesis/code_base/autoplan/resources/preprocessed_obj/00010/0.obj" load_sdf="true" save_sdf="true" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.05" col_th="0.013587286900234731" mu="0.0" rgba="0.8235294117647058 0.3411764705882353 0.34901960784313724 1.0"/>
</link>
</robot>
<robot>
<link name="part1">
<joint name="part1" type="free3d-exp" axis="0.0 0.0 0.0" pos="1.639126402467036 0.0029291938313094857 0.07973205865685809" quat="1 0 0 0" frame="WORLD" damping="0.0"/>
<body name="part1" type="SDF" filename="/home/rkx/Telekinesis/code_base/autoplan/resources/preprocessed_obj/00010/1.obj" load_sdf="true" save_sdf="true" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.05" col_th="0.013587286900234731" mu="0.0" rgba="0.9294117647058824 0.8 0.28627450980392155 1.0"/>
</link>
</robot>
<robot>
<link name="part3">
<joint name="part3" type="fixed" axis="0.0 0.0 0.0" pos="0.09422816528262493 7.229772216127662e-07 -0.7300483687716826" quat="1 0 0 0" frame="WORLD" damping="0.0"/>
<body name="part3" type="SDF" filename="/home/rkx/Telekinesis/code_base/autoplan/resources/preprocessed_obj/00010/3.obj" load_sdf="true" save_sdf="true" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.05" col_th="0.013587286900234731" mu="0.0" rgba="0.7450980392156863 0.49411764705882355 0.8156862745098039 1.0"/>
</link>
</robot>
<contact>
<general_SDF_contact general_body="part2" SDF_body="part0"/>
<general_SDF_contact general_body="part0" SDF_body="part2"/>
<general_SDF_contact general_body="part2" SDF_body="part1"/>
<general_SDF_contact general_body="part1" SDF_body="part2"/>
<general_SDF_contact general_body="part2" SDF_body="part3"/>
<general_SDF_contact general_body="part3" SDF_body="part2"/>
</contact>
</redmax>For the 2nd gif, 'part1' refers to the medium one, 'part0' the small one, and 'part3' the base holding the gears.
<redmax model="assemble">
<option integrator="BDF1" timestep="1e-3" gravity="0. 0. 1e-12"/>
<render bg_color="255 255 255 255"/>
<default>
<general_SDF_contact kn="1000000.0" kt="1e-12" mu="0.0" damping="0.0"/>
</default>
<robot>
<link name="part1">
<joint name="part1" type="free3d-exp" axis="0.0 0.0 0.0" pos="1.639126402467036 0.0029291938313094857 0.07973205865685809" quat="1 0 0 0" frame="WORLD" damping="0.0"/>
<body name="part1" type="SDF" filename="/home/rkx/Telekinesis/code_base/autoplan/resources/preprocessed_obj/00010/1.obj" load_sdf="true" save_sdf="true" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.05" col_th="0.013617513774617371" mu="0.0" rgba="0.9294117647058824 0.8 0.28627450980392155 1.0"/>
</link>
</robot>
<robot>
<link name="part0">
<joint name="part0" type="free3d-exp" axis="0.0 0.0 0.0" pos="4.098382401387394 -0.0003343830775857804 0.08086139665247331" quat="1 0 0 0" frame="WORLD" damping="0.0"/>
<body name="part0" type="SDF" filename="/home/rkx/Telekinesis/code_base/autoplan/resources/preprocessed_obj/00010/0.obj" load_sdf="true" save_sdf="true" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.05" col_th="0.013617513774617371" mu="0.0" rgba="0.8235294117647058 0.3411764705882353 0.34901960784313724 1.0"/>
</link>
</robot>
<robot>
<link name="part3">
<joint name="part3" type="fixed" axis="0.0 0.0 0.0" pos="0.09422816528262493 7.229772216127662e-07 -0.7300483687716826" quat="1 0 0 0" frame="WORLD" damping="0.0"/>
<body name="part3" type="SDF" filename="/home/rkx/Telekinesis/code_base/autoplan/resources/preprocessed_obj/00010/3.obj" load_sdf="true" save_sdf="true" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.05" col_th="0.013617513774617371" mu="0.0" rgba="0.7450980392156863 0.49411764705882355 0.8156862745098039 1.0"/>
</link>
</robot>
<contact>
<general_SDF_contact general_body="part1" SDF_body="part0"/>
<general_SDF_contact general_body="part0" SDF_body="part1"/>
<general_SDF_contact general_body="part1" SDF_body="part3"/>
<general_SDF_contact general_body="part3" SDF_body="part1"/>
</contact>
</redmax>Many thanks for any kindly helpful support!