Moveit does not wait for trajectory completion

asked 2020-06-02 23:48:47 -0500

PratNag gravatar image

updated 2020-06-02 23:50:26 -0500

I am running ROS melodic on Ubuntu 18.04 with latest moveit binary.

I am working with interbotix arms sdk to send multiple points for the robot to move to. The code is simple as mentioned in https://ros-planning.github.io/moveit...

> while row is not None:
>                     joint_goal = self.group.get_current_joint_values()
>                     joint_goal[0] = row[0]
>                     joint_goal[1] = row[1]
>                     joint_goal[2] = row[2]
>                     joint_goal[3] = row[3]
>                     joint_goal[4] = row[4]
>                     joint_goal[5] = row[5]
>                     rospy.logwarn("sent goal to moveit")
>                     self.group.go(joint_goal, wait=True)
>                     rospy.logwarn("goal reached sending
> next goal if available")
>                     self.group.stop()

What happens is first point is sent and immediately the second one is sent and so on.

So if i want to send point a, b, c and d, the robot just moves from point a to point d. It changes path midway. in gazebo it works perfectly but not on real robot.

If i send only movement from point a to point b then it works perfectly on the real robot. If I give a big delay for between multiple points then it works.

The issue it seems is "self.group.go(joint_goal, wait=True)" when this is executed the script should wait until the robot reaches the goal and only then move to the next line of code for execution but it does not.

What could be the reason? Is there any other way externally I can check if the goal is reached and then only move to the next line.

edit retag flag offensive close merge delete

Comments

Hello. i would like to give a better response but unfortunatelly I am not familiar with trajectory execution using rospy.

Anyway, comparing what is happenning to you to what i would though it could be with the c++ variant, i would say that error appears when storing this points.

Generally when you declare a path execution ( if i am not wrong go is equal to plan + execute) you can send the execution information of the differents point before the past points are executed, and all the information of all the points still not used is stored inside a buffer. Sometimes happens that when you define a small buffer capacity(or by default the buffer is defined as small), these points still not executed are lost (seems exactly what is happening to you!).

I am not quite sure that the error in your case i here, because i have done a ...(more)

Solrac3589 gravatar image Solrac3589  ( 2020-06-03 00:51:52 -0500 )edit

Hello, Yes I thought of that. But couldn't see any such buffer in the python api. And also the weird thing is it works with gazebo perfectly. It's only issue with the real robot.

PratNag gravatar image PratNag  ( 2020-06-03 00:57:19 -0500 )edit

The issue it seems is "self.group.go(joint_goal, wait=True)" when this is executed the script should wait until the robot reaches the goal and only then move to the next line of code for execution but it does not.

Taking a step back: you assume it's MoveIt where the problem is. In reality, MoveIt depends on the driver to correctly report progress executing the active goal.

The fact the code "works with Gazebo" suggests that MoveIt is doing OK, and your code is OK, but the thing which has changed (ie: Gazebo replaced by a real robot driver) is somehow involved.

I would take a look at the driver and check it does what it is supposed to.

If the driver for instance accepts a goal and immediately returns 'completed', MoveIt will not be able to know whether or not that is true.

gvdhoorn gravatar image gvdhoorn  ( 2020-06-03 02:12:59 -0500 )edit

Yes makes sense. I will look into the driver.

PratNag gravatar image PratNag  ( 2020-06-03 02:21:39 -0500 )edit