Right now, the orientation constraint message from moveit has 3 theta bounds for each axes in the constraint frame. The current orientation constraint implementation just applies one of the three. See comment below.
A better approach would be to add an additional rotational transformation to R_BbarB, computing R_BbarBx, R_BbarBy and R_BbarBz - add 3 different orientation constraints for each axis with the theta_bounds being the individual tolerances.
Right now, the orientation constraint message from moveit has 3 theta bounds for each axes in the constraint frame. The current orientation constraint implementation just applies one of the three. See comment below.
A better approach would be to add an additional rotational transformation to
R_BbarB, computingR_BbarBx,R_BbarByandR_BbarBz- add 3 different orientation constraints for each axis with the theta_bounds being the individual tolerances._Originally posted by @kamiradi in https://github.com/moveit/moveit_drake/pull/55#discussion_r1801321218_