[Question] Spawned object moved by invisible force

[phsilvarepo]

Hello there,

I am trying to make a Direct Workflow RL task to move Franka's hand to the position of a cube, right now it seems when the object resets sometimes it moves without apparent collision with the robot. I have ensured the velocity of the object is zero and it mantains the same orientation, the only change is in its x and y position, since it is placed on a surface. I based the reset_idx function on the Manager-Based Workflow for Franka Lift Cube Task.

Any help would be great.

def _reset_idx(self, env_ids: torch.Tensor | None):
        super()._reset_idx(env_ids)

        # 1. Reset robot state: randomize joint positions and set joint velocities to zero
        joint_pos = self._robot.data.default_joint_pos[env_ids] + sample_uniform(
            -0.125, 0.125, (len(env_ids), self._robot.num_joints), self.device
        )
        joint_pos = torch.clamp(joint_pos, self.robot_dof_lower_limits, self.robot_dof_upper_limits)
        joint_vel = torch.zeros_like(joint_pos)
        self._robot.set_joint_position_target(joint_pos, env_ids=env_ids)
        self._robot.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids)

        root_states = self._cuboid.data.default_root_state[env_ids].clone()

        pose_range = {
            "x": (-0.2, 0.2),   # Range for x position
            "y": (-0.6, 0.6), # Range for y position
            "z": (0.0, 0.0),    # Fixed z position (can be adjusted as needed)
            "roll": (0.0, 0.0),  # Roll range
            "pitch": (0.0, 0.0), # Pitch range
            "yaw": (0.0, 0.0)    # Yaw range
        }

        velocity_range = {
            "x": (0.0, 0.0),   # Example range for x velocity
            "y": (0.0, 0.0),   # Example range for y velocity
            "z": (0.0, 0.0),   # Example range for z velocity
            "roll": (0.0, 0.0), # Example roll velocity range
            "pitch": (0.0, 0.0),# Example pitch velocity range
            "yaw": (0.0, 0.0)   # Example yaw velocity range
        }

        # poses
        range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]]
        ranges = torch.tensor(range_list, device=self.device)
        rand_samples = sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=self.device)

        positions = root_states[:, 0:3] + self.scene.env_origins[env_ids] + rand_samples[:, 0:3]
        orientations_delta = quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5])
        orientations = quat_mul(root_states[:, 3:7], orientations_delta)
        # velocities
        range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]]
        ranges = torch.tensor(range_list, device=self.device)
        rand_samples = sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=self.device)

        velocities = root_states[:, 7:13] + rand_samples

        # set into the physics simulation
        self._cuboid.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids)
        self._cuboid.write_root_velocity_to_sim(velocities, env_ids=env_ids)

Complete script: custom_task.txt

[mpgussert]

Heya @phsilvarepo

You caught the positions and velocities, but you missed the forces! Checkout the reset call on RigidObject assets

I think the call you specifically want is set_external_force_and_torque

please let me know if this works :D

An even better option may be to move to a managed environment workflow!

[phsilvarepo]

I fixed my issue by adding some height to the cube's spawn positions, I believe the collision with the ground plane was causing the movement. But I will also look into your input. Thank you so much.