search

google / brax

Massively parallel rigidbody physics simulation on accelerator hardware.
Apache License 2.0
2.14k stars 234 forks source link

Forward kinematics fail for models with different numbers of joints and links #382

Open alberthli opened 11 months ago

alberthli commented 11 months ago

For robots like fixed-base serial manipulators, it is not uncommon to have n joints and n+1 links, as there will be a nontrivial base link with inertial properties. For example, consider the Franka Panda arm with the following MJCF slightly modified from the mujoco_menagerie repo.

<mujoco model="panda nohand">
  <compiler angle="radian" meshdir="assets" autolimits="true"/>

  <default>
    <default class="panda">
      <material specular="0.5" shininess="0.25"/>
      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973" type="hinge"/>
      <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.04"/>
      </default>

      <default class="panda/visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
      </default>
      <default class="panda/collision">
        <geom type="mesh" group="3"/>
      </default>
      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
    </default>
  </default>

  <asset>
    <material class="panda" name="white" rgba="1 1 1 1"/>
    <material class="panda" name="off_white" rgba="0.901961 0.921569 0.929412 1"/>
    <material class="panda" name="dark_grey" rgba="0.25 0.25 0.25 1"/>
    <material class="panda" name="green" rgba="0 1 0 1"/>
    <material class="panda" name="light_blue" rgba="0.039216 0.541176 0.780392 1"/>

    <!-- Collision meshes -->
    <mesh name="link0_c" file="link0.stl"/>
    <mesh name="link1_c" file="link1.stl"/>
    <mesh name="link2_c" file="link2.stl"/>
    <mesh name="link3_c" file="link3.stl"/>
    <mesh name="link4_c" file="link4.stl"/>
    <mesh name="link5_c0" file="link5_collision_0.obj"/>
    <mesh name="link5_c1" file="link5_collision_1.obj"/>
    <mesh name="link5_c2" file="link5_collision_2.obj"/>
    <mesh name="link6_c" file="link6.stl"/>
    <mesh name="link7_c" file="link7.stl"/>

    <!-- 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"/>
  </asset>

  <worldbody>
    <body name="link0" childclass="panda">
      <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
        fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
      <geom mesh="link0_0" material="off_white" class="panda/visual"/>
      <geom mesh="link0_1" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_2" material="off_white" class="panda/visual"/>
      <geom mesh="link0_3" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_4" material="off_white" class="panda/visual"/>
      <geom mesh="link0_5" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_7" material="white" class="panda/visual"/>
      <geom mesh="link0_8" material="white" class="panda/visual"/>
      <geom mesh="link0_9" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_10" material="off_white" class="panda/visual"/>
      <geom mesh="link0_11" material="white" class="panda/visual"/>
      <geom mesh="link0_c" class="panda/collision"/>
      <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 material="white" mesh="link1" class="panda/visual"/>
        <geom mesh="link1_c" class="panda/collision"/>
        <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 material="white" mesh="link2" class="panda/visual"/>
          <geom mesh="link2_c" class="panda/collision"/>
          <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" material="white" class="panda/visual"/>
            <geom mesh="link3_1" material="white" class="panda/visual"/>
            <geom mesh="link3_2" material="white" class="panda/visual"/>
            <geom mesh="link3_3" material="dark_grey" class="panda/visual"/>
            <geom mesh="link3_c" class="panda/collision"/>
            <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" material="white" class="panda/visual"/>
              <geom mesh="link4_1" material="white" class="panda/visual"/>
              <geom mesh="link4_2" material="dark_grey" class="panda/visual"/>
              <geom mesh="link4_3" material="white" class="panda/visual"/>
              <geom mesh="link4_c" class="panda/collision"/>
              <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" material="dark_grey" class="panda/visual"/>
                <geom mesh="link5_1" material="white" class="panda/visual"/>
                <geom mesh="link5_2" material="white" class="panda/visual"/>
                <geom mesh="link5_c0" class="panda/collision"/>
                <geom mesh="link5_c1" class="panda/collision"/>
                <geom mesh="link5_c2" class="panda/collision"/>
                <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" material="off_white" class="panda/visual"/>
                  <geom mesh="link6_1" material="white" class="panda/visual"/>
                  <geom mesh="link6_2" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_3" material="white" class="panda/visual"/>
                  <geom mesh="link6_4" material="white" class="panda/visual"/>
                  <geom mesh="link6_5" material="white" class="panda/visual"/>
                  <geom mesh="link6_6" material="white" class="panda/visual"/>
                  <geom mesh="link6_7" material="light_blue" class="panda/visual"/>
                  <geom mesh="link6_8" material="light_blue" class="panda/visual"/>
                  <geom mesh="link6_9" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_10" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_11" material="white" class="panda/visual"/>
                  <geom mesh="link6_12" material="green" class="panda/visual"/>
                  <geom mesh="link6_13" material="white" class="panda/visual"/>
                  <geom mesh="link6_14" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_15" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_16" material="white" class="panda/visual"/>
                  <geom mesh="link6_c" class="panda/collision"/>
                  <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" material="white" class="panda/visual"/>
                    <geom mesh="link7_1" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_2" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_3" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_4" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_5" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_6" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_7" material="white" class="panda/visual"/>
                    <geom mesh="link7_c" class="panda/collision"/>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <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="3500" biasprm="0 -3500 -350"/>
    <general class="panda" name="actuator4" joint="joint4" gainprm="3500" biasprm="0 -3500 -350"
      ctrlrange="-3.0718 -0.0698"/>
    <general class="panda" name="actuator5" joint="joint5" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
    <general class="panda" name="actuator6" joint="joint6" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"
      ctrlrange="-0.0175 3.7525"/>
    <general class="panda" name="actuator7" joint="joint7" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
  </actuator>
