Continuous Jerks during Cartesian Planning

edit

Hi

I am using ROS Indigo on Ubuntu 14.04. I am trying to move a 7 axis manipulator arm in Cartesian path in only "-X" direction, in small steps keeping the step size constant (0.0001 m). However I am seeing a jerky motion throughout the execution of whole event. I have tried using IterativeParabolicTimeParameterization and spline trajectory but the jerky motion still exists. I want to get a smooth trajectory execution. Can anyone tell me what I am doing wrong here ??

Here's my code :

int main(int argc, char **argv)
{
  ros::init(argc, argv, "circle");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  //  sleep(5.0);

  moveit::planning_interface::MoveGroup group("arm");
  moveit::planning_interface::PlanningSceneInterface plannning_scene_interface;
  tf::TransformListener listener;
  tf::StampedTransform transform;

  group.setGoalPositionTolerance(0.0001);
  group.setGoalOrientationTolerance(0.001);
  group.setPlannerId("RRTConnectkConfigDefault");

  robot_model_loader::RobotModelLoader model_loader("robot_description");

  std::vector<double> joint_positions;
  joint_positions.resize(7);
  geometry_msgs::PoseStamped pose;
  joint_positions[0] = -2.277;
  joint_positions[1] = -1.0;
  joint_positions[2] = 0.0;
  joint_positions[3] = -0.80;
  joint_positions[4] = 0.0;
  joint_positions[5] = -0.8;
  joint_positions[6] = 0;

  group.setJointValueTarget(joint_positions);

  std::vector<geometry_msgs::Pose> waypoints;

  group.move();

  sleep(2.0);

  listener.lookupTransform("world", group.getEndEffectorLink().c_str(),
                           ros::Time(0), transform);

  moveit::planning_interface::MoveGroup::Plan my_plan;

  pose.header.stamp = ros::Time::now();
  pose.header.frame_id = "/world";
  pose.pose.position.x = transform.getOrigin().getX();
  pose.pose.position.y = transform.getOrigin().getY();
  pose.pose.position.z = transform.getOrigin().getZ();
  pose.pose.orientation.x = transform.getRotation().getX();
  pose.pose.orientation.y = transform.getRotation().getY();
  pose.pose.orientation.z = transform.getRotation().getZ();
  pose.pose.orientation.w = transform.getRotation().getW();
  geometry_msgs::Pose pose2 = pose.pose;

  group.setMaxVelocityScalingFactor(0.50);
  group.setMaxAccelerationScalingFactor(0.50);

  robot_trajectory::RobotTrajectory rt(model_loader.getModel(), "arm");
  moveit_msgs::RobotTrajectory trajectory;

  const double jump_threshold = 0.0;
  const double eef_step = 0.0001;

  for (int i = 0; i <= 900; i++) {
    pose2.position.x -= 0.001;

    waypoints.clear();
    waypoints.push_back(pose2);

    double fraction = group.computeCartesianPath(waypoints, eef_step,
                                                 jump_threshold, trajectory);

    // ROS_INFO("Visualize the plan (%.2f%% achieved)", fraction*100.0);

    // Second get a RobotTrajectory from trajectory
    rt.setRobotTrajectoryMsg(robot_state::RobotState(model_loader.getModel()),
                             trajectory);

    // Thrid create a IterativeParabolicTimeParameterization object
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    trajectory_processing::SplineParameterization sp;

    // Fourth compute computeTimeStamps
    bool success = iptp.computeTimeStamps(rt, 0.5, 0.5);
    // bool success = sp.computeTimeStamps(rt, 0.5, 0.5);
    ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");

    // Get RobotTrajectory_msg from RobotTrajectory
    rt.getRobotTrajectoryMsg(trajectory);

    // Finally plan and execute the trajectory
    my_plan.trajectory_ = trajectory;
    ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%%  acheived)",
             fraction * 100.0);
    sleep(5.0);
    group.execute(my_plan);

    // sleep(2.0); }

    ros::shutdown();

    return 0;
  }
}