diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 083afcfc..35e4df2f 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -76,9 +76,9 @@ public: protected: SolutionBase* storeSequential(const std::vector& sub_trajectories, const std::vector& intermediate_scenes); - robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state); + robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state); protected: GroupPlannerVector planner_; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 04e754dc..db2bb8af 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -64,6 +64,7 @@ MOVEIT_CLASS_FORWARD(Task) */ class Task : protected WrapperBase { public: + // +1 TODO: move into MoveIt! core static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model, const std::string &ns = "move_group", const std::string &planning_plugin_param_name = "planning_plugin", @@ -81,6 +82,7 @@ public: /// load robot model from given parameter void loadRobotModel(const std::string& robot_description = "robot_description"); + // TODO: use Stage::insert as well? void add(Stage::pointer &&stage); void clear() override; @@ -111,6 +113,7 @@ public: /// publish all top-level solutions void publishAllSolutions(bool wait = true); + // +1 TODO: convenient access to arbitrary stage by name. traverse hierarchy using / separator? /// access stage tree ContainerBase *stages(); const ContainerBase *stages() const; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 1281675e..bbd2ff81 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -116,6 +116,8 @@ typedef std::vector> IKSolutions; namespace { +// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... +// move into MoveIt! core bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d pose, const robot_model::LinkModel* link) { @@ -215,6 +217,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s) assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff(); + // -1 TODO: this should not be necessary in my opinion: Why do you think so? + // It is, because the properties on the interface might change from call to call... // enforced initialization from interface ensures that new target_pose is read properties().performInitFrom(INTERFACE, s.start()->properties(), true); const auto& props = properties(); @@ -376,6 +380,9 @@ void ComputeIK::onNewSolution(const SolutionBase &s) spawn(InterfaceState(scene), std::move(solution)); } + // TODO: magic constant should be a property instead ("current_seed_only", or equivalent) + // Yeah, you are right, these are two different semantic concepts: + // One could also have multiple IK solutions derived from the same seed if (!succeeded && max_ik_solutions == 1) break; // first and only attempt failed } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index beeb19b6..a8f2735c 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -126,7 +126,7 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& const unsigned int num = jm->getVariableCount(); Eigen::Map positions_from (from.getJointPositions(jm), num); Eigen::Map positions_to (to.getJointPositions(jm), num); - if (!positions_from.array().isApprox(positions_to.array())) { + if (!positions_from.array().isApprox(positions_to.array(), 1e-4)) { ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() << "] != [" << positions_to.transpose() << "]"); @@ -158,7 +158,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions); robot_trajectory::RobotTrajectoryPtr trajectory; - if (!pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints)) + if (!pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints)) break; sub_trajectories.push_back(trajectory); @@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { // mark solution as failure solution->setCost(std::numeric_limits::infinity()); } else { - robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); + auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); if (t) { connect(from, to, SubTrajectory(t)); return true; @@ -218,11 +218,16 @@ SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state) +robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state) { + // no need to merge if there is only a single sub trajectory + if (sub_trajectories.size() == 1) + return sub_trajectories[0]; + auto jmg = merged_jmg_.get(); + assert(jmg); robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg); // TODO: check merged trajectory for collisions return trajectory; diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 0f83286d..2c788418 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -122,6 +122,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning } // Cartesian targets require the link name + // TODO: use ik_frame property as in ComputeIK std::string link_name = props.get("link"); const moveit::core::LinkModel* link; if (link_name.empty()) @@ -241,10 +242,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning // add an arrow marker visualization_msgs::Marker m; + // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" m.ns = props.get("marker_ns"); if (!m.ns.empty()) { m.header.frame_id = scene->getPlanningFrame(); if (linear_norm > 1e-3) { + // +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder) rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN : rviz_marker_tools::RED); rviz_marker_tools::makeArrow(m, linear_norm); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 2c6b66d3..7b916f6f 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -48,7 +48,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan , planner_(planner) { auto& p = properties(); - p.declare("timeout", 10.0, "planning timeout"); + p.declare("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage p.declare("group", "name of planning group"); p.declare("link", "", "link to move (default is tip of jmg)"); @@ -184,6 +184,8 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP return success; } +// -1 TODO: move these as default implementation to PropagateEitherWay? +// Essentially, here compute() is a class-specific worker function bool MoveTo::computeForward(const InterfaceState &from){ planning_scene::PlanningScenePtr to; SubTrajectory trajectory; diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 2970a130..fcb19e30 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -106,9 +106,10 @@ public: const moveit::core::RobotStatePtr& getWayPointPtr(size_t index) const { return getWayPointPtr(indexPair(index)); } + const planning_scene::PlanningSceneConstPtr& startScene() const { return start_scene_; } const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const; const planning_scene::PlanningSceneConstPtr& scene(size_t index) const { - if (index == steps_) + if (index >= steps_) return data_.back().scene_; return scene(indexPair(index)); } diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h index cbd86807..ca873ea4 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h @@ -88,7 +88,7 @@ protected: QPushButton* button_; bool paused_; - int last_way_point_; + bool empty_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index b5f5d32c..dd79b4e2 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -156,9 +156,9 @@ protected: void unsetRobotColor(rviz::Robot* robot); DisplaySolutionPtr displaying_solution_; - DisplaySolutionPtr solution_to_display_; + DisplaySolutionPtr next_solution_to_display_; std::vector trail_; - bool animating_ = false; + bool animating_ = false; // auto-progressing the current waypoint? bool drop_displaying_solution_ = false; bool locked_ = false; int current_state_ = -1; diff --git a/visualization/visualization_tools/src/task_solution_panel.cpp b/visualization/visualization_tools/src/task_solution_panel.cpp index 6ef190ef..4e59b236 100644 --- a/visualization/visualization_tools/src/task_solution_panel.cpp +++ b/visualization/visualization_tools/src/task_solution_panel.cpp @@ -41,6 +41,7 @@ namespace moveit_rviz_plugin { TaskSolutionPanel::TaskSolutionPanel(QWidget* parent) : Panel(parent) { + empty_ = false; } TaskSolutionPanel::~TaskSolutionPanel() @@ -92,16 +93,16 @@ void TaskSolutionPanel::onDisable() void TaskSolutionPanel::update(int way_point_count) { - int max_way_point = std::max(0, way_point_count - 1); + int max_way_point = std::max(1, way_point_count - 1); + empty_ = (way_point_count == 0); - slider_->setEnabled(way_point_count != 0); - button_->setEnabled(way_point_count != 0); + slider_->setEnabled(way_point_count >= 0); + button_->setEnabled(way_point_count >= 0); - last_way_point_ = max_way_point; paused_ = false; - slider_->setSliderPosition(0); slider_->setMaximum(max_way_point); - maximum_label_->setText(QString::number(max_way_point)); + maximum_label_->setText(way_point_count >= 0 ? QString::number(way_point_count) : ""); + slider_->setSliderPosition(0); } void TaskSolutionPanel::pauseButton(bool pause) @@ -115,7 +116,7 @@ void TaskSolutionPanel::pauseButton(bool pause) { button_->setText("Pause"); paused_ = false; - if (slider_->sliderPosition() == last_way_point_) + if (slider_->sliderPosition() == slider_->maximum()) slider_->setSliderPosition(0); } } @@ -127,7 +128,11 @@ void TaskSolutionPanel::setSliderPosition(int position) void TaskSolutionPanel::sliderValueChanged(int value) { - minimum_label_->setText(QString::number(value)); + QString text; + if (!slider_->isEnabled()) text = ""; + else if (empty_) text = value == 0 ? "S" : "E"; + else text = QString::number(value+1); + minimum_label_->setText(text); } void TaskSolutionPanel::buttonClicked() diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 2b8e327b..189a1993 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -161,7 +161,7 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi TaskSolutionVisualization::~TaskSolutionVisualization() { clearTrail(); - solution_to_display_.reset(); + next_solution_to_display_.reset(); displaying_solution_.reset(); scene_render_.reset(); @@ -233,9 +233,11 @@ void TaskSolutionVisualization::onRobotModelLoaded(robot_model::RobotModelConstP void TaskSolutionVisualization::reset() { clearTrail(); - solution_to_display_.reset(); + next_solution_to_display_.reset(); displaying_solution_.reset(); - animating_ = false; + current_state_ = -1; + if (slider_panel_) + slider_panel_->update(-1); robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); @@ -253,16 +255,19 @@ void TaskSolutionVisualization::clearTrail() void TaskSolutionVisualization::changedLoopDisplay() { - if (loop_display_property_->getBool() && slider_panel_) + // restart animation if current_state_ is at end and looping got activated + if (displaying_solution_ && loop_display_property_->getBool() && + slider_panel_ && slider_panel_->isVisible() && + current_state_+1 >= (int)displaying_solution_->getWayPointCount()) { + current_state_ = -1; slider_panel_->pauseButton(false); + } } void TaskSolutionVisualization::changedTrail() { clearTrail(); - DisplaySolutionPtr t = solution_to_display_; - if (!t) - t = displaying_solution_; + DisplaySolutionPtr t = displaying_solution_; if (!t || !trail_display_property_->getBool()) { setVisibility(trail_scene_node_, main_scene_node_, false); @@ -273,8 +278,7 @@ void TaskSolutionVisualization::changedTrail() setVisibility(trail_scene_node_, main_scene_node_, true); int stepsize = trail_step_size_property_->getInt(); - // always include last trajectory point - trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize)); + trail_.resize(t->getWayPointCount() / stepsize); for (std::size_t i = 0; i < trail_.size(); i++) { int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point @@ -325,7 +329,8 @@ void TaskSolutionVisualization::onDisable() parent_scene_node_->removeChild(main_scene_node_); displaying_solution_.reset(); - animating_ = false; + next_solution_to_display_.reset(); + current_state_ = -1; if (slider_panel_) { slider_panel_was_visible_ = slider_panel_->isVisible(); slider_panel_->onDisable(); @@ -374,107 +379,121 @@ float TaskSolutionVisualization::getStateDisplayTime() void TaskSolutionVisualization::update(float wall_dt, float ros_dt) { - if (drop_displaying_solution_) - { - animating_ = false; + if (drop_displaying_solution_) { + current_state_ = -1; displaying_solution_.reset(); - slider_panel_->update(0); + if (slider_panel_) slider_panel_->update(-1); drop_displaying_solution_ = false; } - if (!animating_) - { // finished last animation + if (current_state_ < 0 || // last animation finished + (slider_panel_ && slider_panel_->isPaused())) { boost::mutex::scoped_lock lock(display_solution_mutex_); // new trajectory available to display? - if (solution_to_display_ && (!locked_ || !displaying_solution_)) { + if (next_solution_to_display_ && (!locked_ || !displaying_solution_)) { + current_state_ = -1; animating_ = true; - displaying_solution_ = solution_to_display_; + displaying_solution_ = next_solution_to_display_; changedTrail(); if (slider_panel_) - slider_panel_->update(solution_to_display_->getWayPointCount()); - } - else if (displaying_solution_) { - if (loop_display_property_->getBool()) { - animating_ = true; - } else if (slider_panel_ && slider_panel_->isVisible()) { - if (slider_panel_->getSliderPosition() >= (int)displaying_solution_->getWayPointCount() - 1) - return; // nothing more to do - else - animating_ = true; - } else if (locked_) - return; - } - solution_to_display_.reset(); - - if (animating_) - { - // restart animation - current_state_ = -1; - trail_scene_node_->setVisible(false); + slider_panel_->update(next_solution_to_display_->getWayPointCount()); } + // also reset if locked_ + next_solution_to_display_.reset(); + } + if (!displaying_solution_) { + animating_ = false; + setVisibility(); + return; } - if (animating_) - { - int previous_state = current_state_; - int waypoint_count = displaying_solution_->getWayPointCount(); + int previous_state = current_state_; + // for an empty trajectory, show the start and end state at least + int waypoint_count = displaying_solution_->getWayPointCount(); + int max_state_index = std::max(1, waypoint_count); + if (slider_panel_ && slider_panel_->isVisible()) { + animating_ = !slider_panel_->isPaused(); + if (current_state_ >= 0) + // user can override current_state_ at any time with slider + current_state_ = slider_panel_->getSliderPosition(); + } else if (current_state_ < max_state_index) + animating_ = true; // auto-activate animation if slider_panel_ is hidden + + QString msg = "no change"; + if (animating_ && current_state_ == previous_state) { + // auto-advance current_state_ based on time progress current_state_time_ += wall_dt; float tm = getStateDisplayTime(); - if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused()) - current_state_ = slider_panel_->getSliderPosition(); - else if (current_state_ < 0) { // special case indicating restart of animation + if (current_state_ < 0) { // special case indicating restart of animation current_state_ = 0; current_state_time_ = 0.0; + trail_scene_node_->setVisible(false); + msg = "restart"; } else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time - while (current_state_ < waypoint_count && + while (current_state_ < max_state_index && (tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) { - current_state_time_ -= tm; ++current_state_; + current_state_time_ -= tm; } } else if (current_state_time_ > tm) { // fixed display time per state + ++current_state_; current_state_time_ = 0.0; - ++current_state_; - } - - if (current_state_ == previous_state) - return; - - if (current_state_ < waypoint_count) - { - renderWayPoint(current_state_, previous_state); - - int stepsize = trail_step_size_property_->getInt(); - for (int i = std::max(0, previous_state / stepsize), - end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) - trail_[i]->setVisible(true); - } - else - { - animating_ = false; // animation finished - if (!loop_display_property_->getBool() && slider_panel_) - slider_panel_->pauseButton(true); - // ensure to render end state - renderWayPoint(waypoint_count, previous_state); + msg = "progress"; } + } else if (current_state_ != previous_state) { // current_state_ changed from slider + current_state_time_ = 0.0; + msg = "slider"; } + + if ((waypoint_count > 0 && current_state_ >= max_state_index) || + (waypoint_count == 0 && current_state_ > max_state_index)) { // animation finished? + if (loop_display_property_->getBool()) + current_state_ = -1; // restart in next cycle + else { + current_state_ = max_state_index; + if (slider_panel_) + slider_panel_->pauseButton(true); + } + setVisibility(); + return; + } + if (current_state_ == previous_state) + return; + + renderWayPoint(current_state_, previous_state); + + // show / hide trail between start .. end + int stepsize = trail_step_size_property_->getInt(); + int start = std::max(0, previous_state / stepsize); + int end = current_state_ / stepsize; + bool show = start <= end; + if (!show) std::swap(start, end); + end = std::min(end, trail_.size()); + for (; start < end; ++start) trail_[start]->setVisible(show); + setVisibility(); } void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) { + assert(index >= 0); + size_t waypoint_count = displaying_solution_->getWayPointCount(); moveit::core::RobotStateConstPtr robot_state; planning_scene::PlanningSceneConstPtr scene; - if (index == displaying_solution_->getWayPointCount()) { - // render last state - scene = displaying_solution_->scene(index); - renderPlanningScene (scene); - robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); + if (index+1 >= waypoint_count) { + if (index == 0 && waypoint_count == 0) + // special case: render start scene + scene = displaying_solution_->startScene(); + else // render end state + scene = displaying_solution_->scene(waypoint_count); + renderPlanningScene (scene); + robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); } else { auto idx_pair = displaying_solution_->indexPair(index); scene = displaying_solution_->scene(idx_pair); - if (previous_index < 0 || + if (previous_index < 0 || previous_index >= (int)waypoint_count || displaying_solution_->indexPair(previous_index).first != idx_pair.first) { // switch to new stage: show new planning scene renderPlanningScene (scene); @@ -532,7 +551,7 @@ void TaskSolutionVisualization::showTrajectory(const DisplaySolutionPtr& s, bool { if (lock_display || !s->empty()) { boost::mutex::scoped_lock lock(display_solution_mutex_); - solution_to_display_ = s; + next_solution_to_display_ = s; if (lock_display) locked_ = drop_displaying_solution_ = true; else if (interrupt_display_property_->getBool())