Object weird behavior in a object-level teleoperation simulation with robots and spacemouse #1141
Description
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:
- I close the grip towards the object
- I lift the left robot while maintaining the grasp
- switch control to balance the right robot
- 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:
- It seems like a small glitch happens as the box trembles for a millisecond and the end effectors are repeled from the object.
- 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:
- Changed the friction on the object to see the behavior, no noticeable changes.
- 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 - 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)
2024-10-04.15-25-38.mp4
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.