cartesian path cancelation in middle of execution
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 ...