ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A
Ask Your Question

cartesian path cancelation in middle of execution

asked 2020-08-14 04:11:56 -0500

updated 2020-08-20 22:50:16 -0500

Hi everyone, I am facing an issue while trying to use cartesian paths with moveit. I am developing a GUI for my robot, which includes functionalities to achieve stop pause and continue for robot control.

I am using a staubli arm(tx260). I can stop active motions by using /stop_motion service provided by industrial_core. It works well when I use joint space methods. But when I use this service to stop Cartesian paths it cracks the program. I have tried to understand the reason but failed so far.

Right after calling /stop_motion service in log I sometimes see;

[ERROR] [1597395259.432551613]: Failed to initialize message
[ WARN] [1597395259.432589447]: Failed sent joint point, will try again

This most probably occurs because Cartesian paths are denser and I call service /stop_motion right away cutting the message initialization.

After I call /stop_motion service I also call move_group.stop() which results in;

[ INFO] [1597395259.441869902]: Received event 'stop'
[ INFO] [1597395259.441915581]: Cancelling execution for 
[ INFO] [1597395259.441944588]: Stopped trajectory execution.
[ INFO] [1597395259.442181566]: Controller  successfully finished
[ INFO] [1597395259.442224900]: Completed trajectory execution with status PREEMPTED ...
[ INFO] [1597395259.442394479]: Execution completed: PREEMPTED

which looks ok. But the problem occurs when I want to continue the path. In every motion command I keep a copy of initial goal, so the continue function just tries to move robot to there. But when I call move_group.execute(myplan); Trajectory wont be executed and I have to restart the program to control robot. I will be glad to hear if someone can show me what are possible mistakes I am doing here;

more information can be provided in case it is needed.

Edit for answer of @fvd;

There is really not much of error outputs excepts those two lines of my original question.

Now the exact function that I use is below Whenever pause is called(Through GUI) /stop_motion service is utilized to stop robot immediately and the 2 ros params are used to define the logic of pause, continue, After pause , the robot goal is reset and preempted, with the continue a new plan is being planned from the current pose to original goal(in base_link frame), so I think it should be not related to it start pose being to far from the current one. Please note that robot_tcp_goal_in_tool_space is not actually a pose , but distances to travel in x,y,z coordinates in tool space,

bool RobotController::moveEndEffectortoGoalinToolSpace(geometry_msgs::Pose robot_tcp_goal_in_tool_space,
                                                       moveit::planning_interface::MoveGroupInterface *move_group_ptr_, tf::TransformListener *listener_) {

    bool returnMoveitResult = false;
    // get given goal position(in tool_link frame)
    geometry_msgs::Pose msg_robot_goal_in_tool_link;
    msg_robot_goal_in_tool_link.position = robot_tcp_goal_in_tool_space.position;

    // we will trasnform the given goal from tool_link to base_link
    tf::Pose tf_robot_goal_in_tool_link;
    tf::poseMsgToTF(msg_robot_goal_in_tool_link, tf_robot_goal_in_tool_link);

    // get transform between tool_link and base_link
    tf::StampedTransform tool_to_base_link_transform;
    // lookup transform (this should be cached, since it’s probably static)
    listener_->lookupTransform(robot_base_link, robot_eef_link, ros::Time(0.0f), tool_to_base_link_transform);

    // do the transform by multiplying transform matrices with goal pose in tool_link
    tf::Pose tf_robot_goal_in_base_link;
    tf_robot_goal_in_base_link = tool_to_base_link_transform * tf_robot_goal_in_tool_link;

    // MOVEIT uses geometry_msgs type , so convert transformed goal ...
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2020-08-20 22:30:45 -0500

fvd gravatar image

It would probably be helpful if you posted the error message for the part where you say that the trajectory is not executed, and what "the continue function" does exactly. My guess is that the trajectory you try to "continue" on is not valid anymore.

If you are executing a trajectory, pre-empting it during execution, and then send the same trajectory (or the myplan object that still contains the same trajectory) to execute, then the first points of the trajectory will be too far away from the current state of the robot and it will refuse to move.

To fix this problem, either recalculate the cartesian path from the current state of the robot, or (less safe, probably less reliable and more manual work) remove the points from your trajectory that the robot has already executed.

edit flag offensive delete link more


Thank you for your answer, I have edited my question to include some more information. please comment if more is needed or something not clear

Fetullah Atas gravatar image Fetullah Atas  ( 2020-08-20 22:51:12 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2020-08-14 04:11:56 -0500

Seen: 298 times

Last updated: Aug 20 '20