Semi-constrained end-effector goal orientation
I'm using MoveIt! with TRAC-IK to do motion planning for a 6-DOF robotic arm. To set the scene, I am trying to grasp a cylindrical object within the reachable workspace. As far as setting the goal pose goes, I would like to grasp the cylinder at a particular point along its length (aligned with the world z-axis), so the target position is fully specified. However, as long as the gripper is upright (aligned to the length of the cylinder), I don't care about its rotation relative to the world z-axis. In other words, I would like to fully constraint the end-effector's roll and pitch relative to the reference frame, but leave the yaw completely unconstrained and allow the planner to select any valid pose that meets the position, roll and pitch criteria.
I figured this would be possible by relaxing the yaw goal tolerance, however it seems like r, p, y specific tolerances are not supported by setGoalOrientationTolerance.
Is there a way to achieve this? Thanks for the help, as I am fairly new to ROS and MoveIt!
