Trajectory execution error code

[Harsh Deshpande]

I am sending a trajectory to robot arm which uses joint_trajectory_controller. After executing the trajectory, I need to check if the robot has indeed reached it's goal position before planning for the next goal.

My idea was to use the

control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED

which FollowJointTrajectoryControllerHandle should be able to return [1]. But as can be seen, it always returns true without returning the result of the goal. Also the ExecuteTrajectoryAction capability gets the result of the trajectroy execution in the form of

moveit_msgs::MoveItErrorCodes [2]. I can implement something like in URDriver.py in my node

def within_tolerance(a_vec, b_vec, tol_vec):
    for a, b, tol in zip(a_vec, b_vec, tol_vec):
        if abs(a - b) > tol:
            return False
    return True

but I want to know if there's any way I could use GOAL_TOLERANCE_VIOLATED.

Thanks!

[1] https://github.com/ros-planning/moveit/blob/4bc43c56ca2a5293f3ef8d55796ec586cfde5f69/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h#L59

[2] http://docs.ros.org/jade/api/moveit_ros_move_group/html/execute__trajectory__action__capability_8cpp_source.html#l00102

[v4hn]

Hello,

Please use answers.ros.org instead of this list.

You dug quite deep into the framework, but you did not
say which interface you actually use to request the execution
of the trajectory. What is the code you execute and what is
the unexpected behavior? If you use move_group's ExecuteTrajectoryAction,
this should already return an error code when the controller returns
GOAL_TOLERANCE_VIOLATED. We don't have this exact error code in MoveItErrorCodes though.
At first glance I would expect CONTROL_FAILED in this case.


v4hn

On Mon, Sep 18, 2017 at 07:25:23AM -0700, Harsh Deshpande wrote:
> I am sending a trajectory to robot arm which uses
> joint_trajectory_controller. After executing the trajectory, I need to
> check if the robot has indeed reached it's goal position before planning
> for the next goal.
>
> My idea was to use the
>
> control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED
>
> which FollowJointTrajectoryControllerHandle should be able to return [1].
> But as can be seen, it always returns true without returning the result of
> the goal. Also the ExecuteTrajectoryAction capability gets the result of
> the trajectroy execution in the form of
>
> moveit_msgs::MoveItErrorCodes [2]. I can implement something like in URDriver.py in my node
>
> def within_tolerance(a_vec, b_vec, tol_vec):
> for a, b, tol in zip(a_vec, b_vec, tol_vec):
> if abs(a - b) > tol:
> return False
> return True
>
> but I want to know if there's any way I could use GOAL_TOLERANCE_VIOLATED.
>
> Thanks!

--
Michael Görner, M.Sc. Cognitive Science, PhD Student
Universität Hamburg
Faculty of Mathematics, Informatics and Natural Sciences
Department of Informatics
Group Technical Aspects of Multimodal Systems

Vogt-Kölln-Straße 30
D-22527 Hamburg

Room: F-315
Phone: +49 40 42883-2432
Website: https://tams.informatik.uni-hamburg.de/people/goerner/

[Harsh Deshpande]

Sorry about that.

I am using moveit::planning_interface::MoveGroup interface to execute trajectories.
The code is a series of plan and execute commands which the arm is supposed to move to
one after the other. But at times, it misses one of the targets and directly moves to the next one
(which is actually dangerous for the arm). I am assuming the planning takes too long and the node doesn't wait for it finish...?

As you said, ideally the ExecuteTrajectoryAction should return CONTROL_FAILED error when controller returns a GOAL_TOLERANCE_VIOLATED.
But it is never set as an error in the MoveitSimpleControllerManager. I can only check if the action was a success or pre-empted / aborted. Which should be OK for now.

Another hack I can think of is to subscribe to the /follow_joint_trajectory/result.