Moveit-PathPlanner giving strange trajectories? Here's one good fix!!

edit

Hi Guys,

I'm not here to ask a question but rather to answer an age-old one! I hope this helps someone because I was struggling with this problem for 3 years until I found this fix recently. If you're like me and don't like reading long posts, skip to the solution.

Problem description: I work with 2 manipulators - Sawyer and UR3e. Sawyer has 7 DOF and hence can generate hundreds of IK solutions for any given end-effector pose. UR3e has 6, however, the issue here is that all 6 are 360 deg joints and hence, the same problem. I need the robots to sort objects on a table and since the object locations are dynamic I get them using a 3D camera and CV techniques. Since the locations aren't fixed I need the planner(currently using RRTConnect, but tried almost every planner available) to find a path there and dip down straight, grasp the object and lift up straight! Thanks to Jeroen, we have a way to impose constraints along an axis and here's an example usage. However, the planner comes up with all sorts of weird unintuitive plans to move between simple straightforward locations and this used to mess up my sort by knocking off some objects on the table or failing to pick, etc (I do have accurate collision objects setup and this happens still!).

I've turned over every rock I could to find a fix, read several GitHub issue threads, tutorials, and forum posts but nothing helped. I switched to the enhanced IK solver - TRAC IK which helped to an extent with Sawyer's trajectories but not with UR. Restricting all the joint angles to (-180, +180) or (-270, +270) helped a bit with UR(This can also be done on the teach pendant, or here if you're on Gazebo). So how did I fix it you ask? Here it is:

THE SOLUTION:

• Caveat:

  Since with UR, the robot is small, always above the table, and can't physically reach beyond 0.5m radius, this fix won't be a problem. Sawyer could run into issues because it's a long bulky manipulator, however, as long as you always start from above the table and make sure your coordinates are valid and above the table, you'll be fine.

Step 1: Switch back to the default KDL kinematics solver if you're using something else. This can be done here for UR3e. This helps because TRAC IK doesn't allow approximate IK solutions through code!

Step 2: I believe you'd be using something like this to send your eef coordinates to Moveit planner:

    group = self.group
    # Allow some leeway in position(meters) and orientation (radians)
    group.set_goal_position_tolerance(thresh)
    group.set_goal_orientation_tolerance(thresh)

    pose_goal = geometry_msgs.msg.Pose()
    pose_goal.orientation.x = ox
    pose_goal.orientation.y = oy
    pose_goal.orientation.z = oz
    pose_goal.orientation.w = ow
    pose_goal.position.x = px
    pose_goal.position.y = py
    pose_goal.position.z = pz
    group.set_pose_target(pose_goal)
    group.allow_replanning(allow_replanning ...