Using Time Optimal Parameterization with Cartesian Paths

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

DrewHamilton
31 ●4 ●5 ●8

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 ( 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 ( 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 ( 2021-01-03 04:12:39 -0500 )edit

add a comment

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);

Using Time Optimal Parameterization with Cartesian Paths

Comments

1 Answer

Question Tools

Stats

Related questions

Using Time Optimal Parameterization with Cartesian Paths edit

Comments

1 Answer

Question Tools

Stats

Related questions

Using Time Optimal Parameterization with Cartesian Paths