Question

Hello, We are developing 6 axis manipulator with software mainly based on ROS and MoveIt.

Everything was working fine regarding planning (trajectories were optimal and deterministic, 100% of cases moveit spat out the same trajectory from A to B).

After addition of collision objects to the scene (some cubes to avoid, table, wall, etc.) the planner started to return very strange trajectories. Sometimes they are ok, almost never exactly the same, but its acceptable. But sometimes, they are totally wrong - start and finish positions match, but trajectory is moving the robot through extremely non-optimal paths. Im planning movement:

python  self.group.set_pose_target(pose_goal)
self.group.go()

where the pose_goal is position very near the current robot state (5 cm up on Z axis, no rotation). The trajectory is moving robot 360 degrees around, up, down, righ, left - everywhere. In the end it arrives to the goal position but ... It should just move robot end effector up. The path is clear, robot is nowhere near collision zone (nearest obstacle is XX cm away), and end pose is fully in the robots reach (no singularities).

Has anyone faced similiar problem and came up with some solutions? What is causing described behaviour? How can I make those trajectories less random, and more optimal?

We are using RRTConnect planner, TracIK for kinematic calculations and pretty standard MoveIt pipeline. Our robot is very standard 6-axis manipualator, kinematics is almost identical to UR robot.

-- EDIT --

kinematics.yaml:

arm:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_attempts: 3
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.025
  solve_type: Distance

ompl_planning.yaml config of the RRTConnect:

range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

Asked by mkt on 2020-01-24 09:29:46 UTC

Answer 1

First of all RRT is well-known to produce non-optimal paths and especially to randomly produce off-the-wall trajectories in some cases (the second "R" stands for "random" after all). Path simplification often ends up with a very nice trajectory, but it also does not avoid the problem 100% of the time.

Secondly, and I believe this is more likely the issue you observe, you rely on pose targets which might yield multiple IK solutions. If you rely on pose targets and not on specific joint goals, the random exploration is free to prefer other IK solutions. So while you might think you are asking "to move up a little", you are indeed asking to "move to this arbitrary location". If the first IK solution that can be connected is not the closest one (RRT is rapidly-exploring so this is not unlikely to happen), even the shortest trajectory to go there might look awkward.

Gijs already asked about the information on the configuration of your IK solver and solve_type: Distance is the correct choice to reduce the problem. Nonetheless, I would expect you still have problems with this.

kinematics is almost identical to UR robot

It might help to reduce the joint ranges of your robot to match specific use case. If there is only a single IK solution, planning can't succeed to move to an unintuitive solution instead.

Alternatively you might want to prefer Jacobian approaches as implemented in moveit_servo.

EDIT:

Another alternative to improve paths that are simply not smooth enough is to use the relatively new chomp optimizer adapter that can optimize a trajectory after the planner finished. This will not magically "unknot" convoluted trajectories, but it can improve RRT-generated trajectories.

Asked by v4hn on 2020-10-02 10:02:56 UTC

Comments

Could you clarify if path simplification is on by default, and if there are any parameters to tune it?

Asked by fvd on 2020-10-02 15:16:05 UTC

simplification can be disabled as a dynamic reconfigure parameter, but it's active by default and disabling it only makes sense for educational reasons (to see the effect on the sampled trajectories).

Asked by v4hn on 2020-10-04 06:37:50 UTC

Answer 2

To add to @v4hn's answer, a common workaround to avoid this issue is to use joint goals or named poses (you can set them up in the setup assistant (roslaunch your_moveit_config setup_assistant.launch)) when you want to move to a distant goal. That ensures that your robot is in a known configuration, and for nearby pose goals the IK will generally behave well. But you should still make sure the configuration does not flip, e.g. by limiting the joint ranges of your robot as suggested in the other answer.

Asked by fvd on 2020-10-02 15:23:16 UTC

Wrong (extremely non-optimal) MoveIt trajectories on scene with collision objects

Comments

Answers

Comments

Comments