Странное поведение при моделировании небольшого многосвязного объекта

Question

Я пытаюсь моделировать небольшие и легкие объекты (звенья веревки). Это пример sdf. Если я уменьшу массу с 1,0 до 0,3 и соответствующим образом отрегулирую инерцию, кажется, что она исчезнет с экрана, как только коснется земли. С большим количеством ссылок я также получаю

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 (и большие инерции) на фиктивной ссылке, но мне интересно, есть ли правильный способ устранить эти ошибки. Я также добавлю, что я не наблюдаю странного поведения для одной ссылки.

SDF: 2 звена массой 1 кг

<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 звена массой 0,3 кг

<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>

Answer 1

Эта ошибка обычно возникает из-за того, что наш дискретный решатель не работает. Как следует из сообщения, пытались ли вы сократить дискретный период (также известный как временной шаг) при создании MultibodyPlant? Я предполагаю, что здесь происходит то, что ваши маленькие ссылки приводят к быстрой динамике, и поэтому вам нужны меньшие временные шаги для ее решения.

Исчезновение ссылки, скорее всего, связано с тем, что визуализатор уже отображает плохое состояние (разошлись, возможно, с большим количеством NaN). Так что визуализатор сам по себе мало что вам скажет.

При настройке дискретного решателя вам всегда нужна оценка шкалы времени вашей системы, исходя из физических свойств, которые вы хотите решить. В вашем случае и только потому, что я не знаю, приложили ли вы силы, представление о каждом звене как о простом маятнике позволит вам оценить масштаб времени, например, вычислив период этого маятника.

С вашими числами для физического маятника с инерцией Iyy = 0,001 (около конца звена движение вокруг z кажется ограниченным вашими суставами), m = 0,3, R = 0,05 (расстояние до COM), я получаю эффективную длину составного маятника L = I / (м * R) = 0,0667 м. С этим я получаю период T = 2 * pi * sqrt (L / g) = 0,52 секунды. Скажем, я бы сделал 500 шагов за период, тогда я бы оценил временной шаг dt = 1 мс.

Это очень приблизительная оценка, учитывая, что я ничего не знаю о вашей системе или внешних силах. Но резюмируя, я считаю, что ваша проблема - это временной шаг установки.

Edit

А как насчет допуска на проникновение? (MultibodyPlant::set_penetration_allowance()). При наличии контакта MultibodyPlant::get_contact_penalty_method_time_scale() даст вам приблизительную оценку времени контакта. Обычно вам нужно установить временной шаг, который позволяет комфортно разрешить эту динамику.