4 wheels differential drive robot issues

Hey all,

I am trying to set up a simple robot with 4 wheels, controlled using a differential controller, but it is not behaving as expected. The main issue is that when I try to set a rotation only, which should see the robot rotating about its center, it is instead rotating(ish) in a relatively large circle.
From what I can see, one of the robot wheel is not turning properly.
The wheel rotation stutters, which could indicated that the controller doesn’t have enough force to make it turn. Note that this is only true for one of the four wheels, which all share the same colliders, and joint parameters.

Would someone have suggestions on how to solve this issue?

I tried to play with the physics parameters and colliders parameters without success.

NB: the wheels are using cylinders as colliders.

I tried two different ways to control the wheels:
One based on Nvidia’s controllers:

from omni.isaac.wheeled_robots.robots import WheeledRobot
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.controllers import BaseController
import numpy as np

class CoolController(BaseController):
    def __init__(self,name="my_cool_controller", wheel_radius=0.1, wheel_base=0.1):
        super().__init__(name="my_cool_controller")
        # An open loop controller that uses a unicycle model
        self._wheel_radius = wheel_radius
        self._wheel_base = wheel_base
        return

    def forward(self, command):
        # command will have two elements, first element is the forward velocity
        # second element is the angular velocity (yaw only).
        joint_velocities = [0.0, 0.0, 0.0, 0.0]
        joint_velocities[0] = ((2 * command[0]) - (command[1] * self._wheel_base)) / (2 * self._wheel_radius)
        joint_velocities[1] = ((2 * command[0]) - (command[1] * self._wheel_base)) / (2 * self._wheel_radius)
        joint_velocities[2] = ((2 * command[0]) + (command[1] * self._wheel_base)) / (2 * self._wheel_radius)
        joint_velocities[3] = ((2 * command[0]) + (command[1] * self._wheel_base)) / (2 * self._wheel_radius)
        # A controller has to return an ArticulationAction
        return ArticulationAction(joint_velocities=joint_velocities)


husky = WheeledRobot(prim_path="/husky",
                name="husky",
                wheel_dof_names=["front_left_wheel", 'rear_left_wheel',"front_right_wheel", 'rear_right_wheel'],
                create_robot=False)

husky.initialize()
controller = CoolController(name="simple_control", wheel_radius=0.155, wheel_base=0.275)
position, orientation = husky.get_world_pose()
print(position)

husky.apply_action(controller.forward([0, 0.25]))

And one directly based on dynamic control:

import numpy as np
from omni.isaac.dynamic_control import _dynamic_control

class DifferentialBase:
    def __init__(self, d:float, r:float,
                 DCIFace: None,
                 robot_path:str,
                 left_wheels:list,
                 right_wheels:list):

        self.dist_chassis = d
        self.wheel_radius = r
        self.DCIFace = DCIFace
        self.robot_path = robot_path
        # Articulation
        self.ar = None
        self.right_wheels = None
        self.left_wheels = None
        self.left_wheel_joints = left_wheels
        self.right_wheel_joints = right_wheels
        # Commands
        self.vw_R = 0.0
        self.vw_L = 0.0
        # Odometry
        self.vw_R_z = 0.0
        self.vw_L_z = 0.0

    def twist2wheels(self, x,z):
        self.vw_L = (x - z*self.dist_chassis)/(self.wheel_radius)
        self.vw_R = (x + z*self.dist_chassis)/(self.wheel_radius)

    def applyCommand(self, dt):
        # Only ran at start-up
        if self.ar is None:
            # Grabs the articulation at the robot path
            self.ar = self.DCIFace.get_articulation(self.robot_path) # ar is an ID
            # Gets the root of the articulation/robot
            self.chassis = self.DCIFace.get_articulation_root_body(self.ar)
            # Lists in which we store the ID associated to each wheel
            self.left_wheels = []
            self.right_wheels = []
            # Puts in the lists the ID of the wheels
            for joint in self.left_wheel_joints:
                self.left_wheels.append(self.DCIFace.find_articulation_dof(self.ar, joint))
            for joint in self.right_wheel_joints:
                self.right_wheels.append(self.DCIFace.find_articulation_dof(self.ar, joint))

        # First make sure the articulation is enabled
        self.DCIFace.wake_up_articulation(self.ar)
        # Apply the desired velocity to each wheel
        for wheel in self.left_wheels:
            self.DCIFace.set_dof_velocity_target(wheel, self.vw_L)
        for wheel in self.right_wheels:
            self.DCIFace.set_dof_velocity_target(wheel, self.vw_R)

DCIFace = _dynamic_control.acquire_dynamic_control_interface()
DB = DifferentialBase(0.275, 0.155, DCIFace, "/husky", ["front_left_wheel", "rear_left_wheel"], ["front_right_wheel", "rear_right_wheel"])
DB.twist2wheels(0,0.25)
DB.applyCommand(0.1)

Cheers,

Antoine