Fix position limits oink constraint for models with continuous joints

[sea-bass]

Oink was going unstable with the Kinova model because using the Pinocchio model's joint limits fields actually considers continuous joints as [cos(theta), sin(theta)], meaning there are 2 elements both with limits +/- 1.0.

This properly uses the RoboPlan Scene's joint limits info, as well as the collapseContinuousJointPositions() utility, to handle continuous joints. Kinova example is now numerically stable by using correct position limits.

Screencast.from.2026-02-21.15-25-17.webm

[sea-bass]

This highlights a need to possibly have tasks accept the scene in their constructor so that information can be pre-allocated at construction time.

The same thing happens with FrameTask re: getting the frame_id value for the joint.

Although, I do like the solver doing its own thing so that the scene usage is consistent. Maybe then the best approach is to have a setup() method in these tasks/constraints that is automatically called by the solver? Or at least we can just call solver.setup() and that invokes it for all the tasks/constraints/barriers?

[sjahr]

Good point! I opened an issue to have to track this as additional work. #152

[sea-bass]

I already captured that in #148 I think!

[sjahr]

Tested it and it works! One bigger comment & independent of that, do you mind adding 1-2 tests for the new functionality?

[sjahr]

Good point! I opened an issue to have to track this as additional work. #152

[sjahr]

@sea-bass Looks like the last commit broke something

[OsqpEigen::Settings::setTimeLimit] OSPQ has been set without PROFILING, hence this setting is disabled.
ERROR in validate_data: Lower bound at index 21 is greater than upper bound: 5.0000e-03 > -5.0000e-03
ERROR in osqp_setup: Problem data validation.
[OsqpEigen::Solver::initSolver] Unable to setup the workspace.
Warning: IK solver failed: IK solve failed: Failed to initialize solver, using zero delta_q

I just quickly tested it and got this error. Does it happen to you as well? The commit before seems to work. Do you mind taking a look into this? Not sure why the test isn't catching it.

[sea-bass]

Ah I think I know what this is. The max velocity/accel/jerk has to be multiplied by the absolute value of the scaling, so in the negative case it gets messed up.

Will fix and test when home.

[sea-bass]

Ah I think I know what this is. The max velocity/accel/jerk has to be multiplied by the absolute value of the scaling, so in the negative case it gets messed up.

Will fix and test when home.

This was indeed it. Fixed, thank you!

[sjahr]

Jap, that fixed it! Nice work 🎉