ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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);
2 | No.2 Revision |
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: