Skip to content

Preserve signed target yaw in QuinticPathPlanner - #785

Open
sylvesterkaczmarek wants to merge 3 commits into
isaac-sim:developfrom
sylvesterkaczmarek:fix/quintic-target-prim-yaw
Open

Preserve signed target yaw in QuinticPathPlanner#785
sylvesterkaczmarek wants to merge 3 commits into
isaac-sim:developfrom
sylvesterkaczmarek:fix/quintic-target-prim-yaw

Conversation

@sylvesterkaczmarek

Copy link
Copy Markdown

Description

Fix target-prim yaw extraction and yaw-change comparison in QuinticPathPlanner.

When inputs:targetPrim is used, the node derives yaw from m.ExtractRotation().angle. That value is the rotation magnitude, not a signed Z-axis yaw, so a negative yaw can be flipped positive and rotation about another axis can be interpreted as yaw.

Extract the target transform quaternion instead and pass it through the node's existing quaternion-to-Euler conversion so the signed yaw is preserved.

The target-change threshold now also compares the wrapped yaw delta. This prevents physically close orientations on opposite sides of +pi/-pi from triggering a spurious replan.

Validation

  • regression test verifies a target prim at -90° Z yields target yaw -pi/2
  • regression test verifies +179° -> -179° is treated as a ~2° change, not ~358°
  • test target is one metre from the robot to avoid a degenerate zero-length path
  • production diff is four additions and two replacements

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant