diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 0558a605ab..07cb9874c1 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -264,6 +264,11 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla ROS_DEBUG_STREAM("Motion planner reported a solution path with " << state_count << " states"); if (check_solution_paths_) { + visualization_msgs::MarkerArray arr; + visualization_msgs::Marker m; + m.action = visualization_msgs::Marker::DELETEALL; + arr.markers.push_back(m); + std::vector index; if (!planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index)) { @@ -301,7 +306,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla << private_nh_.resolveName(MOTION_CONTACTS_TOPIC)); // call validity checks in verbose mode for the problematic states - visualization_msgs::MarkerArray arr; for (std::size_t it : index) { // check validity with verbose on @@ -325,8 +329,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } } ROS_ERROR_STREAM("Completed listing of explanations for invalid states."); - if (!arr.markers.empty()) - contacts_publisher_.publish(arr); } } else @@ -335,6 +337,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } else ROS_DEBUG("Planned path was found to be valid when rechecked"); + contacts_publisher_.publish(arr); } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index aa343aead9..2ae3f98e16 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -337,28 +337,20 @@ void MotionPlanningDisplay::updateBackgroundJobProgressBar() if (n == 0) { - p->setValue(p->maximum()); - p->update(); p->hide(); p->setMaximum(0); + p->setValue(0); } else { - if (n == 1) - { - if (p->maximum() == 0) - p->setValue(0); - else - p->setValue(p->maximum() - 1); - } - else + if (p->maximum() < n) // increase max { - if (p->maximum() < n) - p->setMaximum(n); - else - p->setValue(p->maximum() - n); + p->setMaximum(n); + if (n > 1) // only show bar if there will be a progress to show + p->show(); } - p->show(); + else // progress + p->setValue(p->maximum() - n); p->update(); } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 652c2e0e30..0ee9b0127c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -335,6 +335,9 @@ void MotionPlanningFrameJointsWidget::updateNullspaceSliders() if (i == 0) nullspace_.resize(0, 0); + // show/hide dummy slider + ui_->dummy_ns_slider_->setVisible(i == 0); + // hide remaining sliders for (; i < ns_sliders_.size(); ++i) ns_sliders_[i]->hide(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 221318fd01..346228701e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -2042,82 +2042,12 @@ This is usually achieved by random seeding, which can flip the robot configurati 0 - - - 0 - 5 - - 16777215 5 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 0 - 0 - - - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 0 - 0 - - - - - - - - - 240 - 240 - 240 - - - - - - - 255 - 0 - 0 - - - - - - <html><head/><body><p>Progress of background jobs</p></body></html> diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui index 389365e9bd..ecde6d0514 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui @@ -43,14 +43,44 @@ - + + + The sliders below allow for jogging the nullspace of the current configuration, +i.e. trigger joint motions that don't affect the end-effector pose. + +Typically, redundant arms (with 7+ joints) offer such a nullspace. +However, also singular configurations provide a nullspace. + +Each basis vector of the (linear) nullspace is represented by a separate slider. + Nullspace exploration: - + + + + + false + + + The slider will become active if the current robot configuration has a nullspace. +That's typically the case for redundant robots, i.e. 7+ joints, or singular configurations. + + + -1 + + + 1 + + + Qt::Horizontal + + + +