Object weird behavior in a object-level teleoperation simulation with robots and spacemouse

[mikelasa]

Hello,

I'm working on a object level teleoperation and I have an strange issue with the box behavior in the simulation. I assume it has something to do with how I teleoperate the object itself but so far I haven't come up with a solution, so I want to share the video and see if anybody can help me to understand whats happening.

I can switch the control of the spacemouse among the right/left robots and the object itself adding a small deltaX to the position of the end effectors and the central frame of the object. The simulation (video atttached) works in this order:

1. I close the grip towards the object
2. I lift the left robot while maintaining the grasp
3. switch control to balance the right robot
4. switch control and teleoperate the object

The object I created using RigidObjectCfg and adding some properties to have control on the friction and mass of the box:

# Set Cube as object
    bin: RigidObjectCfg = RigidObjectCfg(
        prim_path="/World/envs/env_0/Bin",
        init_state=RigidObjectCfg.InitialStateCfg(
            pos=[0.65, 0.5, 0.1],
            rot=

[0, 1, 0, 0]),
        spawn=sim_utils.CuboidCfg(
            size=[0.2, 0.4, 0.2],
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                solver_position_iteration_count=16,
                solver_velocity_iteration_count=1,
                max_angular_velocity=1000.0,
                max_linear_velocity=1000.0,
                max_depenetration_velocity=5.0,
                disable_gravity=False,
            ),
            mass_props=sim_utils.MassPropertiesCfg(
                mass=0.1
            ),
            physics_material=sim_utils.RigidBodyMaterialCfg(
                static_friction=0.5, dynamic_friction=0.5, restitution=0.0
            ),
            visual_material=sim_utils.PreviewSurfaceCfg(
                diffuse_color=(1.0, 0.0, 0.0), metallic=0.2),
            
            activate_contact_sensors=True,
            
            collision_props=sim_utils.CollisionPropertiesCfg(
                collision_enabled=True), 
        ),
    )

The issue I'm facing is that when I have the object grasped with the two end effectors and I control one of the two robots, I can lift the box without any problem and the behavior is smooth as it can be seen in the video. The issue comes when I change the control mode to the object. There are two things that happen here:

1. It seems like a small glitch happens as the box trembles for a millisecond and the end effectors are repeled from the object.
2. When I move the object, seems is like the it slides away from the grip of the robots. The box transisition isn't smooth and the movement render is slow and laggy.

I did the following tests w.r.t frictions, grip forces and teleoperation sensitivities but I didn't see anything:

1. Changed the friction on the object to see the behavior, no noticeable changes.
2. Changed the force that I use to close the grip with 3 different forces, the box behavior doesn't change 2.1- 10N 2.2 - 30N 2.3 - 100N
3. Changed the spacemouse sensitivity, the box moves faster or slower but the behavior is the same.

To control the object first I take the input signal from the spacemouse:

# define the target pose
            robot_teleop_command, gripper_command = teleop_interface.advance()
            robot_teleop_command = robot_teleop_command.astype("float32")
            
            # convert to torch
            robot_teleop_command = torch.tensor(robot_teleop_command, device="cuda").repeat(args_cli.num_envs, 1)

And I add to the actual pose of the object (bin) in the simulation the deltaX:

# set command
            if controlled_robot[0] == "robot1":
                robot1_controller.set_target_command(robot_command, robot1_q_null)
                robot2_controller.set_target_command(robot_null_command, robot2_q_null)
               
            elif controlled_robot[0] == "robot2":
                robot1_controller.set_target_command(robot_null_command, robot1_q_null)
                robot2_controller.set_target_command(robot_command, robot2_q_null)
                
            **elif controlled_robot[0] == "bin":
                # concatenate position and orientation errors
                bin_state[:, 0:3] += robot_teleop_command[:, 0:3]
                bin_state[:, 3:7] = robot_teleop_command[:, 3:7]
                print(bin_state)
                bin.write_root_pose_to_sim(bin_state[:, :7])**
                
                # Calculate desired end-effector poses
                robot1_controller.set_target_command(robot_null_command, robot1_q_null)
                robot2_controller.set_target_command(robot_null_command, robot2_q_null)

https://github.com/user-attachments/assets/27134c23-72b2-400c-b102-395b0e492c77

How am I supposed to move the object in a simulation? Maybe my method is not the optimal way? I would appreciate any insight about this please, I'm kind of desperate with this issue.