</mujoco>

We can load this model and try to run forward kinematics on it, but it fails:

import jax.numpy as jnp
import mujoco
from brax import kinematics
from brax.io.mjcf import load_model
mj_model = mujoco.MjModel.from_xml_path("panda_nohand.xml")  # keep the same file name, replace MJCF contents w/above
sys = load_model(mj_model)
q = jnp.array([0.0, -0.7854, 0.0, -2.3562, 0.0, 1.5708, 0.7854])
qd = jnp.zeros(7)
x, dx = kinematics.forward(sys, q, dq)  # ERROR

The error message reads

*** ValueError: vmap got inconsistent sizes for array axes to be mapped:
  * most axes (3 of them) had size 7, e.g. axis 0 of argument self.pos of type float32[7,3];
  * one axis had size 8: axis 0 of argument o.rot of type float32[8,4]

This is because brax.kinematics tries to apply vmap over joints and links, but there are more links (8) than joints (7). For these fixed-base type systems, the model loader should detect that the first link has no joint to world (i.e., it is implicitly fixed to the world body) and modify the forward kinematics to take that into account.

As a workaround, I am currently adding a dummy joint between the base link and the world and then doing some bookkeeping to ensure the joint always has some fixed value and ignoring those DOFs.

baixianger commented 11 months ago

Does this mean there should be a joint inside every body-labels? I met the same issue as you mentioned. 

ValueError: vmap got inconsistent sizes for array axes to be mapped:
  * most axes (3 of them) had size 35, e.g. axis 0 of argument self.pos of type float32[35,3];
  * one axis had size 38: axis 0 of argument o.rot of type float32[38,4]

I got 32 joints but 38 links, I suppose it's because I have nested body in my model.

alberthli commented 11 months ago

I'm not sure - the bug I'm seeing here is more like an "off by 1" situation where the first (inferred) dummy joint that fixes the base of the robot to the world isn't being parsed correctly. The examples in this repo don't trigger the bug because they seem to all be floating-body models, so there's an explicit "free" joint connecting the base of the robot to the world.

In your case, you seem to have even more discrepancies between the number of joints and the number of links. If your kinematics satisfy a tree structure (so no loops), then it's not obvious to me where this discrepancy comes from.

