Ask Your Question
1

Using Time Optimal Parameterization with Cartesian Paths

asked 2020-12-09 23:24:15 -0500

DrewHamilton gravatar image

updated 2020-12-09 23:24:51 -0500

Hi All,

I will keep this as brief as possible.

Background: I have been playing around with Time Optimal Parameterization. I hacked a portion of the MoveIt code in "add_time_optimal_parameterization.cpp" to take in parameters from a launch file for resample_dt, path_tolerance, and min_angle_change so I could play around with "higher resolution" trajectories.

Question: I noticed, while plotting some of these trajectories in RQT that Cartesian paths do not get resampled (as far as I can tell). I'm wondering if Cartesian paths are treated differently somehow. I couldn't find anything explicitly refusing to work with Cartesian paths in the "time_optimal_trajectory_generation.cpp" file.

Here's a normal path with resample_dt set to 0.03 image description

And here's a cartesian path with resample_dt set to 0.03 image description

Just curious.

I'll also take this opportunity to re-apologize to gvdhoorn for indirectly being a jerk in a previous question. Please forgive me.

My robot and images of trajectories are on my github: https://github.com/drewhamiltonasdf/c...

Best, Drew Hamilton

P.S. Would it be a silly MoveIt! pull request to add this option to set these variables from the parameter server?

edit retag flag offensive close merge delete

Comments

I believe this is all because the cartesian path generator does not support the general planning request adapter setup.

I found this answer here: https://github.com/ros-planning/movei...

the hard-coded time parameterization should probably be configurable once we have other working methods upstream. A simple ROS parameter to choose, or disable the parameterization if you do it yourself afterwards, could suffice imo.

Background information: Until not too long ago there was no time parameterization in the capability. I just added IPTP (wink) in fc7da44

The code is found here: moveit/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp

DrewHamilton gravatar image DrewHamilton  ( 2020-12-12 21:03:43 -0500 )edit

As far as setting the variables resample_dt etc from the parameter server goes. I hacked something together by editing add_time_optimal_parameterization.cpp, add something like:

 double path_tolerance_override;
    if (!nh.getParam("/path_tolerance", path_tolerance_override))
    {
        ROS_INFO("No param found for path_tolerance");
    }
    else
    {
        path_tolerance = path_tolerance_override;
        ROS_INFO("Overriding path_tolerance parameters: %f", path_tolerance);
    }

for all the three variables in question to the top of the initialize() method, and added default values that are overwritten if there is a matching parameter found. double path_tolerance = 0.1; double resample_dt = 0.1; double min_angle_change = 0.001;

I opened a moveit pull request for this added feature. Who knows if it will be accepted.

DrewHamilton gravatar image DrewHamilton  ( 2020-12-12 21:47:23 -0500 )edit

Seems like this is a duplicate of #q326511.

Nice of you to show the code you ended up using though.

gvdhoorn gravatar image gvdhoorn  ( 2021-01-03 04:12:39 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-01-02 19:28:12 -0500

DrewHamilton gravatar image

updated 2021-01-02 19:30:35 -0500

See the above comments for notes about why MoveIt, barring a pull request, won't compute time optimal cartesian paths from within RViz. With that said, you can still program your own time optimal cartesian paths in a move_group node. This worked for me:

  move_group.setStartStateToCurrentState();
  geometry_msgs::Pose start_pose = move_group.getCurrentPose().pose;

  std::vector<geometry_msgs::Pose> waypoints;
  waypoints.push_back(start_pose);

  geometry_msgs::Pose target_pose3 = start_pose;

  target_pose3.position.x += inch_to_m(8);
  target_pose3.position.z += inch_to_m(-10);
  target_pose3.position.y += inch_to_m(-8);
  waypoints.push_back(target_pose3);  

  moveit_msgs::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;
  const double eef_step = 0.01;
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  ROS_INFO_NAMED("tutorial", "Visualizing (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

  robot_trajectory::RobotTrajectory trajectory_for_totg(move_group.getRobotModel(), PLANNING_GROUP);

  robot_state::RobotState start_state(*move_group.getCurrentState());

  trajectory_for_totg.setRobotTrajectoryMsg(start_state, trajectory);

  //Get these from param server later, plz
  double path_tolerance = 0.002;
  double resample_dt = 0.03;
  double min_angle_change = 0.001;

  trajectory_processing::TimeOptimalTrajectoryGeneration time_param(path_tolerance, resample_dt, min_angle_change);
  time_param.computeTimeStamps(trajectory_for_totg, 1.0);  //1.0 accel scaling

  moveit_msgs::RobotTrajectory totg_trajectory;
  trajectory_for_totg.getRobotTrajectoryMsg(totg_trajectory);

  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);

  const moveit::core::LinkModel* link_model6 = move_group.getCurrentState()->getLinkModel("link6");
  visual_tools.publishTrajectoryLine(totg_trajectory, link_model6, joint_model_group, rviz_visual_tools::ORANGE);

  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window move arm.");

  move_group.execute(totg_trajectory);

A nice time optimal cartesian path: image description

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-12-09 23:24:15 -0500

Seen: 169 times

Last updated: Jan 02 '21