MoveGroupInterface plan execution takes too long
Hello,
I have multiple (10+) planned trajectories stored in a std::vector
, ie:
std::vector<moveit::planning_interface::MoveGroupInterface::Plan> trajArray;
These trajectories are executed one by one as follows.
for (unsigned long i = 0; i < trajArray.size(); i++)
move_group->execute(trajArray[i]);
It works OKey, but there is a quite big wait time (around 15-20 sec) between two plan execution,
i mean, the ith
plan is finished then the robot stays in its end position for 20 sec and just after that the (i+1)th
plan is executed.
What is the reason for this problem? And can this wait time be somehow reduced?
(Actually that is why i plan every trajectory in advance, to have a smooth execution one by one with minimum waiting).
Thanks for the help.
Asked by akosodry on 2019-07-17 04:01:43 UTC
Comments
It is possible to join two joint trajectories together into a single trajectory which would be the preferred way of doing what you want. A 20 second delay is very strange, but you will be unable to produce smooth motion even with no delay because the arm will need to come to a complete stop between trajectory executions.
Asked by PeteBlackerThe3rd on 2019-07-17 04:20:53 UTC
Yes, joining trajectories would solve it (actually i did that before). BUT: in that way i dont know when to actuate the tool that is attached to the EEF.
In the described way (above in the question) i have an array of trajectories, this allows me to know when to turn on/off the tool. Indeed, i
did NOT
use correctly the "smooth" word in the question. Its OK if it stops for a moment or two, but currently this 20 sec stop is too much. The word smooth was just used to refer to a minimum delay between two executions.What i observed is that
every execution
reports an[INFO]: ABORTED: Solution found but controller failed during execution
? Even tho the robotic arm executes the motion "perfectly".I'm testing currently in gazebo, the manipulation is performed based on a
position_controllers/JointTrajectoryController
.Thank you for the help in advance.
Asked by akosodry on 2019-07-17 07:06:05 UTC
@PeteBlackerThe3rd Do you have any idea based on my comment? Thank you in advance.
Asked by akosodry on 2019-07-18 02:14:11 UTC
From your description it sounds as if the goal tolerance is set too fine, so that the arm doesn't ever reach the goal from the point of view of the trajectory execution node. This project is described clearly by @gvdhoorn in this thread. The easiest solution will be to increase the goal tolerance in MoveIt.
Asked by PeteBlackerThe3rd on 2019-07-18 05:52:41 UTC
@PeteBlackerThe3rd thank you for the reply. The goal tolerance is set up in the
MoveGroupInterface
as follows:This i think its a quite loose constraint, especially in gazebo, where everything works "perfectly".
Then, could be a kind of timeout the reason that results in the 20 sec delay, since the desired pose is not reached within the tolerance.
Thanks for the link, im reading it.
Asked by akosodry on 2019-07-18 06:03:22 UTC
Yes, you're right about the timeout, that is the reason for the 20 second delay. I'd also point out that everything doesn't work "perfectly" in Gazebo. There are still PID controllers which move the simulated geometry to within a tolerance of their required locations, so there will still be errors just like in real life. Finally weather those tolerances are appropriate for your robot depends on many factors but I recommend relaxing them temporarily to test if they fix your problem or not, and then see how tight you can make them before the problem returns.
Asked by PeteBlackerThe3rd on 2019-07-18 06:10:17 UTC
Thank you very much for the kind help. I'm working on it and later i will post here the outcome.
Asked by akosodry on 2019-07-18 06:16:26 UTC
Hello @PeteBlackerThe3rd! I did some testing and it turns out that modifying the tolerance does not change the outcome. I even increased up to extreme (0.5 rad for orientation, 0.5 rad joint and 0.5 meter for position tolerances) numbers, but still, the execution was characterized by those quite long (20 sec) delays..
Then i did more testing, and found that moveit perception causes the slow-down of the execution. I commented out the RGBD sensor defined in the
sensors_3d.yaml
file. So if there is no perception/octomap building, then the execution issuper fast
.I had observed this phenomenon in past and asked about it here: LINK, but no answer came , therefore i continued to work on a different part of the project and i totally forgot this issue.
Can you advice something related/workaround to this problem? Thank you for the help in advance.
Asked by akosodry on 2019-07-19 06:39:50 UTC
Currently, this i what i have in the
sensors yaml
file:And in the sensor_manager.launch.xml file contains:
Asked by akosodry on 2019-07-19 06:44:53 UTC
That's interesting to know. Unfortunately this is going outside my experience now, I've not worked with MoveIt perception.
Asked by PeteBlackerThe3rd on 2019-07-19 06:59:12 UTC
@PeteBlackerThe3rd can you maybe suggest+ping someone who is involved and has experience with the problem? Thank you once more.
Asked by akosodry on 2019-07-20 03:07:08 UTC