search

google-deepmind / mujoco

Multi-Joint dynamics with Contact. A general purpose physics simulator.
https://mujoco.org
Apache License 2.0
8.25k stars 823 forks source link

Body-twist: Which frame are they being calculated in? #2210

Closed ajoardar007 closed 1 week ago

ajoardar007 commented 2 weeks ago

Intro

Hi!

I am a graduate student at the University of Waterloo, and I am using MuJoCo for my research on Force-Position control. I am currently trying to match my calculated body-twists against those calculated by MuJoCo. However, I am not sure if the twists being reported for a body are in the body-frame or not.

My end-goal is to be able to compute the dynamic-contribution of the EE on the Force-Torque sensor, so as to subtract this from the readings and only get the value of any externally-supplied wrench on the EE. This will help me perform Force-Position manipulation on the environment. Once my Force-Torque compensator is shown to be valid, I intend to port this over to the actual-robot.

My setup

MuJoCo version: 3.2.3, API: Python, Architecture: x86_64, OS: Ubuntu 20.04.6

What's happening? What did you expect?

Expectation: When asking a body to rotate about its origin's Z-axis at some angular-velocity (no linear-velocity component), I expected the body-twist of another frame attached to the same body to have a linear-velocity component. Reality: Both the origin-frame and the other frame have the same body-twist value. Question: What exactly is being outputted at data.body("some_frame").cvel? Is this not a body-twist (see definition here)? In issue #1866, I see that MuJoCo does not compute the body-jacobian, but another form of the jacobian. In issue #691, while it is mentioned that the angular-velocity provided by data.body("some_frame").cvel (top 3 values) are in the local frame, nothing is mentioned about the linear-velocity.

Steps for reproduction

  1. In a folder, load the following files: a. The python file incorrect_body_twist.py (script provided below), b. Extract the meshes folder in the attached zip-file.
  2. Run the code below.
  3. Observe the outputted twist-values.

Minimal model for reproduction

meshes.zip

Code required for reproduction

import mujoco
import mediapy as media

