diff --git a/.clang-tidy b/.clang-tidy index 9589fc7d..680317d8 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,5 +1,7 @@ --- Checks: 'performance-*, + -performance-noexcept-move-constructor, + -clang-analyzer-core.CallAndMessage, llvm-namespace-comment, modernize-redundant-void-arg, modernize-use-nullptr, @@ -12,7 +14,6 @@ Checks: 'performance-*, readability-redundant-string-cstr, readability-simplify-boolean-expr, readability-container-size-empty, - readability-identifier-naming, ' HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h' AnalyzeTemporaryDtors: false @@ -25,14 +26,14 @@ CheckOptions: value: '2' # type names - key: readability-identifier-naming.ClassCase - value: aNy_CasE # CamelCase + value: CamelCase - key: readability-identifier-naming.EnumCase value: CamelCase - key: readability-identifier-naming.UnionCase value: CamelCase # method names - key: readability-identifier-naming.MethodCase - value: aNy_CasE # camelBack + value: camelBack # variable names - key: readability-identifier-naming.VariableCase value: lower_case diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 22c31ed2..272f8798 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -24,7 +24,7 @@ jobs: -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith - IMAGE: rolling-source CXX: clang++ - CLANG_TIDY: true + CLANG_TIDY: pedantic CXXFLAGS: >- -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-deprecated-copy @@ -56,16 +56,18 @@ jobs: name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}" runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: Cache ccache - uses: pat-s/always-upload-cache@v2.1.5 + uses: rhaschke/cache@main with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} + env: + GHA_CACHE_SAVE: always - id: ici name: Run industrial_ci @@ -73,7 +75,7 @@ jobs: env: ${{ matrix.env }} - name: Upload test artifacts (on failure) - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: name: test-results-${{ matrix.env.IMAGE }} @@ -86,7 +88,7 @@ jobs: workdir: ${{ env.BASEDIR }}/target_ws ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report - uses: codecov/codecov-action@v2 + uses: codecov/codecov-action@v3 if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/target_ws/coverage.info diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index ed4f8c72..f4893507 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -44,10 +44,3 @@ #define MOVEIT_VERSION_GE(major, minor, patch) \ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ major * 1'000'000 + minor * 1'000 + patch) - -#if !MOVEIT_VERSION_GE(3, 0, 0) -// the pointers are not yet available -namespace trajectory_processing { -MOVEIT_CLASS_FORWARD(TimeParameterization); -} -#endif diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index c5f7cfaf..41473e36 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -66,8 +66,8 @@ public: const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; }; } // namespace solvers diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 9fa94113..4e080c56 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -63,8 +63,8 @@ public: const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; }; } // namespace solvers diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 412d0efa..68f10cd8 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -90,8 +90,8 @@ public: const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; protected: diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index f6764932..93ef7772 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -84,9 +84,10 @@ public: robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; - /// plan trajectory from current robot state to Cartesian target + /// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; }; diff --git a/core/include/moveit/task_constructor/stages/fixed_state.h b/core/include/moveit/task_constructor/stages/fixed_state.h index 2ace2726..adb3bb1c 100644 --- a/core/include/moveit/task_constructor/stages/fixed_state.h +++ b/core/include/moveit/task_constructor/stages/fixed_state.h @@ -48,7 +48,7 @@ namespace stages { class FixedState : public Generator { public: - FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr); + FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr); void setState(const planning_scene::PlanningScenePtr& scene); void reset() override; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 992cdc10..2f4429bc 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -249,7 +249,7 @@ private: std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); std::ostream& operator<<(std::ostream& os, const Interface& interface); -std::ostream& operator<<(std::ostream& os, Interface::Direction); +std::ostream& operator<<(std::ostream& os, Interface::Direction dir); /// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found. template diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 70b89482..fcc6f591 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -140,9 +140,6 @@ private: Int i; }; -const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, - std::string frame); - bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); diff --git a/core/src/container.cpp b/core/src/container.cpp index 2a82264c..13bc3738 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -1051,7 +1050,7 @@ void FallbacksPrivateConnect::compute() { void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { // expect failure to be reported from active child - assert(active_ != children().end() && active_->get() == &child); + assert(active_ != children().end() && active_->get() == &child); (void)child; // ... thus we can use std::next(active_) to find the next child auto next = std::next(active_); diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 4ddcecfc..cafc9b28 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -36,7 +36,6 @@ #include #include -#include #include #include diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 8d7b75ff..02be9a75 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -37,8 +37,6 @@ */ #include -#include - #include #include #include @@ -76,11 +74,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, } // reach pose of forward kinematics - return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints); + return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, + timeout, result, path_constraints); } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { const auto& props = properties(); @@ -103,7 +103,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons moveit::core::MaxEEFStep(props.get("step_size")), moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, props.get("kinematics_options"), - props.get("kinematics_cost_fn")); + props.get("kinematics_cost_fn"), offset); assert(!trajectory.empty()); // there should be at least the start state result = std::make_shared(sandbox_scene->getRobotModel(), jmg); diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index fb017431..7b3186a6 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -37,7 +37,6 @@ */ #include -#include #include #include @@ -102,9 +101,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, + const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { const auto start_time = std::chrono::steady_clock::now(); @@ -120,7 +119,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return to->isStateValid(*robot_state, constraints, jmg->getName()); } }; - if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { + if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) { // TODO(v4hn): planners need a way to add feedback to failing plans // in case of an invalid solution feedback should include unwanted collisions or violated constraints RCLCPP_WARN(LOGGER, "IK failed for pose target"); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 36125410..408b1597 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -185,8 +185,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { const auto& props = properties(); moveit_msgs::msg::MotionPlanRequest req; @@ -194,7 +195,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co geometry_msgs::msg::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); - target.pose = tf2::toMsg(target_eigen); + target.pose = tf2::toMsg(target_eigen * offset.inverse()); req.goal_constraints.resize(1); req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index bb2a49e7..617fb476 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -37,7 +37,6 @@ */ #include -#include #include using namespace trajectory_processing; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 151ddb11..8688dff4 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include @@ -126,8 +125,8 @@ StagePrivate& StagePrivate::operator=(StagePrivate&& other) { prev_ends_ = std::move(other.prev_ends_); next_starts_ = std::move(other.next_starts_); - parent_ = std::move(other.parent_); - it_ = std::move(other.it_); + parent_ = other.parent_; + it_ = other.it_; other.unparent(); return *this; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8fbd60e6..49a45c4e 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -90,7 +89,15 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& } // found IK solutions -using IKSolutions = std::vector>; + +struct IKSolution +{ + std::vector joint_positions; + bool feasible; + collision_detection::Contact contact; +}; + +using IKSolutions = std::vector; namespace { @@ -358,14 +365,23 @@ void ComputeIK::compute() { &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { - if (jmg->distance(joint_positions, sol.data()) < min_solution_distance) + if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance) return false; // too close to already found solution } state->setJointGroupPositions(jmg, joint_positions); ik_solutions.emplace_back(); - state->copyJointGroupPositions(jmg, ik_solutions.back()); - - return ignore_collisions || !scene->isStateColliding(*state, jmg->getName()); + auto& solution{ ik_solutions.back() }; + state->copyJointGroupPositions(jmg, solution.joint_positions); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.contacts = true; + req.max_contacts = 1; + scene->checkCollision(req, res, *state); + solution.feasible = ignore_collisions || !res.collision; + if (!res.contacts.empty()) { + solution.contact = res.contacts.begin()->second.front(); + } + return solution.feasible; }; uint32_t max_ik_solutions = props.get("max_ik_solutions"); @@ -393,15 +409,18 @@ void ComputeIK::compute() { solution.setComment(s.comment()); std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); - if (succeeded && i + 1 == ik_solutions.size()) + if (ik_solutions[i].feasible) // compute cost as distance to compare_pose - solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); - else // found an IK solution, but this was not valid - solution.markAsFailure(); - + solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data())); + else { // found an IK solution, but this was not valid + std::stringstream ss; + ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '" + << ik_solutions[i].contact.body_name_2 << "'"; + solution.markAsFailure(ss.str()); + } // set scene's robot state moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); - solution_state.setJointGroupPositions(jmg, ik_solutions[i].data()); + solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data()); solution_state.update(); InterfaceState state(solution_scene); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 8019dc55..e013a180 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -38,7 +38,6 @@ #include #include -#include #include #include diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index bfbff1fb..840b700b 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -38,7 +38,6 @@ #include #include -#include #include #include diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 9c4d99b0..0c8ebaad 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,7 @@ namespace task_constructor { namespace stages { FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) - : Generator(name), scene_(scene) { + : Generator(name), scene_(std::move(scene)) { properties().declare("ignore_collisions", false); setCostTerm(std::make_unique(0.0)); } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 8dd66edd..1b214870 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -110,8 +110,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c return false; } +// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance static void visualizePlan(std::deque& markers, Interface::Direction dir, bool success, - const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose, + const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) { double linear_norm = linear.norm(); @@ -120,7 +121,7 @@ static void visualizePlan(std::deque& markers, auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); // link position before planning; reached link position after planning; target link position - Eigen::Vector3d pos_link = link_pose.translation(); + Eigen::Vector3d pos_start = start_pose.translation(); Eigen::Vector3d pos_reached = reached_pose.translation(); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); @@ -130,7 +131,7 @@ static void visualizePlan(std::deque& markers, if (dir == Interface::FORWARD) { if (success) { // valid part: green arrow - rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); } else { @@ -145,14 +146,14 @@ static void visualizePlan(std::deque& markers, rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); // position half-way between pos_link and pos_reached - m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) }); + m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) }); m.pose.orientation = tf2::toMsg(quat_cylinder); markers.push_back(m); } } else { // valid part: green arrow // head length according to above comment - rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_reached, pos_start, 0.1 * linear_norm, 0.23 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); if (!success) { @@ -209,10 +210,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning Eigen::Vector3d linear; // linear translation Eigen::Vector3d angular; // angular rotation double linear_norm = 0.0, angular_norm = 0.0; - Eigen::Isometry3d target_eigen; - Eigen::Isometry3d link_pose = - scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success try { // try to extract Twist const geometry_msgs::msg::TwistStamped& target = boost::any_cast(direction); @@ -246,13 +244,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning angular *= -1.0; } - // compute absolute transform for link + // compute target transform for ik_frame applying motion transform of twist + // linear+angular are expressed w.r.t. model frame and thus we need left-multiplication linear = frame_pose.linear() * linear; angular = frame_pose.linear() * angular; - target_eigen = ik_pose_world; - target_eigen.linear() = - target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); - target_eigen.translation() += linear; + auto R = Eigen::AngleAxisd(angular_norm, angular); + auto p = ik_pose_world.translation(); + target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world); goto COMPUTE; } catch (const boost::bad_any_cast&) { /* continue with Vector */ } @@ -274,31 +272,31 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (dir == Interface::BACKWARD) linear *= -1.0; - // compute absolute transform for link + // compute target transform for ik_frame applying delta transform of twist linear = frame_pose.linear() * linear; - target_eigen = ik_pose_world; - target_eigen.translation() += linear; + target_eigen = Eigen::Translation3d(linear) * ik_pose_world; } catch (const boost::bad_any_cast&) { solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); return false; } COMPUTE: - // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); + // offset from link to ik_frame + const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + success = + planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); - const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link); + const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; double distance = 0.0; if (use_rotation_distance) { - Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); + Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose()); distance = rotation.angle(); } else - distance = (reached_pose.translation() - link_pose.translation()).norm(); + distance = (reached_pose.translation() - ik_pose_world.translation()).norm(); // min_distance reached? if (min_distance > 0.0) { @@ -316,8 +314,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // visualize plan auto ns = props.get("marker_ns"); if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance - visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear, - distance); + visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose, + linear, distance); } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 6d4027c9..e7257d44 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -41,7 +41,6 @@ #include #include -#include #include #include @@ -237,11 +236,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP add_frame(target, "target frame"); add_frame(ik_pose_world, "ik frame"); - // transform target pose such that ik frame will reach there if link does - target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); + // offset from link to ik_frame + Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // plan to Cartesian target - success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints); + success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); } // store result diff --git a/core/src/task.cpp b/core/src/task.cpp index 5290c34c..eda92741 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -249,7 +249,7 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { if (impl->preempt_requested_) return success_or(moveit::core::MoveItErrorCode::PREEMPTED); - if (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() > available_time) + if (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() >= available_time) return success_or(moveit::core::MoveItErrorCode::TIMED_OUT); compute(); for (const auto& cb : impl->task_cbs_) diff --git a/core/src/utils.cpp b/core/src/utils.cpp index d1d51a89..8cfc480e 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -44,7 +44,6 @@ #include #include -#include #include #include @@ -52,11 +51,6 @@ namespace moveit { namespace task_constructor { namespace utils { -const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, - std::string frame) { - return state.getRigidlyConnectedParentLinkModel(frame); -} - bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { @@ -79,23 +73,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); } else { auto ik_pose_msg = boost::any_cast(property.value()); - if (ik_pose_msg.header.frame_id.empty()) { - if (!(robot_link = get_tip())) { - solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); - return false; - } - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame; - } else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) { - robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id); - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; - } else { + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + + robot_link = nullptr; + bool found = false; + auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found); + if (!found && !ik_pose_msg.header.frame_id.empty()) { std::stringstream ss; ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'"; solution.markAsFailure(ss.str()); return false; } + if (!robot_link) + robot_link = get_tip(); + if (!robot_link) { + solution.markAsFailure("ik_frame doesn't specify a link frame"); + return false; + } else if (!found) { // use robot link's frame as reference by default + ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); + } + + tip_in_global_frame = ref_frame * tip_in_global_frame; } return true; diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 660b8a46..5a36457f 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -4,9 +4,9 @@ #include #include #include -#include #include +#include #include #include @@ -18,7 +18,7 @@ using namespace planning_scene; using namespace moveit::core; constexpr double TAU{ 2 * M_PI }; -constexpr double EPS{ 1e-6 }; +constexpr double EPS{ 5e-5 }; // provide a basic test fixture that prepares a Task struct PandaMoveRelative : public testing::Test @@ -68,10 +68,24 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string return aco; } -inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) { - return scene->getFrameTransform(frame).translation(); +void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip, + const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) { + const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast(solution)->trajectory(); + const Eigen::Vector3d start_position = (t.getFirstWayPoint().getFrameTransform(tip) * offset).translation(); + for (size_t i = 0; i < t.getWayPointCount(); ++i) { + const Eigen::Vector3d position = (t.getWayPoint(i).getFrameTransform(tip) * offset).translation(); + ASSERT_TRUE(start_position.isApprox(position, EPS)) + << "Rotation must maintain position\n" + << i << ": " << start_position.transpose() << " != " << position.transpose(); + } } +#define EXPECT_CONST_POSITION(...) \ + { \ + SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \ + expect_const_position(__VA_ARGS__); \ + } + TEST_F(PandaMoveRelative, cartesianRotateEEF) { move->setDirection([] { geometry_msgs::msg::TwistStamped twist; @@ -81,21 +95,28 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) { }()); ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName()); +} - const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() }; - const auto start_eef_position{ position(scene, tip_name) }; - const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) }; +TEST_F(PandaMoveRelative, cartesianCircular) { + const std::string tip = "panda_hand"; + auto offset = Eigen::Translation3d(0, 0, 0.1); + move->setIKFrame(offset, tip); + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.x = TAU / 4.0; + return twist; + }()); - EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) - << "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n" - << start_eef_position << "\nvs\n" - << end_eef_position; + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset)); } TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); - move->setIKFrame(ATTACHED_OBJECT); + const std::string attached_object{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); + move->setIKFrame(attached_object); move->setDirection([] { geometry_msgs::msg::TwistStamped twist; @@ -104,15 +125,8 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { return twist; }()); - ASSERT_TRUE(t.plan()); - - const auto start_eef_position{ position(scene, ATTACHED_OBJECT) }; - const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) }; - - EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) - << "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n" - << start_eef_position << "\nvs\n" - << end_eef_position; + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), attached_object); } int main(int argc, char** argv) { diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 92eb1772..807bcb9e 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include @@ -78,7 +77,8 @@ TEST_F(PandaMoveTo, stateTarget) { EXPECT_ONE_SOLUTION; } -geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { +geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, const std::string& pose, + const std::string& frame) { state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose); auto frame_eigen{ state.getFrameTransform(frame) }; geometry_msgs::msg::PoseStamped p; @@ -104,9 +104,9 @@ TEST_F(PandaMoveTo, poseTarget) { } TEST_F(PandaMoveTo, poseIKFrameLinkTarget) { - const std::string IK_FRAME{ "panda_hand" }; - move_to->setIKFrame(IK_FRAME); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + const std::string ik_frame{ "panda_hand" }; + move_to->setIKFrame(ik_frame); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame)); EXPECT_ONE_SOLUTION; } @@ -123,10 +123,8 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string p.dimensions[p.SPHERE_RADIUS] = 0.01; return p; }()); - aco.object.primitive_poses.resize(1); aco.object.pose.position.z = 0.2; aco.object.pose.orientation.w = 1.0; - // If we don't have this, we also don't have subframe support aco.object.subframe_names.resize(1, "subframe"); aco.object.subframe_poses.resize(1, [] { geometry_msgs::msg::Pose p; @@ -137,23 +135,22 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string } TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + const std::string attached_object{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); - move_to->setIKFrame(ATTACHED_OBJECT); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT)); + move_to->setIKFrame(attached_object); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", attached_object)); EXPECT_ONE_SOLUTION; } -// If we don't have this, we also don't have subframe support TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; + const std::string attached_object{ "attached_object" }; + const std::string ik_frame{ attached_object + "/subframe" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); - move_to->setIKFrame(IK_FRAME); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + move_to->setIKFrame(ik_frame); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame)); EXPECT_ONE_SOLUTION; } diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 976219be..56b8ba77 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -21,7 +21,7 @@ struct StandaloneGeneratorMockup : public GeneratorMockup InterfacePtr next; StandaloneGeneratorMockup(std::initializer_list&& costs) - : StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {} + : StandaloneGeneratorMockup{ PredefinedCosts{ costs, true } } {} StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true }) : GeneratorMockup{ std::move(costs) } { diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index bf27da69..7bcdccef 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -3,8 +3,6 @@ #include #include -#include - #include #include #include diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 5d607944..3c3af2a1 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -149,7 +149,7 @@ void PropertyFactory::addRemainingProperties(rviz_common::properties::Property* } #ifndef HAVE_YAML -rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, +rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, const std::string& description, const std::string& value, rviz_common::properties::Property* old) { diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 34910a82..50948b23 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -176,7 +176,7 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) { tool_buttons_group = new QButtonGroup(panel); tool_buttons_group->setExclusive(true); button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); - button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages")); + button_show_stage_dock_widget->setToolTip("Show available stages"); property_root = new rviz_common::properties::Property("Global Settings"); } diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 53f625ad..624e9c75 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -44,6 +44,7 @@ #include #include #include +#include using namespace moveit::task_constructor; @@ -224,12 +225,19 @@ TEST_F(TaskListModelTest, deletion) { // process deleteLater() events QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete); // as m is owned by model, m should be destroyed - // EXPECT_EQ(num_deletes, 1); // TODO: event is not processed, missing event loop? + EXPECT_EQ(num_deletes, 1); EXPECT_EQ(model.rowCount(), 0); } int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - return RUN_ALL_TESTS(); + QCoreApplication app(argc, argv); + // https://bugs.llvm.org/show_bug.cgi?id=40367 + // NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks) + QTimer::singleShot(0, [&]() { + ::testing::InitGoogleTest(&argc, argv); + auto testResult = RUN_ALL_TESTS(); + app.exit(testResult); + }); + return app.exec(); } diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp index a8453d8a..343942a8 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp @@ -467,6 +467,7 @@ std::pair FlatMergeProxyModel::getModel(const const QModelIndex& src_index = d_ptr->mapToSource(index, data); Q_ASSERT(data); + // NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker) return std::make_pair(data->model_, src_index); } diff --git a/visualization/motion_planning_tasks/utils/icon.cpp b/visualization/motion_planning_tasks/utils/icon.cpp index 2296d720..0ee0db95 100644 --- a/visualization/motion_planning_tasks/utils/icon.cpp +++ b/visualization/motion_planning_tasks/utils/icon.cpp @@ -55,7 +55,7 @@ static QPixmap maskToColorAndAlpha(const QPixmap& mask, const QColor& color) { using MaskAndColor = QPair; using MasksAndColors = QList; -static MasksAndColors masksAndColors(const Icon& icon, int dpr) { +static MasksAndColors masksAndColors(const Icon& icon, int /*dpr*/) { MasksAndColors result; for (const IconMaskAndColor& i : icon) { const QString& file_name = i.first; diff --git a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp index f57023ed..75e1ad30 100644 --- a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp @@ -440,6 +440,7 @@ std::pair TreeMergeProxyModel::getModel(const const QModelIndex& src_index = d_ptr->mapToSource(index, data); Q_ASSERT(data); + // NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker) return std::make_pair(data->model_, src_index); }