建模小型 multi-link 对象时的奇怪行为
Weird behavior when modeling small multi-link object
我正在尝试模拟一个小而轻的物体（link 绳索）。这是一个示例 sdf。如果我将质量从 1.0 减少到 0.3 并相应地调整惯性，它似乎一接触地面就从屏幕上消失了。更多 links，我也得到
RuntimeError: MultibodyPlant's discrete update solver failed to converge at simulation time = 0.004 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:
1. reduce the discrete update period set at construction,
2. decrease the high gains in your controller whenever possible,
3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.
或
terminated by signal SIGSEGV (Address boundary error)
如果我不计算物理上准确的惯性或者如果我将质量为 1（和大的惯性）放在假人 link 上，我可以解决很多这些问题，但我想知道如果有解决这些错误的正确方法。我还要补充一点，我没有看到单个 link 的奇怪行为。
SDF：2 links 质量为 1kg
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>1.0</mass>
<inertia>
<ixx>0.0008395833333333335</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0008395833333333335</iyy>
<iyz>0</iyz>
<izz>1.25e-05</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>1.0</mass>
<inertia>
<ixx>0.0008395833333333335</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0008395833333333335</iyy>
<iyz>0</iyz>
<izz>1.25e-05</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose> 0 0 1.9 0 0 0 </pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 0.0 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 0.0 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
SDF：2 links，质量为 0.3kg
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose>0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose>0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
该错误通常是由于我们的离散求解器失败造成的。如消息所示，您是否尝试在构建 MultibodyPlant 时减少离散周期（也称为时间步长）？我想象这里发生的是你的小 links 导致快速动态，因此你需要更小的时间步来解决它。
link 消失很可能是因为可视化工具已经在绘制一个错误的状态（发散，可能有很多 NaN）。所以光是可视化工具不会告诉你太多。
设置离散求解器时，您始终需要估计系统的时间尺度，这源于您要解决的物理问题。对于您的情况，只是因为我不知道您是否施加了力，将每个 link 视为一个简单的钟摆，您可以通过例如计算该钟摆的周期来估计时间尺度。
根据你的数字，对于惯性 Iyy = 0.001 的物理钟摆（关于 link 的末端，关于 z 的运动似乎受到你的关节的限制），m = 0.3，R=0.05（距离到COM)，我得到一个复摆有效长度 L = I/(m*R) = 0.0667 m。有了这个，我得到了一个周期 T = 2*pi*sqrt(L/g) = 0.52 秒。假设我每个周期走 500 步，那么我估计时间步长为 dt = 1ms。
这是一个非常粗略的估计，因为我对您的系统或外力一无所知。但总而言之，我相信你的问题是植物的时间步长。
编辑
渗透津贴呢？ (MultibodyPlant::set_penetration_allowance())。当接触存在时 MultibodyPlant::get_contact_penalty_method_time_scale() 会给你一个估计的时间尺度，因为接触。通常您需要设置一个能够轻松解决该动态的时间步长。
我正在尝试模拟一个小而轻的物体（link 绳索）。这是一个示例 sdf。如果我将质量从 1.0 减少到 0.3 并相应地调整惯性，它似乎一接触地面就从屏幕上消失了。更多 links，我也得到
RuntimeError: MultibodyPlant's discrete update solver failed to converge at simulation time = 0.004 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:
1. reduce the discrete update period set at construction,
2. decrease the high gains in your controller whenever possible,
3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.
或
terminated by signal SIGSEGV (Address boundary error)
如果我不计算物理上准确的惯性或者如果我将质量为 1（和大的惯性）放在假人 link 上，我可以解决很多这些问题，但我想知道如果有解决这些错误的正确方法。我还要补充一点，我没有看到单个 link 的奇怪行为。
SDF：2 links 质量为 1kg
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>1.0</mass>
<inertia>
<ixx>0.0008395833333333335</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0008395833333333335</iyy>
<iyz>0</iyz>
<izz>1.25e-05</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>1.0</mass>
<inertia>
<ixx>0.0008395833333333335</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0008395833333333335</iyy>
<iyz>0</iyz>
<izz>1.25e-05</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose> 0 0 1.9 0 0 0 </pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 0.0 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 0.0 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
SDF：2 links，质量为 0.3kg
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose>0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose>0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
该错误通常是由于我们的离散求解器失败造成的。如消息所示，您是否尝试在构建 MultibodyPlant 时减少离散周期（也称为时间步长）？我想象这里发生的是你的小 links 导致快速动态，因此你需要更小的时间步来解决它。
link 消失很可能是因为可视化工具已经在绘制一个错误的状态（发散，可能有很多 NaN）。所以光是可视化工具不会告诉你太多。
设置离散求解器时，您始终需要估计系统的时间尺度，这源于您要解决的物理问题。对于您的情况，只是因为我不知道您是否施加了力，将每个 link 视为一个简单的钟摆，您可以通过例如计算该钟摆的周期来估计时间尺度。
根据你的数字，对于惯性 Iyy = 0.001 的物理钟摆（关于 link 的末端，关于 z 的运动似乎受到你的关节的限制），m = 0.3，R=0.05（距离到COM)，我得到一个复摆有效长度 L = I/(m*R) = 0.0667 m。有了这个，我得到了一个周期 T = 2*pi*sqrt(L/g) = 0.52 秒。假设我每个周期走 500 步，那么我估计时间步长为 dt = 1ms。
这是一个非常粗略的估计，因为我对您的系统或外力一无所知。但总而言之，我相信你的问题是植物的时间步长。
编辑
渗透津贴呢？ (MultibodyPlant::set_penetration_allowance())。当接触存在时 MultibodyPlant::get_contact_penalty_method_time_scale() 会给你一个估计的时间尺度，因为接触。通常您需要设置一个能够轻松解决该动态的时间步长。