xml = """
<mujoco model="playground_wagon_manipulation(v1.0.0)">
    <!--#########################
        ### Compiler Specific ###
        ######################### -->
    <compiler coordinate="local" angle="radian" meshdir="/home/arnab/UWARL_catkin_ws/src/uwarl-mujoco-summit-wam-sim/src/meshes/"/>

    <!-- Options: -->
    <option 
        timestep="1e-3"
        iterations="10"
        tolerance="1e-10"
        impratio="1"
        gravity="0 0 -9.81"
        solver="Newton"
        cone="elliptic" 
    />
    <default>
        <geom margin="0.001" solimp="0.99 0.99 0.01" solref="0.01 1" condim="4" /> 
    </default>
    <!-- Statistic -->
    <!-- https://mujoco.readthedocs.io/en/latest/XMLreference.html#statistic -->
    <!-- Looka at this `center` point using the free-camera, which is located at a distance of 1.5x`extent` distance -->
    <!-- Apparently, the camera cannot zoom to within the extent distance, so keep it small. -->
    <!-- The value of `center` does not move the initial location of the camera view-point.-->
    <statistic center="0 0 0" extent="1"/>
    <!-- Visual -->
    <visual>
        <map fogstart="3" fogend="5" force="0.1" znear="0.001"/>
        <quality shadowsize="1024" offsamples="16"/>
        <scale forcewidth="0.05" contactwidth="0.05" contactheight="0.05" 
               connect="0.05" com="0.1" jointlength="0.1" jointwidth="0.01"
               framelength="0.1" framewidth="0.01"/>
        <!-- <global offwidth="1960" offheight="800"/> -->
    </visual>

    <!--############################
        ### Environment Specific ###
        ############################ -->
    <asset>
        <!-- [ENV] Ground PLane -->
        <texture name="groundplane" type="2d" builtin="checker" rgb1=".25 .26 .25" 
            rgb2=".22 .22 .22" width="100" height="100" mark="edge" markrgb=".3 .3 .3"/>
        <material name="MatGnd" texture="groundplane"  texrepeat="5 5" specular="1" shininess=".001" reflectance="0.00001"/>

        <mesh file="meshes_summit/base_link_summit.stl"/>
        <mesh file="meshes_summit/mecanum_LF_1.stl"/>
        <mesh file="meshes_summit/mecanum_LR_1.stl"/>
        <mesh file="meshes_summit/mecanum_RF_1.stl"/>
        <mesh file="meshes_summit/mecanum_RR_1.stl"/>
        <mesh file="meshes_summit/arm_rest.stl"/>
        <mesh file="meshes_sensors/intel_realsense_l515.stl"/>
        <mesh file="meshes_wam/base_link_fine.stl"/>
        <mesh file="meshes_wam/base_link_convex.stl"/>
        <mesh file="meshes_wam/shoulder_link_fine.stl"/>
        <mesh file="meshes_wam/shoulder_link_convex_decomposition_p1.stl"/>
        <mesh file="meshes_wam/shoulder_link_convex_decomposition_p2.stl"/>
        <mesh file="meshes_wam/shoulder_link_convex_decomposition_p3.stl"/>
    </asset>

    <!--##################
        ### Extension: ###
        ################## -->
    <extension>
    </extension>

    <!--##################
        ### Actuators: ###
        ################## -->
    <!-- [SUMMIT] 
        - Note:
            - The configuration below may not reflect the actual robot
    -->
    <actuator>
        <!-- [SUMMIT] control summit base in a effort fashion (Tim van Meijel) -->
        <motor name="smt/pose/x"  joint="smt/pose/x" ctrllimited="true" ctrlrange="-10.0 10.0" gear="100.0"/>
        <motor name="smt/pose/y"  joint="smt/pose/y" ctrllimited="true" ctrlrange="-10.0 10.0" gear="100.0"/>
        <motor name="smt/orie/z"  joint="smt/orie/z" ctrllimited="true" ctrlrange="-10.0 10.0" gear="50.0"/>
    </actuator>

    <!-- [WAM 7DOF] 
        - Note:
            - The configuration below may not reflect the actual robot
            - Position gain is based on the config file hosted in WAM PC
    -->
    <actuator>
        <!-- 
            Attributes Descriptions:
            - SPEC: https://web.barrett.com/support/WAM_Documentation/WAM_InertialSpecifications_AC-02.pdf
            - gear: scakes the length of the actuator (different gain / moment arms and velocity, force)
        -->
        <!-- [WAM 7DOF] force control mode -->
        <!-- Limits initialized by the URDF Files!! -->
        <motor name='wam/J1/F' ctrllimited="true" joint='wam/J1' ctrlrange='-80 80' gear="42"      />
    </actuator>

    <!--###############
        ### Config: ###
        ############### -->
    <!-- DEFAULT -->
    <default>
        <!-- The class-name being specified under default maps back to the class-names being specified for a body (childclass/class) , or  -->
        <!-- These names have to be unique; it must not clash with class-names for un-intended bodies/joints/geometries -->
        <default class="summit">
        </default>
        <default class="summit/whl">
            <!-- class-names for joints -->
            <joint axis="1 0 0" damping="0.55"/>
            <!-- Bydefault, hinge-type is the default joint type -->
            <!-- https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-joint -->
        </default>
        <default class="summit/col">
            <!-- class-names for geometry-type -->
            <geom contype="1" conaffinity="1" group="0" rgba="0.5 0.6 0.7 0.3"/>
        </default>
        <default class="summit/body/viz">
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.6 0.6 0.6 1"/>
        </default>
        <default class="summit/wheel/viz">
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.3 0.3 0.3 1"/>
        </default>
        <default class="summit/camera/viz">
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1"/>
        </default>

        <default class="wam">
        </default>
        <default class="wam/col">
            <geom type="mesh" contype="1" conaffinity="1" group="0" rgba="0.5 0.6 0.7 0.6"/>
        </default>
        <default class="wam/viz">
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1"/>
        </default>
        <default class="wam/shoulder_yaw_link/viz">
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.8 0.8 0.8 1"/>
        </default>
    </default>

    <worldbody>
        <!-- GND -->
        <light directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular=".1 .1 .1" pos=".1 .2 1.3" dir="-.1 -.2 -1.3"/>
        <geom name="ground" type="plane" pos="0 0 0" size="24 24 1" conaffinity="1" contype="1" material="MatGnd" friction="0.0001 0.001 0.001" group="1"/>
        <body name="waterloo_steel" pos="0.8 0 0" euler="0 0 -1.57079632679">
            <body name="smt/base_link" childclass="summit" pos="0 0 0">
                <!-- === BASE LINK === === === === -->
                <!-- mobile body -->
                <inertial                                           pos="0 0 0.37" mass="125" diaginertia="1.391 6.853 6.125" />
                <geom name="smt/viz/base_link"                      class="summit/body/viz" mesh="base_link_summit"/>

                <!-- Front Camera -->
                <body name="smt/front/camera" pos="0 0.362 0.373" quat="0 0 0.707107 0.707107">
                    <inertial               pos="0 0 0.015" mass="0.095"/>
                    <geom                   class="summit/camera/viz" size="0.06 0.04" mesh="intel_realsense_l515"/>
                    <camera name="smt/front/camera/intel/rgb" pos="0 0 0" quat="0 0 1 0" fovy="55"/>
                </body>
                <!-- Rear Camera --><!-- quant=w x y z -->
                <body name="smt/rear/camera" pos="0 -0.362 0.373" quat="0.707107 0.707107 0 0"> 
                    <inertial               pos="0 0 0.015" mass="0.095"/>
                    <geom                   class="summit/camera/viz" size="0.06 0.04" mesh="intel_realsense_l515"/>
                    <camera name="smt/rear/camera/intel/rgb" pos="0 0 0" quat="0 0 1 0" fovy="55"/>
                </body>
                <!-- Simplified the collision into boxes + arm rest -->
                <geom name="smt/col/base_link/box_body"             class="summit/col" pos="0 0 0.42" size="0.24 0.32 0.16" type="box"/>
                <geom name="smt/col/base_link/arm_rest"             class="summit/col" pos="0 0.098 0.55" quat="0.5 0.5 -0.5 -0.5" mesh="arm_rest"/>
                <geom name="smt/col/base_link/cylinder_lidar"       class="summit/col" pos="0.246 0.362 0.313" size="0.06 0.044" type="cylinder"/>

                <!-- world placement -->
                <joint name="smt/world_x" armature="0.0001" axis="0 1 0" damping="1e+11" pos="0 0 0" type="slide"></joint>
                <joint name="smt/world_y" armature="0.0001" axis="-1 0 0" damping="1e+11" pos="0 0 0" type="slide"></joint>
                <joint name="smt/world_z" armature="0.0001" axis="0 0 1" damping="1e+0"  pos="0 0 0" type="slide"></joint>

                <!-- control summit base -->
                <joint name="smt/pose/x"                            type="slide" pos="0 0 0.4" axis="0 1 0"  damping="15"/>
                <joint name="smt/pose/y"                            type="slide" pos="0 0 0.4" axis="-1 0 0"  damping="15"/>
                <joint name="smt/orie/z"                            type="hinge" pos="0 0 0.4" axis="0 0 1"  damping="10"/>

                <!-- === whl LINK === === === === -->
                <!-- Mobile whls -->
                <body name="smt/whl/LF_link" pos="0 0 0">
                    <geom name="smt/viz/whl/LF"                    class="summit/wheel/viz" mesh="mecanum_LF_1"/>
                    <geom name="smt/col/whl/LF_cylinder"           class="summit/col" pos="-0.220 0.222 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                         pos="-0.220 0.222 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_LF"                       class="summit/whl" pos="-0.264 0.222 0.128"/>
                </body>
                <body name="smt/whl/LR_link" pos="0 0 0">
                    <geom name="smt/viz/whl/LR"                    class="summit/wheel/viz" mesh="mecanum_LR_1"/>
                    <geom name="smt/col/whl/LR_cylinder"           class="summit/col" pos="-0.220 -0.223 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                         pos="-0.220 -0.223 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_LR"                       class="summit/whl" pos="-0.264 -0.223 0.128"/>
                </body>
                <body name="smt/whl/RF_link" pos="0 0 0">
                    <geom name="smt/viz/whl/RF"                   class="summit/wheel/viz" mesh="mecanum_RF_1"/>
                    <geom name="smt/col/whl/RF_cylinder"          class="summit/col" pos="0.220 0.222 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                        pos="0.220 0.222 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_RF"                      class="summit/whl" pos="0.264 0.222 0.128"/>
                </body>
                <body name="smt/whl/RR_link" pos="0 0 0">
                    <geom name="smt/viz/whl/RR"                   class="summit/wheel/viz" mesh="mecanum_RR_1"/>
                    <geom name="smt/col/whl/RR_cylinder"          class="summit/col" pos="0.220 -0.223 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                        pos="0.220 -0.223 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_RR"                      class="summit/whl" pos="0.264 -0.223 0.128"/>
                </body>

                <!-- === WAM 7DOF HAND LINK === === === === -->
                <!-- Place WAM Module Here -->
                <!-- WAM -->
                <body name="wam/base_link" childclass="wam" pos="0 0.14 0.405" quat="0.707 0 0 0.707">
                    <body name="wam/base" childclass="wam" pos="0 0 0">
                        <inertial   pos="-0.14071720 -0.02017671 0.07995294" mass="9.97059584" fullinertia="0.11760385 0.10916849 0.18294303 0.02557874 0.00161433 0.00640270" /> 
                        <geom       class="wam/viz" mesh="base_link_fine"/>
                        <geom       class="wam/col" mesh="base_link_convex"/>
                    </body>
                    <body name="wam/shoulder_yaw_link" pos="0 0 0.346">
                        <inertial               pos="-0.00443422 -0.00066489 -0.12189039" mass="10.76768767" fullinertia="0.13488033 0.09046330 0.11328369 -0.00012485 0.00213041 -0.00068555" />
                        <joint name="wam/J1"    range="-2.6 2.6" damping="1000" frictionloss="1000" type="hinge" limited="true" pos="0 0 0" axis="0 0 1"/>
                        <geom                   class="wam/shoulder_yaw_link/viz" mesh="shoulder_link_fine"/>
                        <geom                   class="wam/col" mesh="shoulder_link_convex_decomposition_p1"/>
                        <geom                   class="wam/col" mesh="shoulder_link_convex_decomposition_p2"/>
                        <geom                   class="wam/col" mesh="shoulder_link_convex_decomposition_p3"/>

                    </body>
                </body>
            </body>
        </body>
    </worldbody>
    <!-- CONTACT -->
    <contact>
    <!-- NOTE: Exclude overlapping collision mesh to be used in contact physics.  -->
        <exclude body1="smt/base_link" body2="smt/whl/LF_link"/>
        <exclude body1="smt/base_link" body2="smt/whl/RF_link"/>
        <exclude body1="smt/base_link" body2="smt/whl/RR_link"/>
        <exclude body1="smt/base_link" body2="smt/whl/LR_link"/>

        <exclude body1="wam/base" body2="wam/shoulder_yaw_link"/>
    </contact>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
# enable joint visualization option:
scene_option = mujoco.MjvOption()
scene_option.flags[mujoco.mjtVisFlag.mjVIS_JOINT] = True

duration = 3  # (seconds)
framerate = 60  # (Hz)

# Simulate and display video.
frames = []
mujoco.mj_resetData(model, data)
with mujoco.Renderer(model) as renderer:
  while data.time < duration:
    # Give joint commands
    data.actuator('smt/orie/z').ctrl = 0.1
    # data.actuator('smt/pose/x').ctrl = 0.1
    # data.actuator('wam/J1/F').ctrl = 42*(0.1-data.joint('wam/J1').qvel[0])
    mujoco.mj_step(model, data)
    if len(frames) < data.time * framerate:
      renderer.update_scene(data, scene_option=scene_option)
      pixels = renderer.render()
      frames.append(pixels)
# Print twist of the MB and the Shoulder-link
print('smt/base_link')
print(data.body('smt/base_link').xpos)
print(data.body('smt/base_link').xquat)
print(data.body('smt/base_link').cvel)
print('wam/base_link')
print(data.body('wam/base_link').xpos)
print(data.body('wam/base_link').xquat)
print(data.body('wam/base_link').cvel)
print('wam/shoulder_yaw_link')
print(data.body('wam/shoulder_yaw_link').xpos)
print(data.body('wam/shoulder_yaw_link').xquat)
print(data.body('wam/shoulder_yaw_link').cvel)
print("MB Yaw-rate")
print(data.joint('smt/orie/z').qvel)
print("Wam Shoulder yaw-Link Yaw-pos")
print(data.joint('wam/J1').qpos)
print("Wam Shoulder yaw-Link Yaw-rate")
print(data.joint('wam/J1').qvel)

print("Hey!")
media.show_video(frames, fps=framerate)

Confirmations

ajoardar007 commented 2 weeks ago

Forgot to attach the output:

smt/base_link
pos = [ 0.80081408 -0.0043481   0.00299323]
quat = [ 0.89249014  0.          0.         -0.4510669 ]
twist = [ 0.00000000e+00  0.00000000e+00  2.98476055e-01 -6.40404659e-05
  3.52216903e-04  7.67892825e-04]
wam/base_link
pos = [0.91353446 0.07868272 0.40799323]
quat = [0.95003829 0.         0.         0.31213337]
twist = [ 0.00000000e+00  0.00000000e+00  2.98476055e-01 -6.40404659e-05
  3.52216903e-04  7.67892825e-04]
wam/shoulder_yaw_link
pos = [0.91353446 0.07868272 0.75399323]
quat = [0.95004274 0.         0.         0.31211982]
twist = [ 0.00000000e+00  0.00000000e+00  2.98474390e-01 -6.41726430e-05
  3.52393197e-04  7.67892825e-04]
MB Yaw-rate
[0.29849468]
Wam Shoulder yaw-Link Yaw-pos
[-2.85131327e-05]
Wam Shoulder yaw-Link Yaw-rate
[-1.66994597e-06]

Question: Why is the twist for both the smt/base_link and the wam/base_link the same?

ajoardar007 commented 2 weeks ago

Upon subtracting the joint yaw-rate of smt/base_link (self.data.joint('smt/orie/z').qvel[0]) from the yaw-rate component of smt/base_link's twist, I see an error of 0.06 rad/sec sometimes when it is in motion. And when I did the same for wam/shoulder_yaw_link (subtract self.data.joint('smt/orie/z').qvel[0] and self.data.joint('wam/J1').qvel[0] from the yaw-rate twist-component of wam/shoulder_yaw_link), I observe errors of 0.26 rad/sec sometimes when it is in motion.

Is it because the joint is considered a flexible-element? How are the twists being calculated then? If they are using Jacobians to compute it, then the errors in the above examples should be 0.

yuvaltassa commented 2 weeks ago

Are these notes helpful? https://mujoco.readthedocs.io/en/latest/APIreference/APItypes.html#c-frame-variables https://mujoco.readthedocs.io/en/latest/programming/simulation.html#coordinate-frames-and-transformations

ajoardar007 commented 2 weeks ago

Hello @yuvaltassa, Thanks a ton for pointing out the second link. From the last half of the last paragraph on that page, I understand that the quantity cvel at a frame is computed w.r.t the coordinate-frame of the Kinematic-chains' CoM (center of mass). In the example presented there, the queried-frame was the origin of the spinning body, and it was spinning about one of the axis of the origin-frame, not off-setted from it. This is why the author assumes that the reader might interpret the computed twist to have no linear-velocity and only angular-velocity.

If that is the case, then the cvel twist provided at two-different frames on the same body, of which one of the frames does not lie along the axis of spin, should be different, correct?

yuvaltassa commented 2 weeks ago

Your question doesn't entirely make sense to me. What is "cvel twist at two different frames"?

cvel is in a specific frame, as described. What are these two frames?

ajoardar007 commented 2 weeks ago

Having gone through your comments here, I will be attaching the requisite sensors for all data that I am relying upon to perform controls. It is best to have all the incoming data at the same time-step. The twist I should be getting from velocimeter and gyroscope , when attached to the same-site, should be equal to the product of the body-jacobian and the joint-velocities, right? That should be how the velocimeter and gyroscope sensor-data are computed. That is all I really need.

ajoardar007 commented 2 weeks ago

I can query cvel for frame-A (the origin of body-A | smt/base_link), and I can query cvel for frame-B (the origin of body-B | wam/base_link). In my case, body-B is rigidly attached to body-A. So why does the cvel value of body-A = cvel value of body-B when they body-B's frame does not lie along the spin-axis of body-A?

yuvaltassa commented 2 weeks ago

If you make a minimal model that precisely explains what you are asking, I will take a look. However, I suspect that if you do that then you will figure it out yourself.

PS, note that function docs now link to the definitions, just click on the function name and read the implementation in the source. These are the relevant functions: https://mujoco.readthedocs.io/en/latest/APIreference/APIfunctions.html#mj-compos https://mujoco.readthedocs.io/en/latest/APIreference/APIfunctions.html#mj-comvel

ajoardar007 commented 2 weeks ago

In the below-script, I have emulated a cuboid-box smt/base_link on 4 wheels, with a up-right cylinder wam/base_link on the left. I have given it a counter-clockwise spin about the Z-axis of the cuboid-box. The outputted cvel for both bodies at the end of the 3-second simulation is printed. Please copy and run the below script in its entirety in this cell of the MuJoCo Collab Tutorial notebook (after having run all the previous cells). No meshes are required.

Question: Why is the cvel for smt/base_link and wam/base_link the same? While the twist at the origin of smt/base_link is purely rotational, the twist at the origin of wam/base_link should have a non-zero translational velocity as-well (a lot larger than 1 mm/sec).

import mujoco
import mediapy as media

xml = """
<mujoco model="playground_wagon_manipulation(v1.0.0)">
    <!--#########################
        ### Compiler Specific ###
        ######################### -->
    <compiler coordinate="local" angle="radian"/>

    <!-- Options: -->
    <option 
        timestep="1e-3"
        iterations="10"
        tolerance="1e-10"
        impratio="1"
        gravity="0 0 -9.81"
        solver="Newton"
        cone="elliptic" 
    />
    <default>
        <geom margin="0.001" solimp="0.99 0.99 0.01" solref="0.01 1" condim="4" /> 
    </default>
    <!-- Statistic -->
    <!-- https://mujoco.readthedocs.io/en/latest/XMLreference.html#statistic -->
    <!-- Looka at this `center` point using the free-camera, which is located at a distance of 1.5x`extent` distance -->
    <!-- Apparently, the camera cannot zoom to within the extent distance, so keep it small. -->
    <!-- The value of `center` does not move the initial location of the camera view-point.-->
    <statistic center="0 0 0" extent="1"/>
    <!-- Visual -->
    <visual>
        <map fogstart="3" fogend="5" force="0.1" znear="0.001"/>
        <quality shadowsize="1024" offsamples="16"/>
        <scale forcewidth="0.05" contactwidth="0.05" contactheight="0.05" 
               connect="0.05" com="0.1" jointlength="0.1" jointwidth="0.01"
               framelength="0.1" framewidth="0.01"/>
        <!-- <global offwidth="1960" offheight="800"/> -->
    </visual>

    <!--############################
        ### Environment Specific ###
        ############################ -->
    <asset>
        <!-- [ENV] Ground PLane -->
        <texture name="groundplane" type="2d" builtin="checker" rgb1=".25 .26 .25" 
            rgb2=".22 .22 .22" width="100" height="100" mark="edge" markrgb=".3 .3 .3"/>
        <material name="MatGnd" texture="groundplane"  texrepeat="5 5" specular="1" shininess=".001" reflectance="0.00001"/>
    </asset>

    <!--##################
        ### Extension: ###
        ################## -->
    <extension>
    </extension>

    <!--##################
        ### Actuators: ###
        ################## -->
    <!-- [SUMMIT] 
        - Note:
            - The configuration below may not reflect the actual robot
    -->
    <actuator>
        <!-- [SUMMIT] control summit base in a effort fashion (Tim van Meijel) -->
        <motor name="smt/pose/x"  joint="smt/pose/x" ctrllimited="true" ctrlrange="-10.0 10.0" gear="100.0"/>
        <motor name="smt/pose/y"  joint="smt/pose/y" ctrllimited="true" ctrlrange="-10.0 10.0" gear="100.0"/>
        <motor name="smt/orie/z"  joint="smt/orie/z" ctrllimited="true" ctrlrange="-10.0 10.0" gear="50.0"/>
    </actuator>

    <!--###############
        ### Config: ###
        ############### -->
    <!-- DEFAULT -->
    <default>
        <default class="summit/whl">
            <!-- class-names for joints -->
            <joint axis="1 0 0" damping="0.55"/>
            <!-- Bydefault, hinge-type is the default joint type -->
            <!-- https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-joint -->
        </default>
        <default class="summit/col">
            <!-- class-names for geometry-type -->
            <geom contype="1" conaffinity="1" group="0" rgba="0.5 0.6 0.7 0.3"/>
        </default>
        <default class="summit/body/viz">
            <geom contype="0" conaffinity="0" group="1" rgba="0.6 0.6 0.6 1"/>
        </default>
        <default class="summit/wheel/viz">
            <geom contype="0" conaffinity="0" group="1" rgba="0.3 0.3 0.3 1"/>
        </default>

        <default class="wam/col">
            <geom type="mesh" contype="1" conaffinity="1" group="0" rgba="0.5 0.6 0.7 0.6"/>
        </default>
        <default class="wam/viz">
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1"/>
        </default>
    </default>

    <worldbody>
        <!-- GND -->
        <light directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular=".1 .1 .1" pos=".1 .2 1.3" dir="-.1 -.2 -1.3"/>
        <geom name="ground" type="plane" pos="0 0 0" size="24 24 1" conaffinity="1" contype="1" material="MatGnd" friction="0.0001 0.001 0.001" group="1"/>
        <body name="waterloo_steel" pos="0 0 0" euler="0 0 -1.57079632679">
            <body name="smt/base_link" pos="0 0 0">
                <!-- === BASE LINK === === === === -->
                <!-- mobile body -->
                <inertial                                           pos="0 0 0.37" mass="125" diaginertia="1.391 6.853 6.125" />
                <geom name="smt/viz/base_link"                      class="summit/body/viz" pos="0 0 0.42" size="0.24 0.32 0.16" type="box"/>

                <!-- Simplified the collision into boxes + arm rest -->
                <geom name="smt/col/base_link/box_body"             class="summit/col" pos="0 0 0.42" size="0.24 0.32 0.16" type="box"/>

                <!-- world placement -->
                <joint name="smt/world_x" armature="0.0001" axis="1 0 0" damping="1e+11" pos="0 0 0" type="slide"></joint>
                <joint name="smt/world_y" armature="0.0001" axis="0 1 0" damping="1e+11" pos="0 0 0" type="slide"></joint>
                <joint name="smt/world_z" armature="0.0001" axis="0 0 1" damping="1e+0"  pos="0 0 0" type="slide"></joint>

                <!-- control summit base -->
                <joint name="smt/pose/x"                            type="slide" pos="0 0 0.4" axis="1 0 0"  damping="15"/>
                <joint name="smt/pose/y"                            type="slide" pos="0 0 0.4" axis="0 1 0"  damping="15"/>
                <joint name="smt/orie/z"                            type="hinge" pos="0 0 0.4" axis="0 0 1"  damping="10"/>

                <!-- === whl LINK === === === === -->
                <!-- Mobile whls -->
                <body name="smt/whl/LF_link" pos="0 0 0">
                    <geom name="smt/viz/whl/LF"                    class="summit/wheel/viz" pos="-0.220 0.222 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <geom name="smt/col/whl/LF_cylinder"           class="summit/col" pos="-0.220 0.222 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                         pos="-0.220 0.222 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_LF"                       class="summit/whl" pos="-0.264 0.222 0.128"/>
                </body>
                <body name="smt/whl/LR_link" pos="0 0 0">
                    <geom name="smt/viz/whl/LR"                    class="summit/wheel/viz" pos="-0.220 -0.223 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <geom name="smt/col/whl/LR_cylinder"           class="summit/col" pos="-0.220 -0.223 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                         pos="-0.220 -0.223 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_LR"                       class="summit/whl" pos="-0.264 -0.223 0.128"/>
                </body>
                <body name="smt/whl/RF_link" pos="0 0 0">
                    <geom name="smt/viz/whl/RF"                   class="summit/wheel/viz" pos="0.220 0.222 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <geom name="smt/col/whl/RF_cylinder"          class="summit/col" pos="0.220 0.222 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                        pos="0.220 0.222 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_RF"                      class="summit/whl" pos="0.264 0.222 0.128"/>
                </body>
                <body name="smt/whl/RR_link" pos="0 0 0">
                    <geom name="smt/viz/whl/RR"                   class="summit/wheel/viz" pos="0.220 -0.223 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <geom name="smt/col/whl/RR_cylinder"          class="summit/col" pos="0.220 -0.223 0.128" quat="0.707107 0 0.707106 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
                    <inertial                                                        pos="0.220 -0.223 0.128" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/>
                    <joint name="smt/whl_RR"                      class="summit/whl" pos="0.264 -0.223 0.128"/>
                </body>

                <!-- === WAM 7DOF HAND LINK === === === === -->
                <!-- Place WAM Module Here -->
                <!-- WAM -->
                <body name="wam/base_link" pos="0 -0.24 0.505" quat="0.707 0 0 0.707">
                    <inertial   pos="-0.14071720 -0.02017671 0.07995294" mass="9.97059584" fullinertia="0.11760385 0.10916849 0.18294303 0.02557874 0.00161433 0.00640270" /> 
                    <geom       class="wam/viz" type="cylinder" size="0.1 0.1" rgba="0.7 0 0 1"/>
                    <geom       class="wam/col" type="cylinder" size="0.1 0.1"/>
                </body>
            </body>
        </body>
    </worldbody>
    <!-- CONTACT -->
    <contact>
    <!-- NOTE: Exclude overlapping collision mesh to be used in contact physics.  -->
        <exclude body1="smt/base_link" body2="smt/whl/LF_link"/>
        <exclude body1="smt/base_link" body2="smt/whl/RF_link"/>
        <exclude body1="smt/base_link" body2="smt/whl/RR_link"/>
        <exclude body1="smt/base_link" body2="smt/whl/LR_link"/>

        <exclude body1="smt/base_link" body2="wam/base_link"/>
    </contact>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
# enable joint visualization option:
scene_option = mujoco.MjvOption()
scene_option.flags[mujoco.mjtVisFlag.mjVIS_JOINT] = True

duration = 3  # (seconds)
framerate = 60  # (Hz)

# Simulate and display video.
frames = []
mujoco.mj_resetData(model, data)
with mujoco.Renderer(model) as renderer:
  while data.time < duration:
    # Give joint commands
    data.actuator('smt/orie/z').ctrl = 0.1
    # data.actuator('smt/pose/x').ctrl = 0.1
    mujoco.mj_step(model, data)
    if len(frames) < data.time * framerate:
      renderer.update_scene(data, scene_option=scene_option)
      pixels = renderer.render()
      frames.append(pixels)

# Print twist of the MB and the Shoulder-link
print('smt/base_link')
print("Position of smt/base_link's origin w.r.t world-frame: "+str(data.body('smt/base_link').xpos))
print("Orientation of smt/base_link's frame w.r.t world-frame: "+str(data.body('smt/base_link').xquat))
print("Twist at smt/base_link's frame w.r.t Kinematic chain's CoM frame: "+str(data.body('smt/base_link').cvel))
print('wam/base_link')
print("Position of wam/base_link's origin w.r.t world-frame: "+str(data.body('wam/base_link').xpos))
print("Orientation of wam/base_link's frame w.r.t world-frame: "+str(data.body('wam/base_link').xquat))
print("Twist at wam/base_link's frame w.r.t Kinematic chain's CoM frame: "+str(data.body('wam/base_link').cvel))

