Wrong (extremely non-optimal) MoveIt trajectories on scene with collision objects
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()
and how have you configured it?
PS: please don't swear. There is no need for it. There are other ways to express your surprise.
Sorry :( I updated answer with more information.
i am facing similar issue. will follow this discussion and share results from testing on my side.