baixianger commented 11 months ago

In your case, you seem to have even more discrepancies between the number of joints and the number of links. If your kinematics satisfy a tree structure (so no loops), then it's not obvious to me where this discrepancy comes from.

XML file link list

I experimented with my XML; There is a whip_start body but without a joint geom (see left figure), then I print all the links. A body without joint, but is listed in the link_names method in my System object (see right figure).

baixianger commented 11 months ago

So I think it's quite clear. If we want to model fixed stuff, like Franka panda, all the geoms from link0 or aka. base, <body name="link0" childclass="panda">, should not be wrapped inside a body, instead, they should be directly located in the worldbody. But for robot like ant, because its whole body is independent of the worldbody, it should be wrapped within a main body with a freejoint. That might be a really trick part of Brax.

alberthli commented 11 months ago

So I think it's quite clear. If we want to model fixed stuff, like Franka panda, all the geoms from link0 or aka. base, , should not be wrapped inside a body, instead, they should be directly located in the worldbody. But for robot like ant, because its whole body is independent of the worldbody, it should be wrapped within a main body with a freejoint. That might be a really trick part of Brax.

I haven't tried this yet, but to my understanding, part of the reason why this might be difficult to do is that link0 is a body with inertial properties that have to be non-trivially specified. On the other hand, in MJCFs, the worldbody can't have inertial properties. This is one reason why the fixed joint type exists in the first place.

baixianger commented 10 months ago

So I think it's quite clear. If we want to model fixed stuff, like Franka panda, all the geoms from link0 or aka. base, , should not be wrapped inside a body, instead, they should be directly located in the worldbody. But for robot like ant, because its whole body is independent of the worldbody, it should be wrapped within a main body with a freejoint. That might be a really trick part of Brax.

I haven't tried this yet, but to my understanding, part of the reason why this might be difficult to do is that link0 is a body with inertial properties that have to be non-trivially specified. On the other hand, in MJCFs, the worldbody can't have inertial properties. This is one reason why the fixed joint type exists in the first place.

Dear alberthli,

Would you mind sharing your modified Franka panda with me? I met other issues with the actuators. Thank you adcance.

alberthli commented 10 months ago

@baixianger See below. All the assets for this MJCF can be found here.

<mujoco model="panda nohand">
  <compiler angle="radian" meshdir="assets" autolimits="true"/>

  <default>
    <default class="panda">
      <material specular="0.5" shininess="0.25"/>
      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973" type="hinge"/>
      <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.04"/>
      </default>

      <default class="panda/visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2" margin="1.0" gap="1.0"/>
      </default>
      <default class="panda/collision">
        <geom type="mesh" group="3" margin="1.0" gap="1.0"/>
      </default>
      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
    </default>
  </default>

  <asset>
    <material class="panda" name="white" rgba="1 1 1 1"/>
    <material class="panda" name="off_white" rgba="0.901961 0.921569 0.929412 1"/>
    <material class="panda" name="dark_grey" rgba="0.25 0.25 0.25 1"/>
    <material class="panda" name="green" rgba="0 1 0 1"/>
    <material class="panda" name="light_blue" rgba="0.039216 0.541176 0.780392 1"/>

    <!-- Collision meshes -->
    <mesh name="link0_c" file="link0.stl"/>
    <mesh name="link1_c" file="link1.stl"/>
    <mesh name="link2_c" file="link2.stl"/>
    <mesh name="link3_c" file="link3.stl"/>
    <mesh name="link4_c" file="link4.stl"/>
    <mesh name="link5_c0" file="link5_collision_0.obj"/>
    <mesh name="link5_c1" file="link5_collision_1.obj"/>
    <mesh name="link5_c2" file="link5_collision_2.obj"/>
    <mesh name="link6_c" file="link6.stl"/>
    <mesh name="link7_c" file="link7.stl"/>

    <!-- 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"/>
  </asset>

  <worldbody>
    <body name="link0" childclass="panda">
      <joint name="joint0" type="free"/>
      <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
        fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
      <geom mesh="link0_0" material="off_white" class="panda/visual"/>
      <geom mesh="link0_1" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_2" material="off_white" class="panda/visual"/>
      <geom mesh="link0_3" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_4" material="off_white" class="panda/visual"/>
      <geom mesh="link0_5" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_7" material="white" class="panda/visual"/>
      <geom mesh="link0_8" material="white" class="panda/visual"/>
      <geom mesh="link0_9" material="dark_grey" class="panda/visual"/>
      <geom mesh="link0_10" material="off_white" class="panda/visual"/>
      <geom mesh="link0_11" material="white" class="panda/visual"/>
      <geom mesh="link0_c" class="panda/collision"/>
      <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 material="white" mesh="link1" class="panda/visual"/>
        <geom mesh="link1_c" class="panda/collision"/>
        <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 material="white" mesh="link2" class="panda/visual"/>
          <geom mesh="link2_c" class="panda/collision"/>
          <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" material="white" class="panda/visual"/>
            <geom mesh="link3_1" material="white" class="panda/visual"/>
            <geom mesh="link3_2" material="white" class="panda/visual"/>
            <geom mesh="link3_3" material="dark_grey" class="panda/visual"/>
            <geom mesh="link3_c" class="panda/collision"/>
            <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" material="white" class="panda/visual"/>
              <geom mesh="link4_1" material="white" class="panda/visual"/>
              <geom mesh="link4_2" material="dark_grey" class="panda/visual"/>
              <geom mesh="link4_3" material="white" class="panda/visual"/>
              <geom mesh="link4_c" class="panda/collision"/>
              <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" material="dark_grey" class="panda/visual"/>
                <geom mesh="link5_1" material="white" class="panda/visual"/>
                <geom mesh="link5_2" material="white" class="panda/visual"/>
                <geom mesh="link5_c0" class="panda/collision"/>
                <geom mesh="link5_c1" class="panda/collision"/>
                <geom mesh="link5_c2" class="panda/collision"/>
                <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" material="off_white" class="panda/visual"/>
                  <geom mesh="link6_1" material="white" class="panda/visual"/>
                  <geom mesh="link6_2" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_3" material="white" class="panda/visual"/>
                  <geom mesh="link6_4" material="white" class="panda/visual"/>
                  <geom mesh="link6_5" material="white" class="panda/visual"/>
                  <geom mesh="link6_6" material="white" class="panda/visual"/>
                  <geom mesh="link6_7" material="light_blue" class="panda/visual"/>
                  <geom mesh="link6_8" material="light_blue" class="panda/visual"/>
                  <geom mesh="link6_9" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_10" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_11" material="white" class="panda/visual"/>
                  <geom mesh="link6_12" material="green" class="panda/visual"/>
                  <geom mesh="link6_13" material="white" class="panda/visual"/>
                  <geom mesh="link6_14" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_15" material="dark_grey" class="panda/visual"/>
                  <geom mesh="link6_16" material="white" class="panda/visual"/>
                  <geom mesh="link6_c" class="panda/collision"/>
                  <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" material="white" class="panda/visual"/>
                    <geom mesh="link7_1" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_2" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_3" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_4" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_5" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_6" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link7_7" material="white" class="panda/visual"/>
                    <geom mesh="link7_c" class="panda/collision"/>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <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="3500" biasprm="0 -3500 -350"/>
    <general class="panda" name="actuator4" joint="joint4" gainprm="3500" biasprm="0 -3500 -350"
      ctrlrange="-3.0718 -0.0698"/>
    <general class="panda" name="actuator5" joint="joint5" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
    <general class="panda" name="actuator6" joint="joint6" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"
      ctrlrange="-0.0175 3.7525"/>
    <general class="panda" name="actuator7" joint="joint7" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
  </actuator>

  <keyframe>
    <key name="home" qpos="0 0 0 -1.57079 0 1.57079 -0.7853" ctrl="0 0 0 -1.57079 0 1.57079 -0.7853"/>
  </keyframe>
</mujoco>