Fix Position Threshold Check for State Transitions

[DorsaRoh]

Description

Adds a position threshold check, resolving 3 TODO error comments, to ensure the robot's end effector is within a specified distance from the target position before transitioning between states in the pick and lift state machine. Improves the precision of state transitions and helps prevent premature actions during object manipulation. I.e, the threshold ensures the robot is "close enough" to the target position before proceeding, reducing the likelihood of failed grasps or incorrect movements.

Type of change

• Bug fix (non-breaking change which fixes an issue)
• New feature (non-breaking change which adds functionality)

Screenshots

Not applicable for this change

Checklist

• I have run the pre-commit checks with ./isaaclab.sh --format
• I have made corresponding changes to the documentation
• My changes generate no new warnings
• I have added tests that prove my fix is effective or that my feature works
• I have updated the changelog and the corresponding version in the extension's config/extension.toml file
• I have added my name to the CONTRIBUTORS.md or my name already exists there