Small vibrations in gripper fingers

[sento2310]

Intro

Hi!

I am a graduate student currently researching on teleoperation of robotic arms.
My goal is to teleoperate different robots like franka emika panda or the ur5 with a gamepad.
For this I am trying to implement a velocity based control for the robotic arms.
Here I encountered a problem which leads to the robots not being able to pick up the object (here it is a cube).

My setup

I am using Mujoco 3.2.7 on windows, and use Python

My question

The Problem is explained after my code snippets.

I use the robot files from the Menagerie and implemented a method to convert gamepad inputs to joint velocities for the joints of the arm.

For the inverse kinematics I use following method:

 def _mujoco_jacobian(self, q, sim_model, sim_data):

        # Set the joint positions in the simulation data
        sim_data.qpos[:self.n_arm_joints] = q
        mujoco.mj_forward(sim_model, sim_data)  # Update kinematics

        # Use the configured end-effector body name
        ee_body_id = mujoco.mj_name2id(sim_model, mujoco.mjtObj.mjOBJ_BODY, self.end_effector_body)

        # Compute Jacobian matrices for position and orientation
        jac_pos = np.zeros((3, sim_model.nv))
        jac_rot = np.zeros((3, sim_model.nv))
        mujoco.mj_jacBody(sim_model, sim_data, jac_pos, jac_rot, ee_body_id)

        # Extract only the columns corresponding to arm joints
        J_pos = jac_pos[:, :self.n_arm_joints]
        J_rot = jac_rot[:, :self.n_arm_joints]

        # Combine into full Jacobian
        J = np.vstack([J_pos, J_rot])
        return J

And compute joint velocities with:

 def compute_joint_velocities(self, q, twist, is_rotating=False, sim_data=None):
    # Get the Jacobian (arm joints only)
    J = self._mujoco_jacobian(q, self.sim_model, sim_data)

    # Damped least-squares pseudoinverse
    J_pinv = pinv(J.T @ J + self.lambda_damping ** 2 * np.eye(J.shape[1])) @ J.T

    # Compute joint velocities
    q_dot = J_pinv @ twist

    # Apply joint velocity limits
    q_dot = np.clip(q_dot, -self.max_joint_vel, self.max_joint_vel)

    return q_dot

Then after transforming inputs from the joysticks of my gamepad. I set join velocities with:

       q_dot_arm = ik_solver.compute_joint_velocities(q_arm, twist, is_rotating, sim.data)

       # Combine arm velocities with zero velocities for gripper joints
        q_dot_full = np.zeros(n_total_joints)
        q_dot_full[:n_arm_joints] = q_dot_arm

        # Set joint velocities
        sim.data.qvel[:n_total_joints] = q_dot_full
        sim.step()

The gripper is itself is closed and opened by setting or resetting a flag with each button press of the A button:
The code for the opening and closing itself is:

    def move_gripper(self, gripper_pos, speed):

        # Set maximum speed for scaling
        max_speed = 255.0  # Units per second

        # Scale input position and speed to internal range [0, 255]
        self.gripper_target = np.clip(gripper_pos * 255.0, 0, 255)
        self.gripper_speed = np.clip(speed * max_speed, 0, max_speed)

    def update_gripper(self):

        if self.gripper_control_index >= len(self.sim.data.ctrl):
            print(f"Warning: Gripper control index {self.gripper_control_index} out of bounds")
            return  # No gripper control available

        current_pos = self.sim.data.ctrl[self.gripper_control_index]
        delta = self.gripper_target - current_pos

        # Exit if no movement is required
        if delta == 0:
            return

        # Move the gripper towards the target
        step_size = self.gripper_speed * self.sim.model.opt.timestep
        if abs(delta) <= step_size:
            self.sim.data.ctrl[self.gripper_control_index] = self.gripper_target
        else:
            self.sim.data.ctrl[self.gripper_control_index] += np.sign(delta) * step_size

The Problem:

Now, I can control the franka emika panda and the ur5 and it feels very smooth.
But I can´t pick up the cubes with either robot, the cube seems to slip out.
For both grippers it is a bit different:

When doing it with the Franka, it looks like the fingers are vibrating a bit. My first thought was, that it is getting pushed open by the cube, leading to not enough pressure.

But then I tried it with the ur5, and there it is different.
If I move the robot while I grip the cube, the cube slips out. But as soon as I stop the robot from moving, the cube doesn´t slip.

I also noticed, that the joint values at the top right change slightly while moving my robotic arm.
This happens, even though only the velocities for the arm joints are set when using the joytstick, not for the gripper.
This also happens without a cube in between.

I also recorded 2 videos of this and uploaded them to youtube:
Panda
ur5

So I wonder, does anyone have an idea what might lead to this shaky bheaviour of the fingers, or how this could be fixed?

I already changed friction values, solref, solimp and so on, pretty much anything that is suggested on the following website:
mujoco preventing-slip

Also I added high damping and stiffness values to the fingers, but it didn´t help, just made the fingers move slower when opening and closing.

Does anyone know what might be the problem in this case? I would really appreciate some help.

Minimal model and/or code that explain my question

No response

Confirmations

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