Ask Your Question
1

Continuous Jerks during Cartesian Planning

asked 2018-07-15 23:44:06 -0500

Pulkit123 gravatar image

updated 2018-07-16 01:51:24 -0500

gvdhoorn gravatar image

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;
  }
}
edit retag flag offensive close merge delete

Comments

You're asking MoveIt to plan 900 individual trajectories of 1 mm and then executing all of them in each iteration, with each trajectory starting and ending at 0 velocity .. that will lead to some jerk, yes. Try planning the whole 90 cm in one trajectory, and don't use so many trajectory points.

gvdhoorn gravatar imagegvdhoorn ( 2018-07-16 01:47:30 -0500 )edit

Also, for the future: please format your code a bit nicer. Use the Preformatted Text button (the one with 101010).

gvdhoorn gravatar imagegvdhoorn ( 2018-07-16 01:52:09 -0500 )edit

Actually I wanted to implement a functionality in which the user controls the Cartesian path through slider control. The minimum resolution I set was 1 mm. The functionality is just like in universal robot where you press the direction arrow, and the robot follows the Cartesian path.

Pulkit123 gravatar imagePulkit123 ( 2018-07-16 03:43:50 -0500 )edit

This is a classic example of an xy-problem. It would have been better if you'd included the fact that you're looking to implement jogging functionality instead of asking about your selected solution.

gvdhoorn gravatar imagegvdhoorn ( 2018-07-16 07:33:52 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2018-07-16 07:38:35 -0500

gvdhoorn gravatar image

MoveIt was not created for the type of motion you're looking for. There is a big difference between a complete motion planning pipeline and the kind of low-level 'simple' cartesian jogging you're asking about.

Please see the following questions: #q245390 and #q272681.

edit flag offensive delete link more

Comments

I got it, my mistake. It seems like there is a ros-pkg "jog_arm" that implements this functionality. Thanks for the help.

Pulkit123 gravatar imagePulkit123 ( 2018-07-16 07:59:36 -0500 )edit
1

Could you please mark the question as answered by ticking the checkmark (✓) to the left of the answer if you feel it has been answered? Thanks.

gvdhoorn gravatar imagegvdhoorn ( 2018-07-16 08:07:02 -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

2 followers

Stats

Asked: 2018-07-15 23:44:06 -0500

Seen: 280 times

Last updated: Jul 16 '18