MoveIt! correctly planning but sometimes failing to execute trajectories
Hello,
I am on ROS Kinetic and Ubuntu 16.04 LTS. I have been using MoveIt! for 3 months now and recently discovered a problem that is really bothering me. I am testing the MoveIt!'s OMPL planners in an application where a cutting tool (which is the end effector of a manipulator) has to reach a specific point in a branch of a vine tree, all simulated in Gazebo. This tree is added as an collision object to the program, so MoveIt! is aware of it and should not collide with it. The normal behavior of this application is: I request a plan for the manipulator to go to that point, MoveIt! calculates the path and, If a solution is found, it is executed. However, sometimes MoveIt! says that a solution is found (path well calculated) but then the manipulator exhibits strange trajectories and hits the vine, not accomplishing its goal. This is really strange because it happens with all the planners of the OMPL implemented in MoveIt!, happening more often with some planners than others. The one that exhibits the best behaviour of them all is the BFMTkConfigDefault, failing only 4 times out of 20.
I am pretty sure that the controllers of the manipulator are well tuned because I can make the manipulator reach any point that is not a collision object. Plus, the manipulator reaches the point that I want a lot of times (which is not a collision object, of course). Can somebody explain me why the manipulator is sometimes hitting a tree even though MoveIt! says that a solution was found to the path problem and the path was well executed?
Thanks in advance, José Brito
@znbrito Does boundary of the collision object set correctly as its real shape; in Gazebo?
Hello @adroit89, thanks for your answer. Yes it is. When I first began with the tests, I noticed that the ground plane in RViz was intercepting the wheels of my robot (the manipulator is standing on top of a Husky robot).
To correct that, I checked the height between the wheel frame and the \base_footprint frame and started to spawn the tree with that height offset.
To test if the tree spawned in RViz corresponds to the one being spawned in Gazebo, I simply need my program to plan and execute the trajectory correctly. When that happens, the manipulator goes to the right point, both in Gazebo and i n RViz
Besides that, I am pretty sure that that is not the problem because, instead of spawning the tree as a collision object in RViz, I can simply map it in RViz using an Hokuyo laser, which makes MoveIt! detect it as a collision object. In both cases, the results are the same :/
Hi @znbrito ; you mentioned that you fixed the manipulator on Husky. So, I have a question now whether the manipulator should reach a point when the robot stops or when it moves around? If the robot moves during trajectory execution, some extra decision should be made.
Hi @adroit89. Whenever the manipulator is moving, the Husky is stopped, so that's not an extra problem for me to solve :)