This pull request adds code to reconstruct the pose matrix just before solving IK. This fixes a bug that occured when performing remote picks, where the pick pose failed IK.
Pre-fix log:
[ WARN] [1567615434.802556808] [/controller]: Failed to find joint solution for point: dp_offset
[ WARN] [1567615434.802599713] [/controller]: Failed to convert trajectory point: (dp_offset), of type: 2 to joint trajectory
[ERROR] [1567615434.802618994] [/controller]: Conversion to joint trajectory failed for pick due to IK failure of dp_offset
[ERROR] [1567615434.802635465] [/controller]: Failed to convert [pick] to joint trajectory
[ERROR] [1567615434.802739161] [/controller]: IK Failed for trajectory: [pick]
[ERROR] [1567615434.802797739] [/controller]: Failed to execute trajectory because of unexpected exception: Conversion to joint trajectory failed for pick
Post-fix log:
[ INFO] [1567615674.710997250] [/controller]: Found proper IK (no config change) in 1 attempts
[ INFO] [1567615674.711063509] [/controller]: 1 points among 1 successfully interpolated for dp_offset
[ INFO] [1567615674.711082765] [/controller]: Appending trajectory point, size: 3
[ INFO] [1567615674.711096600] [/controller]: Trajectory successfully added till dp_offset
This pull request adds code to reconstruct the pose matrix just before solving IK. This fixes a bug that occured when performing remote picks, where the pick pose failed IK.
Pre-fix log:
Post-fix log: