Determine method to detect when cartesian waypoint should be identified as fixed state in trajopt

[Levi-Armstrong]

See the following PR for more details

[rjoomen]

Claude says:

The algorithm is:
1. Solve IK to get a joint configuration q that satisfies the Cartesian constraints
2. Compute the geometric Jacobian J(q) at that configuration.
3. Extract only the rows corresponding to your constrained Cartesian components, this is the task Jacobian J_task.
4. Compute the rank of J_task (typically via SVD)
5. Null space dimension = n_joints-rank(J _task)
6. If > 0, redundant; if = 0, not redundant.

But I don't know whether this is correct or not.

[Levi-Armstrong]

This should be right.

[rjoomen]

With one more remark:

"Note on discrete solutions

This only tells you about continuous redundancy (null space). Even with null space dimension 0, you might still have multiple discrete IK solutions (elbow up/down, etc.)—but those are handled differently in path planning, typically by choosing one configuration and maintaining consistency.​​​​​​​​​​​​​​​​"

[Levi-Armstrong]

With one more remark:

"Note on discrete solutions

This only tells you about continuous redundancy (null space). Even with null space dimension 0, you might still have multiple discrete IK solutions (elbow up/down, etc.)—but those are handled differently in path planning, typically by choosing one configuration and maintaining consistency.​​​​​​​​​​​​​​​​"

Yea in the case of trajopt the solution it optimizes towards will be dictated by the seed trajectory.