Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
alberthli opened this issue Aug 2, 2023 · 7 comments
Open

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

alberthli opened this issue Aug 2, 2023 · 7 comments
Assignees
@btaba

Comments

@alberthli
Copy link

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

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
Copy link
Author

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

baixianger commented Aug 10, 2023 •
edited
Loading

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

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
Copy link
Author

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

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
Copy link
Author

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

@btaba btaba self-assigned this Oct 20, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@btaba btaba

Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@btaba @alberthli @baixianger