Our cartesian controller currently uses the damped least squares solver. It works well, but has unexpected and scary motion (that often speeds up) near singularities. The motion near singularities also often deviates along unintuitive dimension of the end effector pose (as in, visually I feel the goal could have been better-achieved by deviating along a different dimension). Further, tuning alpha is unintuitive imho.
Instead, this PR shifts our cartesian controller to selectively damped least squares, which adaptively chooses a damping factor, and does so separately for each dimension that has reached singularity. As a result, it promises to have better performance at singularities.
Note that both solvers are implemented in here (the header files for each implementation link to the above papers).
Testing
[x] Test cartesian teleop on the web app. Not only did this work very well, I was able to do large motions, like from staging to above plate, which I previously hadn't been able to do with any of the teleop approaches we used. I felt quite safe around it.
[x] Test acquisition: Acquire 3 bites of food in varied locations relative to the robot, verify it succeeds.
[x] Test motion to/from mouth. Transfer the bite with the face up against the headrest. Then separately transfer it with the mouth up, down, left, right, and forward relative to that position. (6 transfers total). Verify it succeeds and feels safe.
Description
Our cartesian controller currently uses the damped least squares solver. It works well, but has unexpected and scary motion (that often speeds up) near singularities. The motion near singularities also often deviates along unintuitive dimension of the end effector pose (as in, visually I feel the goal could have been better-achieved by deviating along a different dimension). Further, tuning
alphais unintuitive imho.Instead, this PR shifts our cartesian controller to selectively damped least squares, which adaptively chooses a damping factor, and does so separately for each dimension that has reached singularity. As a result, it promises to have better performance at singularities.
Note that both solvers are implemented in here (the header files for each implementation link to the above papers).
Testing