Continuous Joint Planning Failure

[Ponzel]

When calling planToEndEffectorOffset, the constraints are not respected in some cases. Continuous joints turn around 360 degrees mid-trajectory, violating the constraints.

I created one example which fails every time. The code can be found in the repos:

• ada_demos: branch continuous-joint-failure
• libada: commit 6374d618cc
• ada_description: commit 624495d1c4
• aikido: either of the branches master, bugfix/avk/so2, daniel
• pr_ordata: commit 0ca963aacb

The example moves the robot to two configurations by planToConfiguration and after that moves the forque towards the mannequin and back again using planToEndEffectorOffset. You can repeat this motion by pressing enter. planToEndEffectorOffset uses CRRTConnect internally.

It seems to be connected to continuous joints. Behavior for joint different settings:

• continuous joints: 360 degree turn mid-trajectory
• revolute joint, limits (-pi, pi): planning fails (invalid bounds)
• revolute joint, limits (-7, 7): works

In this demo joint 4 fails, but I in other cases I have observed similar behavior on other continuous joints and also on the real robot.

[aditya-vk]

I believe we now handle continuous joints appropriately. AIKIDO: #496 Rewd Controllers: https://github.com/personalrobotics/rewd_controllers/pull/26

@gilwoolee can we close this issue if we no longer see any problems with the latest set of PR merges? Thanks!

[brianhou]

@gilwoolee @aditya-vk @egordon is this still an issue?