Unexpected simulation behavior in variable length cartpole #2374

mihirus · 2025-01-20T04:03:17Z

mihirus
Jan 20, 2025

Intro

Hi!

I am a graduate student/robotics hobbyist working on some basic trajectory optimization problems.

My setup

Pymujoco version 3.1.1

Ubuntu 22.04.5 LTS

My question

TLDR

I am simulating a mass that is connected to a radial sliding joint -> hinge joint -> horizontal sliding joint chain, all of which are frictionless and unactuated. This mass is unconstrained and should fall straight down, but it falls with a horizontal component.

Original question

I am attempting to verify that my analytical model for a cartpole with variable length pole matches simulation. The cart and pole length are actuated, with pole angle free. Both the cart and bob (CoM of pole) are simulated as point masses with low moment of inertia.

The problem I'm running into is that under certain conditions the simulation does not appear to behave in a physical way. If the pole length is left to be free/unactuated and the pole length is within joint limits, the bob should drop vertically. Instead, the bob falls at a small angle from the vertical. I've attached plots to depict this behavior. The data labeled 'Euler' corresponds to my analytical model.

The variables are as follows:
x_c: x position of cart
x_p: x position/horizontal displacement of bob w.r.t. cart
r_p: distance between bob and hinge joint
v*: velocity of quantity

What I'm seeing is that under this free fall condition, vx_p should stay 0 but it does not, resulting in some change in x_p during the simulation. I've double checked that I'm setting x_p = r_p * sin (theta). Intuitively this seems to be consistent with the hinge joint being damped - gravity pushes the bob along the axis of the pole, changing the horizontal displacement. I am using the default damping settings for all my joints in the XML so I'm not sure why this would be the case (or how to change it).

I have also simulated a similar system but with the hinge joint fixed instead of moving on a cart. For this system, the bob correctly falls vertically and my model and the simulation track each other closely (until the slide joint limit is reached). The horizontal displacement changes from 0.399 to 0.396 over 100 timesteps.

Would really appreciate some help with this. Thank you!

Minimal model and/or code that explain my question

Cartpole model:

minimal XML

<mujoco model="cartpole">
    <visual>
        <map force="0.1" zfar="30"/>
        <global fovy="80" offwidth="2560" offheight="1440" elevation="-20" azimuth="0"/>
        <headlight ambient="0.15 0.15 0.15" specular="0 0 0" diffuse="0.6 0.6 0.6" />
    </visual>
    <asset>
        <texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" width="32" height="512"/>
        <texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
        <material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
        <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".4 .5 .6"/>
        <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
    </asset>

    <worldbody>
        <geom name="floor" pos="0 0 -0.025" size="0 0 .05" type="plane" material="grid" condim="4" friction="0.01 0.01 0.0001"/>
        <body name="cart" pos="0 0 0.1">
            <joint name="q1" type="slide" axis="1 0 0" />
            <inertial pos="0 0 0" mass="0.25" diaginertia="0.00001 0.00001 0.00001" />
            <geom type="box" size="0.1 0.1 0.01" rgba="0 1 0 1" />
            <body name="pole" pos="0 0 0.2" >
                <joint type="hinge" pos="0 0 -0.2" axis="0 1 0" name="q2" solimplimit="0.9 0.95 0.001 0.1 10" solreflimit="0.02 1"/>
                <joint type="slide" pos="0 0 0" axis="0 0 1" name="q3" range="0.2 1" />
                <inertial pos="0 0 0" mass="1" diaginertia="0.00001 0.00001 0.00001" />
                <geom type="cylinder" pos="0 0 -0.2" size="0.01 0.2" rgba="1 0 0 1" />
                <geom type="sphere" size="0.02" rgba="0 0 1 1" />
            </body>
        </body>
    </worldbody>

    <actuator>
        <motor name="u1" gear="1 0 0 0 0 0" joint="q1"/>
        <motor name="u2" gear="1 0 0 0 0 0" joint="q3"/>
    </actuator>
</mujoco>

Slide pendulum model:

minimal XML

<mujoco model="slide_pendulum">
        <visual>
        <map force="0.1" zfar="30"/>
        <global fovy="80" offwidth="2560" offheight="1440" elevation="-20" azimuth="0"/>
        <headlight ambient="0.15 0.15 0.15" specular="0 0 0" diffuse="0.6 0.6 0.6" />
    </visual>

    <asset>
        <texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" width="32" height="512"/>
        <texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
        <material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
        <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".4 .5 .6"/>
        <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
    </asset>

    <worldbody>
        <geom name="floor" pos="0 0 -0.025" size="0 0 .05" type="plane" material="grid" friction="1 0.1 0.0001"/>
        <geom type="sphere" pos="0 0 0.5" size="0.02" rgba="0 1 0 1" contype="0" conaffinity="0"  />
        <body name="pole" pos="0 0 1" >
            <joint type="hinge" pos="0 0 -0.5" axis="0 1 0" name="q1"/>
            <joint type="slide" pos="0 0 -0.5" axis="0 0 1" name="q2" range="-0.4 0" />
            <inertial pos="0 0 0" mass="1" diaginertia="0.00001 0.00001 0.00001" />
            <geom type="cylinder" pos="0 0 -0.25" size="0.01 0.25" rgba="1 0 0 1" contype="0" conaffinity="0"  />
            <geom type="sphere" size="0.02" rgba="0 0 1 1" contype="0" conaffinity="0"  />
        </body>
    </worldbody>

    <actuator>
        <motor name="u1" gear="1 0 0 0 0 0" joint="q2"/>
    </actuator>
</mujoco>

Code snippet for running cartpole:

Python

# Set initial conditions
d.qpos[1] = 0.05
d.qpos[2] = 0.5
d.ctrl[1] = 0

x_curr = mujoco_to_casadi(d.qpos, d.qvel)

with mujoco.viewer.launch_passive(m, d) as viewer:
  start = time.time()
  for i in timesteps:
    step_start = time.time()
    
    # euler integration
    f_curr = f(x_curr, d.ctrl) # f is a casadi function that gives the dynamics
    x_curr += 0.002 * f_curr
    euler_traj.append(x_curr.full()[:,0])
    
    # Take the step
    mujoco.mj_step(m, d)

    # Collect mujoco traj
    current_state = mujoco_to_casadi(d.qpos, d.qvel)
    mujoco_traj.append(current_state)

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

mihirus · 2025-01-26T01:20:28Z

mihirus
Jan 26, 2025
Author

I got it working by separating the hinge and slide joints between an intermediate body with low mass/inertia.

However, I would like to point out that the documentation says that an arbitrary set of joints can be used to create degrees of freedom between two bodies: https://mujoco.readthedocs.io/en/stable/modeling.html#kinematic-tree

So it seems there was a problem with the simulator, unless someone can point me towards what I was doing wrong.

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Unexpected simulation behavior in variable length cartpole #2374

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Replies: 1 comment

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Select a reply

Unexpected simulation behavior in variable length cartpole #2374

mihirus Jan 20, 2025

Intro

My setup

My question

TLDR

Original question

Minimal model and/or code that explain my question

Confirmations

Replies: 1 comment

mihirus Jan 26, 2025 Author

mihirus
Jan 20, 2025

mihirus
Jan 26, 2025
Author