Issue with using ctrl for a differential drive robot rotating in place - uneven rotational velocity #2388

husnoo · 2025-01-27T12:08:36Z

husnoo
Jan 27, 2025

Intro

Hi!

I am a hobbyist in robotics/AI simulations.

My setup

Python 3.8.18

>>> mujoco.__version__
'3.1.2'

My question

I have a differential drive robot, with 4 wheels. I can get it to rotate either by setting ctrl or qvel. But aside from the obvious units differences, there seems to be a weird slowing down and speeding up in the rotation speed. Is there a fundamental reason for this? Am I doing something wrong?

This causes the angular rotation speed to vary: data.ctrl[:] = -10,10,-10,10, this keeps it constant: data.qvel[6:] = -10,10,-10,10. It then also happens at higher velocity with qvel: data.qvel[6:] = -100,100,-100,100

I've included a screenshot. The bottom left plot in each case is the interesting part - the plot_ctrl.png shows the issue, while plot_qvel.png is fine. But plot_qvel_high_vel.png shows the issue again.

Minimal model and/or code that explain my question

import pylab
import mujoco as mj
import numpy
import os
import sys

from scipy.spatial.transform import Rotation as R
sys.path.append('/home/nawal/data/calcifer/src')

DO_VIS = False
if DO_VIS:
    import mujoco_viz


def quat2euler(quat_mujoco):
    #mujocoy quat is constant,x,y,z,
    #scipy quaut is x,y,z,constant
    quat_scipy = numpy.array([quat_mujoco[3],quat_mujoco[0],quat_mujoco[1],quat_mujoco[2]])
    r = R.from_quat(quat_scipy)
    euler = r.as_euler('xyz', degrees=True)
    return euler


trajectories = {}



xml = """
<mujoco>
  <option gravity="0 0 -9.81" integrator="Euler" timestep="0.002" />
  
    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>

        <geom type="plane" pos="0 0 -0.132" size="50 50 0.01" rgba=".9 .9 .9 1"/>

        <body name = "chassis" pos="0 0 0.0" euler='0 0 0'>
            <joint type="free"/>
            <geom type="box" size="0.63 0.2 0.1" rgba=".9 .9 0 1" friction="1 0.005 0.0001"  />


            <body name="front-left-tire" pos="0.5 0.35 0" euler='90 0 0'>
                <joint name = "front-left-wheel" type="hinge" axis="0 0 -1"/>
                <geom type="ellipsoid" size="0.15 0.15 0.1"  friction="1 0.005 0.0001" rgba="0 .9 0 1"/>
            </body>

            <body name="front-right-tire" pos="0.5 -0.35 0" euler='90 0 0'>
                <joint name = "front-right-wheel" type="hinge" axis="0 0 -1"/>
                <geom type="ellipsoid" size=".15 0.15 0.1"  friction="1 0.005 0.0001" rgba="0 .9 0 1"/>
            </body>


            <body name="back-left-tire" pos="-0.5 0.35 0" euler='90 0 0'>
                <joint name = "back-left-wheel" type="hinge" axis="0 0 -1"/>
                <geom type="ellipsoid" size=".15 0.15 0.1"  friction="1 0.005 0.0001" rgba="0 .9 0 1"/>
            </body>

            <body name="back-right-tire" pos="-0.5 -0.35 0" euler='90 0 0'>
                <joint name = "back-right-wheel" type="hinge" axis="0 0 -1"/>
                <geom type="ellipsoid" size=".15 0.15 0.1"  friction="1 0.005 0.0001" rgba="0 .9 0 1"/>
            </body>


            <site name="IMU" pos = "0 0 0" size="0.1" />

        </body> <!-- chassis -->

    </worldbody>

    <sensor>
        <accelerometer name="accelerometer" site="IMU"/>
    </sensor>

    <actuator>
          <velocity name="front-left-velocity-servo" joint="front-left-wheel" kv="5" ctrllimited="true" ctrlrange="-10 10"/>
          <velocity name="front-right-velocity-servo" joint="front-right-wheel" kv="5" ctrllimited="true" ctrlrange="-10 10"/>
          <velocity name="back-left-velocity-servo" joint="back-left-wheel" kv="5" ctrllimited="true" ctrlrange="-10 10"/>
          <velocity name="back-right-velocity-servo" joint="back-right-wheel" kv="5" ctrllimited="true" ctrlrange="-10 10"/>
    </actuator>
    
</mujoco>
"""


def mujoco_robot():
    model = mj.MjModel.from_xml_string(xml)
    data = mj.MjData(model)
    if DO_VIS:
        visualiser = mujoco_viz.Visualiser(model, data)

    N = 1200
    trajectories['mujoco'] = {}
    trajectories['mujoco']['time'] = numpy.zeros(N)
    trajectories['mujoco']['position'] = numpy.zeros((N,3))
    trajectories['mujoco']['velocity'] = numpy.zeros((N,3))
    trajectories['mujoco']['acceleration'] = numpy.zeros((N,3))
    trajectories['mujoco']['rotation_speed'] = numpy.zeros((N,3))
    trajectories['mujoco']['rotation_angle'] = numpy.zeros((N,3))
    for i in range(N):
        time_prev = data.time

        #data.ctrl[:] = -10,10,-10,10
        data.qvel[6:] = -10,10,-10,10
        
        
        while (data.time - time_prev < 1.0/60.0):
            mj.mj_step(model, data)

        trajectories['mujoco']['time'][i] = data.time
        trajectories['mujoco']['position'][i,:] = (data.qpos[0], data.qpos[1], data.qpos[2])
        trajectories['mujoco']['velocity'][i,:] = data.qvel[0:3]
        trajectories['mujoco']['acceleration'][i,:] = data.sensor('accelerometer').data
        trajectories['mujoco']['rotation_speed'][i,:] = data.qvel[3:6]

        quat = numpy.array([data.qpos[3],data.qpos[4],data.qpos[5],data.qpos[6]])
        euler = quat2euler(quat)
        trajectories['mujoco']['rotation_angle'][i,:] = euler / 180.0 * numpy.pi

        if DO_VIS:
            visualiser.update_scene()

def main():
    mujoco_robot()
    pylab.ion()
    pylab.figure()
    
    pylab.subplot(2,3,1)
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['position'][:,0], color='red', ls='--', label='mx')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['position'][:,1], color='blue', ls='--',label='my')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['position'][:,2], color='black', ls='--',label='mz')
    pylab.title('distance')
    pylab.legend()
    
    pylab.subplot(2,3,2)
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['velocity'][:,0], color='red', ls='--', label='mx')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['velocity'][:,1], color='blue', ls='--',label='my')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['velocity'][:,2], color='black', ls='--',label='mz')
    pylab.title('velocity')
    pylab.legend()
    
    pylab.subplot(2,3,3)
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['acceleration'][:,0], color='red', ls='--', label='mx')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['acceleration'][:,1], color='blue', ls='--',label='my')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['acceleration'][:,2], color='black', ls='--',label='mz')
    pylab.title('acceleration')
    pylab.legend()
    
    pylab.subplot(2,3,4)
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['rotation_speed'][:,0], color='red', ls='--', label='mx')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['rotation_speed'][:,1], color='blue', ls='--',label='my')
    pylab.plot(trajectories['mujoco']['time'],trajectories['mujoco']['rotation_speed'][:,2], color='black', ls='--',label='mz')
    pylab.title('rotation_speed')
    pylab.legend()

    pylab.subplot(2,3,5)
    pylab.plot(trajectories['mujoco']['time'],2*numpy.pi - trajectories['mujoco']['rotation_angle'][:,0], color='red', ls='--', label='mx')
    pylab.plot(trajectories['mujoco']['time'],2*numpy.pi - trajectories['mujoco']['rotation_angle'][:,1], color='blue', ls='--',label='my')
    pylab.plot(trajectories['mujoco']['time'],2*numpy.pi - trajectories['mujoco']['rotation_angle'][:,2], color='black', ls='--',label='mz')
    pylab.title('rotation angle')
    pylab.legend()

    
    input()
    
    


    


if __name__=='__main__':
    main()

Confirmations

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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Issue with using ctrl for a differential drive robot rotating in place - uneven rotational velocity #2388

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Replies: 0 comments

Select a reply

Issue with using ctrl for a differential drive robot rotating in place - uneven rotational velocity #2388

husnoo Jan 27, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 0 comments

husnoo
Jan 27, 2025