orbit icon indicating copy to clipboard operation
orbit copied to clipboard

[Question] Contact Sensor on UR10 ee_link does not work

Open SonYeongGwang opened this issue 5 months ago • 1 comments

Hi, I'm currently work with the contact sensor.

The example (source/standalone/tutorials/04_sensors/add_sensors_on_robot.py) works well in my environment.

To test with my configuration, I've attached a contact sensor on my UR10 ee_link.

Then I planned the robot to gently push an object on an table.

Q1_1 Q1_2

However, the sensor always returns (net_forces_w=tensor([[[0., 0., 0.]]])

Some essential code for my environment is as below:

@configclass
class URSceneCfg(InteractiveSceneCfg):

    target_obj = AssetBaseCfg(
                    prim_path="{ENV_REGEX_NS}/object",
                    spawn=sim_utils.UsdFileCfg(usd_path="omniverse://localhost/NVIDIA/Assets/Isaac/4.0/Isaac/Props/YCB/Axis_Aligned_Physics/003_cracker_box.usd"),
                    init_state=AssetBaseCfg.InitialStateCfg(pos=(0.8, 0.02, 0.1), rot=(1, 0.0, 0.0, 0.0)))
    
    # ur10 robot
    robot: ArticulationCfg = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")

    contact_forces = ContactSensorCfg(
        prim_path="{ENV_REGEX_NS}/Robot/ee_link", 
        update_period=0.0, 
        history_length=6, 
        debug_vis=True,
        track_pose=True,
    )
...

   ## simulation loop
    while simulation_app.is_running():

        if ALL_ENV_FIN == True or count == 0:
            ## reset the simulation
            print("All environments finished!")
            print("Resetting the simulation...")

            if ALL_ENV_FIN:
                count = 0
                ALL_ENV_FIN = False

            root_state = scene["robot"].data.default_root_state.clone()
            root_state[:, :3] += scene.env_origins
            scene["robot"].write_root_state_to_sim(root_state)

            ## reset joint state
            joint_pos = ur10.data.default_joint_pos.clone()
            joint_vel = ur10.data.default_joint_vel.clone()
            ur10.write_joint_state_to_sim(joint_pos, joint_vel)
            ur10.reset()

            ## reset actions
            ik_commands[:] = ee_goals[current_goal_idx]
            joint_pos_des = joint_pos[:, robot_entity_cfg.joint_ids].clone()

            ## reset controller
            diff_ik_controller.reset()
            diff_ik_controller.set_command(ik_commands)

        if ALL_ENV_ARRV == True:
            
            ## robot has arrived to the home position and inference is ready!
            if current_goal_idx == 0:
                print("All environments arrived!")

            current_goal_idx = (current_goal_idx + 1) % len(ee_goals)

            # reset actions
            ik_commands[:] = ee_goals[current_goal_idx]

            # reset controller
            diff_ik_controller.reset()
            diff_ik_controller.set_command(ik_commands)

            ALL_ENV_ARRV = False

        else:
            # obtain quantities from simulation
            jacobian = ur10.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
            ee_pose_w = ur10.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
            root_pose_w = ur10.data.root_state_w[:, 0:7]
            joint_pos = ur10.data.joint_pos[:, robot_entity_cfg.joint_ids]
            
            ## compute frame in root frame
            ## ee_pose_w[:, 0:3] -> position of robots based on world frame (considering the spacing)
            ## ee_pos_b = position of each robot based on each robot's frame
            ee_pos_b, ee_quat_b = subtract_frame_transforms(
                root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
            )
            ## compute the joint commands
            joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)

            ## Developing
            if torch.sum(torch.norm(ee_pos_b - ik_commands[0][:3], dim=1) < 0.03) >= args_cli.num_envs:
                arrv_count += 1
                print("Goal reached!")

                if arrv_count == 20:
                    ALL_ENV_ARRV = True
                    arrv_count = 0

        ## Apply action
        ## -- apply action to the robot
        ur10.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)

        ## -- write data to sim
        scene.write_data_to_sim()
        ## Perform step
        sim.step()
        ## Increment counter
        count += 1
        ## update buffers
        scene.update(sim_dt)

        ## obtain quantities from simulation
        ## update marker positions
        # ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])

        print("-------------------------------")
        print(ur10.data.body_pos_w)
        print(ur10.body_names)
        print(scene['contact_forces'].data.pos_w)
        print()
        print("contact force: ", scene["contact_forces"].data)
        print("contact sensor initialized: ", scene["contact_forces"].is_initialized)

Thank you!

SonYeongGwang avatar Aug 27 '24 09:08 SonYeongGwang