print("MB Yaw-rate: "+str(data.joint('smt/orie/z').qvel))

media.show_video(frames, fps=framerate)

Output:

smt/base_link
Position of smt/base_link's origin w.r.t world-frame: [-0.005  0.011  0.003]
Orientation of smt/base_link's frame w.r.t world-frame: [ 0.892  0.     0.    -0.453]
Twist at smt/base_link's frame w.r.t Kinematic chain's CoM frame: [ 0.     0.     0.303  0.    -0.001  0.   ]
wam/base_link
Position of wam/base_link's origin w.r.t world-frame: [-0.198 -0.13   0.508]
Orientation of wam/base_link's frame w.r.t world-frame: [0.951 0.    0.    0.31 ]
Twist at wam/base_link's frame w.r.t Kinematic chain's CoM frame: [ 0.     0.     0.303  0.    -0.001  0.   ]
MB Yaw-rate: [0.303]
yuvaltassa commented 1 week ago

The reason they are the same is that cvel is w.r.t the com of the parent subtree and since these bodies have no dofs between them, the com of the parent subtree is the same. The question is why are you not following the advice in the link I posted above:

In general, the user should avoid working with such quantities directly. Instead use the functions mj_objectVelocity, mj_objectAcceleration and the low-level mju_transformSpatial to obtain linear and angular velocities, accelerations and forces for a given body.

The "c" variables are basically internal quantities. Don't use them. Use the API.

ajoardar007 commented 1 week ago

Yes, the advise on the Wiki also said to ignore depending upon c-variables, so I did switch over to sensors to give me the values that I wanted. I still wanted to know the logic behind it, as it is sort-of a sanity-check for me when I am examining the state of the model.

yuvaltassa commented 1 week ago

Great! Not sure if you noticed, but function names in the API documentation now link directly to their definitions in the source code, so feel free to drill down 🙂

If you understand something that you think is not well explained in the docs, you can send a PR to improve them