Merge trajectories in real time

edit

My goal is to pick an object with small movements using UR5. I am getting the updated target pose through a visual feedback. For this purpose I am using a package developed by Team ViGIR : github.

To simulate the continuously changing visual feedback, I updated the original code in the following way :

  geometry_msgs::Pose moving_pose;

  moving_pose.position.x = 0.45;
  moving_pose.position.y = -0.2;
  moving_pose.position.z = 1.0;

  double angle = M_PI;
  Eigen::Quaterniond quat = Eigen::AngleAxis<double>(double(0), Eigen::Vector3d::UnitZ())
                          * Eigen::AngleAxis<double>(double(M_PI), Eigen::Vector3d::UnitX());
  moving_pose.orientation.w = quat.w();
  moving_pose.orientation.x = quat.x();
  moving_pose.orientation.y = quat.y();
  moving_pose.orientation.z = quat.z();

  if (debug_pose_pub_.getNumSubscribers() > 0){
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.frame_id = robot_model->getModelFrame();
    pose_stamped.header.stamp = ros::Time::now();
    tf::poseEigenToMsg(target_pose, pose_stamped.pose);
    debug_pose_pub_.publish(pose_stamped);
  }

  for (size_t i = 0; i < count; ++i){
      //simple test for moving target
      moving_pose.position.x += 0.02;
      context_->planning_scene_monitor_->updateFrameTransforms();

      if (stop_continuous_replanning_){
        context_->trajectory_execution_manager_->stopExecution();
        return;
      }

      robot_state::RobotState current_state = planning_scene->getCurrentState();   
      const robot_state::JointModelGroup* current_group = current_state.getJointModelGroup(group_name);    
      bool ik_solved = current_state.setFromIK(current_group, moving_pose);

But with this method, the merged trajectory is not smooth but step-like

So basically I observe jerks whenever a new trajectory is passed.

From the video, it looks like there's a gap between the current state and the starting state of the new trajectory. So the robot has to jump to the new starting pose, which is also visible in the above /joint_states plot.

My guess is that the new trajectory is planned from current state but because the merge time is in the future, the robot has already moved to a new state when the newly planned trajectory starts executing (Did I make my point clear?). And hence the jump.

Should I plan the trajectory from this new time in the future? For this I will need to know what my pose will be in the future. Or should I use smoothing algorithms, maybe trajectory filters.