joint_trajectory_action/result returning before motion complete

[BenRJG]

When trying to control the TM arm using Moveit, the execute commands end up finishing before the arm movement is actually completed, this leads to issues with larger movements as executions following this will try to run before the arm finishes moving. I've tested this with just using Moveit through RViz and when executing the motion, the joint_trajectory_action/result gets published to before the move is finished.

[KhalilSallem]

Hello, I faced the same issue while working with Moveit, I'll explain the details here: My code uses move_group to execute a trajectory and then a command to open the gripper. The execute function should wait for completion as mentioned here.

But in reality, the gripper opens whilst the robot is still executing the planned trajectory. I am not an expert, but i saw that the tm_driver returns that the trajectory is completed even when it is still running. Thus the action of FollowJointTrajectory is judged as completed when it is not.

So moveit thinks it completed the trajectory and the code executes the gripper command, while it should not.

[TechmanRobotIncOwner]

Hi, I would like to confirm the sequence of the execution in your code. Could you describe the sequence between the trajectory execution and trigger the IO signal?

[KhalilSallem]

hello, So we have an Onrobot gripper 2FG7 installed on the robot, we created a ROS action server to command the gripper through some global variables (we shared them in the Ethernet Salve data table, thus we can access them through the services: AskItem and Write item), the gripper works fine and everything is ok.

Now with moveit c++ interface, we plan and compute a cartesian path then we execute it using execute() function, which normally is blocking and waits for a confirmation from the robot driver that the trajectory is completed (this is what happens with other robots and what is mentioned in moveit documentation) this trajectory is sent to the robot using send script service.

the screenshot shows and execution time of 0.293s which is too fast for the trajectory we have and also shows that it instantly received the gripper action after the confirmation of trajectory execution, but in reality the robot is still executing the trajectory and moves the gripper.

screeshot i'll include a part of the code too so you can understand me more.

#include <cstdlib>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>

#include <gripper_action/gripperMoveAction.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "omron_pick_place_tutorial");
  ros::NodeHandle node_handle;

  ros::AsyncSpinner spinner(0);
  spinner.start();

/************************************* defining planning group ******************************************/
  static const std::string PLANNING_GROUP = "manipulator";

  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);

/************************************* defining planning scene and adding objects*****************************************/
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  /********************** velocity and acceleration params ***********************/
  move_group_interface.setMaxVelocityScalingFactor(0.1);
  move_group_interface.setMaxAccelerationScalingFactor(0.1);

/********************** planning params ***********************/
  move_group_interface.setPlanningTime(5.0);

/***************************************** Starting the gripper's action servers ***********************************************************/

  actionlib::SimpleActionClient<gripper_action::gripperMoveAction> ac("gripperMove", true);
  ROS_INFO("Waiting for action server to start.");
  ac.waitForServer(); 

/******************************************** definning a pre-grasp and grasp pose ************************************************/
  geometry_msgs::Pose target_pose1; //pre-grasp pose
  tf2::Quaternion orientation;
  orientation.setRPY(-3.1373504005853157, -0.010852950781762011, -1.6055350178197567);
  target_pose1.orientation = tf2::toMsg(orientation);
  target_pose1.position.x = 0.49910962160010736;
  target_pose1.position.y = -0.20725804709489565;
  target_pose1.position.z = 0.06382088938496083;

  std::vector<geometry_msgs::Pose> waypoints; // defining a pose vector to store the trajectory points (grasp trajectory)

  waypoints.push_back(target_pose1); // adding the first point (pre-grasp pose)

  geometry_msgs::Pose target_pose2 = target_pose1;
  target_pose2.position.z -= 0.035; // grasp pose (lower the z axis and keep the others)
  waypoints.push_back(target_pose2); // adding the second point

/********************************************** defining the grasp Trajectory ****************************************************/ 
  moveit_msgs::RobotTrajectory trajectory; //grasp trajectory 
  const double jump_threshold = 0.0;  //computation params
  const double eef_step = 0.1;
  double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); //computing the trajectory based on waypoints and params

/**************** definning A goals for the gripper action servers ***********************/

  gripper_action::gripperMoveGoal open;
  open.width = 70.0;
  open.force = 50;
  open.speed = 50;
  open.type = 0;

  gripper_action::gripperMoveGoal closed;
  closed.width = 40.0;
  closed.force = 50;
  closed.speed = 50;
  closed.type = 0;

/************************************* grasp trajectory execution  ***********************************************/

  ac.sendGoal(open); // opening the gripper for init    
  move_group_interface.execute(trajectory); // executing the computed grasp trajectory

/************************************* grasping the object **********************************************/
  ac.sendGoal(closed); //sending the closed goal to the gripper action server
  bool finished_before_timeout = ac.waitForResult(ros::Duration(5.0));
  if (finished_before_timeout)
    {
      ROS_INFO("Grip Action finished.");

    }
  else
       ROS_INFO("Grip Action did not finish before the time out."); 

  ros::waitForShutdown();

  return 0;
}

[TechmanRobotIncOwner]

Hi, There is a paragraph we would like to understand the function of "grasp trajectory execution" And we would suggest communicating with the robot driver and getting the value of the degree, or the other way to refer to the "QueueTag" function in our Expression Editor manual, which you could find in the ROS1 Driver section.

[mojervico]

I'm experiencing the same problem. I am testing with ROS Noetic Ubuntu 20.04 and with the tm5x-900_moveit_planning_execution.launch sample, everytime that i send the execution trajectory goal, the result return almost inmediatelly as succeed, while the robot is moving. Is there anything I can do to avoid that?

[KhalilSallem]

Hello, it has been a long time since i worked with this, but in our case, we implemented a workaround to solve this, we kept comparing the target pose value to the live pose feedback from the robot in a blocking loop, once the taget goal is reached the loop exits.