Merge branch master into ros2

This commit is contained in:
Robert Haschke 2022-11-02 19:16:38 +01:00
commit 9924dfe25f
35 changed files with 180 additions and 159 deletions

View File

@ -1,5 +1,7 @@
--- ---
Checks: 'performance-*, Checks: 'performance-*,
-performance-noexcept-move-constructor,
-clang-analyzer-core.CallAndMessage,
llvm-namespace-comment, llvm-namespace-comment,
modernize-redundant-void-arg, modernize-redundant-void-arg,
modernize-use-nullptr, modernize-use-nullptr,
@ -12,7 +14,6 @@ Checks: 'performance-*,
readability-redundant-string-cstr, readability-redundant-string-cstr,
readability-simplify-boolean-expr, readability-simplify-boolean-expr,
readability-container-size-empty, readability-container-size-empty,
readability-identifier-naming,
' '
HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h' HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h'
AnalyzeTemporaryDtors: false AnalyzeTemporaryDtors: false
@ -25,14 +26,14 @@ CheckOptions:
value: '2' value: '2'
# type names # type names
- key: readability-identifier-naming.ClassCase - key: readability-identifier-naming.ClassCase
value: aNy_CasE # CamelCase value: CamelCase
- key: readability-identifier-naming.EnumCase - key: readability-identifier-naming.EnumCase
value: CamelCase value: CamelCase
- key: readability-identifier-naming.UnionCase - key: readability-identifier-naming.UnionCase
value: CamelCase value: CamelCase
# method names # method names
- key: readability-identifier-naming.MethodCase - key: readability-identifier-naming.MethodCase
value: aNy_CasE # camelBack value: camelBack
# variable names # variable names
- key: readability-identifier-naming.VariableCase - key: readability-identifier-naming.VariableCase
value: lower_case value: lower_case

View File

@ -24,7 +24,7 @@ jobs:
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith
- IMAGE: rolling-source - IMAGE: rolling-source
CXX: clang++ CXX: clang++
CLANG_TIDY: true CLANG_TIDY: pedantic
CXXFLAGS: >- CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith
-Wno-deprecated-copy -Wno-deprecated-copy
@ -56,16 +56,18 @@ jobs:
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}" name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
runs-on: ubuntu-latest runs-on: ubuntu-latest
steps: steps:
- uses: actions/checkout@v2 - uses: actions/checkout@v3
- name: Cache ccache - name: Cache ccache
uses: pat-s/always-upload-cache@v2.1.5 uses: rhaschke/cache@main
with: with:
path: ${{ env.CCACHE_DIR }} path: ${{ env.CCACHE_DIR }}
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
restore-keys: | restore-keys: |
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
ccache-${{ env.CACHE_PREFIX }} ccache-${{ env.CACHE_PREFIX }}
env:
GHA_CACHE_SAVE: always
- id: ici - id: ici
name: Run industrial_ci name: Run industrial_ci
@ -73,7 +75,7 @@ jobs:
env: ${{ matrix.env }} env: ${{ matrix.env }}
- name: Upload test artifacts (on failure) - 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) if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
with: with:
name: test-results-${{ matrix.env.IMAGE }} name: test-results-${{ matrix.env.IMAGE }}
@ -86,7 +88,7 @@ jobs:
workdir: ${{ env.BASEDIR }}/target_ws workdir: ${{ env.BASEDIR }}/target_ws
ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
- name: Upload codecov report - 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' if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
with: with:
files: ${{ env.BASEDIR }}/target_ws/coverage.info files: ${{ env.BASEDIR }}/target_ws/coverage.info

View File

@ -44,10 +44,3 @@
#define MOVEIT_VERSION_GE(major, minor, patch) \ #define MOVEIT_VERSION_GE(major, minor, patch) \
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
major * 1'000'000 + minor * 1'000 + 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

View File

@ -66,8 +66,8 @@ public:
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, 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,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
}; };
} // namespace solvers } // namespace solvers

View File

@ -63,8 +63,8 @@ public:
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
}; };
} // namespace solvers } // namespace solvers

View File

@ -90,8 +90,8 @@ public:
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
protected: protected:

View File

@ -84,9 +84,10 @@ public:
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; 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, 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, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
}; };

View File

@ -48,7 +48,7 @@ namespace stages {
class FixedState : public Generator class FixedState : public Generator
{ {
public: 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 setState(const planning_scene::PlanningScenePtr& scene);
void reset() override; void reset() override;

View File

@ -249,7 +249,7 @@ private:
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); 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, 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. /// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
template <typename T> template <typename T>

View File

@ -140,9 +140,6 @@ private:
Int i; 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, bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);

View File

@ -37,7 +37,6 @@
#include <moveit/task_constructor/container_p.h> #include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h> #include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/merge.h> #include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h> #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
@ -1051,7 +1050,7 @@ void FallbacksPrivateConnect::compute() {
void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
// expect failure to be reported from active child // 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 // ... thus we can use std::next(active_) to find the next child
auto next = std::next(active_); auto next = std::next(active_);

View File

@ -36,7 +36,6 @@
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/collision_detection/collision_common.h> #include <moveit/collision_detection/collision_common.h>
#include <moveit/robot_trajectory/robot_trajectory.h> #include <moveit/robot_trajectory/robot_trajectory.h>

View File

@ -37,8 +37,6 @@
*/ */
#include <moveit/task_constructor/solvers/cartesian_path.h> #include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_parameterization.h> #include <moveit/trajectory_processing/time_parameterization.h>
#include <moveit/kinematics_base/kinematics_base.h> #include <moveit/kinematics_base/kinematics_base.h>
@ -76,11 +74,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
} }
// reach pose of forward kinematics // 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, 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, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties(); const auto& props = properties();
@ -103,7 +103,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
moveit::core::MaxEEFStep(props.get<double>("step_size")), moveit::core::MaxEEFStep(props.get<double>("step_size")),
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid, moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"), props.get<kinematics::KinematicsQueryOptions>("kinematics_options"),
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn")); props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);
assert(!trajectory.empty()); // there should be at least the start state assert(!trajectory.empty()); // there should be at least the start state
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg); result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);

View File

@ -37,7 +37,6 @@
*/ */
#include <moveit/task_constructor/solvers/joint_interpolation.h> #include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_parameterization.h> #include <moveit/trajectory_processing/time_parameterization.h>
@ -102,9 +101,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
} }
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const moveit::core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now(); 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()); 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 // 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 // in case of an invalid solution feedback should include unwanted collisions or violated constraints
RCLCPP_WARN(LOGGER, "IK failed for pose target"); RCLCPP_WARN(LOGGER, "IK failed for pose target");

View File

@ -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, bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties(); const auto& props = properties();
moveit_msgs::msg::MotionPlanRequest req; moveit_msgs::msg::MotionPlanRequest req;
@ -194,7 +195,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
geometry_msgs::msg::PoseStamped target; geometry_msgs::msg::PoseStamped target;
target.header.frame_id = from->getPlanningFrame(); 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.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(

View File

@ -37,7 +37,6 @@
*/ */
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h> #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
using namespace trajectory_processing; using namespace trajectory_processing;

View File

@ -38,7 +38,6 @@
#include <moveit/task_constructor/stage_p.h> #include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h> #include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h> #include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
@ -126,8 +125,8 @@ StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
prev_ends_ = std::move(other.prev_ends_); prev_ends_ = std::move(other.prev_ends_);
next_starts_ = std::move(other.next_starts_); next_starts_ = std::move(other.next_starts_);
parent_ = std::move(other.parent_); parent_ = other.parent_;
it_ = std::move(other.it_); it_ = other.it_;
other.unparent(); other.unparent();
return *this; return *this;

View File

@ -37,7 +37,6 @@
#include <moveit/task_constructor/stages/compute_ik.h> #include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h> #include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
@ -90,7 +89,15 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
} }
// found IK solutions // found IK solutions
using IKSolutions = std::vector<std::vector<double>>;
struct IKSolution
{
std::vector<double> joint_positions;
bool feasible;
collision_detection::Contact contact;
};
using IKSolutions = std::vector<IKSolution>;
namespace { namespace {
@ -358,14 +365,23 @@ void ComputeIK::compute() {
&ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
const double* joint_positions) { const double* joint_positions) {
for (const auto& sol : ik_solutions) { 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 return false; // too close to already found solution
} }
state->setJointGroupPositions(jmg, joint_positions); state->setJointGroupPositions(jmg, joint_positions);
ik_solutions.emplace_back(); ik_solutions.emplace_back();
state->copyJointGroupPositions(jmg, ik_solutions.back()); auto& solution{ ik_solutions.back() };
state->copyJointGroupPositions(jmg, solution.joint_positions);
return ignore_collisions || !scene->isStateColliding(*state, jmg->getName()); 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<uint32_t>("max_ik_solutions"); uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
@ -393,15 +409,18 @@ void ComputeIK::compute() {
solution.setComment(s.comment()); solution.setComment(s.comment());
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); 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 // compute cost as distance to compare_pose
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); 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 else { // found an IK solution, but this was not valid
solution.markAsFailure(); 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 // set scene's robot state
moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); 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(); solution_state.update();
InterfaceState state(solution_scene); InterfaceState state(solution_scene);

View File

@ -38,7 +38,6 @@
#include <moveit/task_constructor/stages/connect.h> #include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/merge.h> #include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>

View File

@ -38,7 +38,6 @@
#include <moveit/task_constructor/stages/fix_collision_objects.h> #include <moveit/task_constructor/stages/fix_collision_objects.h>
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>

View File

@ -44,7 +44,7 @@ namespace task_constructor {
namespace stages { namespace stages {
FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) 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); properties().declare("ignore_collisions", false);
setCostTerm(std::make_unique<cost::Constant>(0.0)); setCostTerm(std::make_unique<cost::Constant>(0.0));
} }

View File

@ -110,8 +110,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
return false; 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<visualization_msgs::msg::Marker>& markers, Interface::Direction dir, bool success, static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& 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) { const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
double linear_norm = linear.norm(); double linear_norm = linear.norm();
@ -120,7 +121,7 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); 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 // 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_reached = reached_pose.translation();
Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0);
@ -130,7 +131,7 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
if (dir == Interface::FORWARD) { if (dir == Interface::FORWARD) {
if (success) { if (success) {
// valid part: green arrow // 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); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
markers.push_back(m); markers.push_back(m);
} else { } else {
@ -145,14 +146,14 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance); rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance);
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
// position half-way between pos_link and pos_reached // 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); m.pose.orientation = tf2::toMsg(quat_cylinder);
markers.push_back(m); markers.push_back(m);
} }
} else { } else {
// valid part: green arrow // valid part: green arrow
// head length according to above comment // 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); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
markers.push_back(m); markers.push_back(m);
if (!success) { if (!success) {
@ -209,10 +210,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
Eigen::Vector3d linear; // linear translation Eigen::Vector3d linear; // linear translation
Eigen::Vector3d angular; // angular rotation Eigen::Vector3d angular; // angular rotation
double linear_norm = 0.0, angular_norm = 0.0; double linear_norm = 0.0, angular_norm = 0.0;
Eigen::Isometry3d target_eigen; 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 try { // try to extract Twist
const geometry_msgs::msg::TwistStamped& target = boost::any_cast<geometry_msgs::msg::TwistStamped>(direction); const geometry_msgs::msg::TwistStamped& target = boost::any_cast<geometry_msgs::msg::TwistStamped>(direction);
@ -246,13 +244,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
angular *= -1.0; 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; linear = frame_pose.linear() * linear;
angular = frame_pose.linear() * angular; angular = frame_pose.linear() * angular;
target_eigen = ik_pose_world; auto R = Eigen::AngleAxisd(angular_norm, angular);
target_eigen.linear() = auto p = ik_pose_world.translation();
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world);
target_eigen.translation() += linear;
goto COMPUTE; goto COMPUTE;
} catch (const boost::bad_any_cast&) { /* continue with Vector */ } 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) if (dir == Interface::BACKWARD)
linear *= -1.0; linear *= -1.0;
// compute absolute transform for link // compute target transform for ik_frame applying delta transform of twist
linear = frame_pose.linear() * linear; linear = frame_pose.linear() * linear;
target_eigen = ik_pose_world; target_eigen = Eigen::Translation3d(linear) * ik_pose_world;
target_eigen.translation() += linear;
} catch (const boost::bad_any_cast&) { } catch (const boost::bad_any_cast&) {
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
return false; return false;
} }
COMPUTE: COMPUTE:
// transform target pose such that ik frame will reach there if link does // offset from link to ik_frame
target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); 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(); moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms(); 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; double distance = 0.0;
if (use_rotation_distance) { 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(); distance = rotation.angle();
} else } else
distance = (reached_pose.translation() - link_pose.translation()).norm(); distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
// min_distance reached? // min_distance reached?
if (min_distance > 0.0) { if (min_distance > 0.0) {
@ -316,8 +314,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// visualize plan // visualize plan
auto ns = props.get<std::string>("marker_ns"); auto ns = props.get<std::string>("marker_ns");
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance 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, visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
distance); linear, distance);
} }
} }

View File

@ -41,7 +41,6 @@
#include <moveit/task_constructor/stages/move_to.h> #include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/utils.h> #include <moveit/task_constructor/utils.h>
#include <rviz_marker_tools/marker_creation.h> #include <rviz_marker_tools/marker_creation.h>
@ -237,11 +236,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
add_frame(target, "target frame"); add_frame(target, "target frame");
add_frame(ik_pose_world, "ik frame"); add_frame(ik_pose_world, "ik frame");
// transform target pose such that ik frame will reach there if link does // offset from link to ik_frame
target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
// plan to Cartesian target // 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 // store result

View File

@ -249,7 +249,7 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
if (impl->preempt_requested_) if (impl->preempt_requested_)
return success_or(moveit::core::MoveItErrorCode::PREEMPTED); return success_or(moveit::core::MoveItErrorCode::PREEMPTED);
if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() > available_time) if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() >= available_time)
return success_or(moveit::core::MoveItErrorCode::TIMED_OUT); return success_or(moveit::core::MoveItErrorCode::TIMED_OUT);
compute(); compute();
for (const auto& cb : impl->task_cbs_) for (const auto& cb : impl->task_cbs_)

View File

@ -44,7 +44,6 @@
#include <moveit/robot_state/robot_state.h> #include <moveit/robot_state/robot_state.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
@ -52,11 +51,6 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace utils { 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, bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { 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); tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
} else { } else {
auto ik_pose_msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(property.value()); auto ik_pose_msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(property.value());
if (ik_pose_msg.header.frame_id.empty()) { tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
if (!(robot_link = get_tip())) {
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); robot_link = nullptr;
return false; bool found = false;
} auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found);
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); if (!found && !ik_pose_msg.header.frame_id.empty()) {
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 {
std::stringstream ss; std::stringstream ss;
ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'"; ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'";
solution.markAsFailure(ss.str()); solution.markAsFailure(ss.str());
return false; 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; return true;

View File

@ -4,9 +4,9 @@
#include <moveit/task_constructor/stages/move_relative.h> #include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/fixed_state.h> #include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/cartesian_path.h> #include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <geometry_msgs/msg/pose_stamped.hpp> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp> #include <geometry_msgs/msg/twist_stamped.hpp>
@ -18,7 +18,7 @@ using namespace planning_scene;
using namespace moveit::core; using namespace moveit::core;
constexpr double TAU{ 2 * M_PI }; 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 // provide a basic test fixture that prepares a Task
struct PandaMoveRelative : public testing::Test struct PandaMoveRelative : public testing::Test
@ -68,10 +68,24 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
return aco; return aco;
} }
inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) { void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip,
return scene->getFrameTransform(frame).translation(); const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) {
const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast<const SubTrajectory>(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) { TEST_F(PandaMoveRelative, cartesianRotateEEF) {
move->setDirection([] { move->setDirection([] {
geometry_msgs::msg::TwistStamped twist; geometry_msgs::msg::TwistStamped twist;
@ -81,21 +95,28 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) {
}()); }());
ASSERT_TRUE(t.plan()) << "Failed to plan"; ASSERT_TRUE(t.plan()) << "Failed to plan";
EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName());
}
const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() }; TEST_F(PandaMoveRelative, cartesianCircular) {
const auto start_eef_position{ position(scene, tip_name) }; const std::string tip = "panda_hand";
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) }; 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)) ASSERT_TRUE(t.plan()) << "Failed to plan";
<< "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n" EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset));
<< start_eef_position << "\nvs\n"
<< end_eef_position;
} }
TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
const std::string ATTACHED_OBJECT{ "attached_object" }; const std::string attached_object{ "attached_object" };
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object));
move->setIKFrame(ATTACHED_OBJECT); move->setIKFrame(attached_object);
move->setDirection([] { move->setDirection([] {
geometry_msgs::msg::TwistStamped twist; geometry_msgs::msg::TwistStamped twist;
@ -104,15 +125,8 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
return twist; return twist;
}()); }());
ASSERT_TRUE(t.plan()); ASSERT_TRUE(t.plan()) << "Failed to plan";
EXPECT_CONST_POSITION(move->solutions().front(), attached_object);
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;
} }
int main(int argc, char** argv) { int main(int argc, char** argv) {

View File

@ -5,7 +5,6 @@
#include <moveit/task_constructor/stages/move_to.h> #include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/fixed_state.h> #include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h> #include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
@ -78,7 +77,8 @@ TEST_F(PandaMoveTo, stateTarget) {
EXPECT_ONE_SOLUTION; 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); state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose);
auto frame_eigen{ state.getFrameTransform(frame) }; auto frame_eigen{ state.getFrameTransform(frame) };
geometry_msgs::msg::PoseStamped p; geometry_msgs::msg::PoseStamped p;
@ -104,9 +104,9 @@ TEST_F(PandaMoveTo, poseTarget) {
} }
TEST_F(PandaMoveTo, poseIKFrameLinkTarget) { TEST_F(PandaMoveTo, poseIKFrameLinkTarget) {
const std::string IK_FRAME{ "panda_hand" }; const std::string ik_frame{ "panda_hand" };
move_to->setIKFrame(IK_FRAME); move_to->setIKFrame(ik_frame);
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame));
EXPECT_ONE_SOLUTION; EXPECT_ONE_SOLUTION;
} }
@ -123,10 +123,8 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
p.dimensions[p.SPHERE_RADIUS] = 0.01; p.dimensions[p.SPHERE_RADIUS] = 0.01;
return p; return p;
}()); }());
aco.object.primitive_poses.resize(1);
aco.object.pose.position.z = 0.2; aco.object.pose.position.z = 0.2;
aco.object.pose.orientation.w = 1.0; 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_names.resize(1, "subframe");
aco.object.subframe_poses.resize(1, [] { aco.object.subframe_poses.resize(1, [] {
geometry_msgs::msg::Pose p; geometry_msgs::msg::Pose p;
@ -137,23 +135,22 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
} }
TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) { TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
const std::string ATTACHED_OBJECT{ "attached_object" }; const std::string attached_object{ "attached_object" };
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object));
move_to->setIKFrame(ATTACHED_OBJECT); move_to->setIKFrame(attached_object);
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT)); move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", attached_object));
EXPECT_ONE_SOLUTION; EXPECT_ONE_SOLUTION;
} }
// If we don't have this, we also don't have subframe support
TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
const std::string ATTACHED_OBJECT{ "attached_object" }; const std::string attached_object{ "attached_object" };
const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; 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->setIKFrame(ik_frame);
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame));
EXPECT_ONE_SOLUTION; EXPECT_ONE_SOLUTION;
} }

View File

@ -21,7 +21,7 @@ struct StandaloneGeneratorMockup : public GeneratorMockup
InterfacePtr next; InterfacePtr next;
StandaloneGeneratorMockup(std::initializer_list<double>&& costs) StandaloneGeneratorMockup(std::initializer_list<double>&& costs)
: StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {} : StandaloneGeneratorMockup{ PredefinedCosts{ costs, true } } {}
StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true }) StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true })
: GeneratorMockup{ std::move(costs) } { : GeneratorMockup{ std::move(costs) } {

View File

@ -3,8 +3,6 @@
#include <moveit/robot_model/robot_model.h> #include <moveit/robot_model/robot_model.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/task.h> #include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/solvers/cartesian_path.h> #include <moveit/task_constructor/solvers/cartesian_path.h>

View File

@ -149,7 +149,7 @@ void PropertyFactory::addRemainingProperties(rviz_common::properties::Property*
} }
#ifndef HAVE_YAML #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& description,
const std::string& value, const std::string& value,
rviz_common::properties::Property* old) { rviz_common::properties::Property* old) {

View File

@ -176,7 +176,7 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) {
tool_buttons_group = new QButtonGroup(panel); tool_buttons_group = new QButtonGroup(panel);
tool_buttons_group->setExclusive(true); tool_buttons_group->setExclusive(true);
button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); 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"); property_root = new rviz_common::properties::Property("Global Settings");
} }

View File

@ -44,6 +44,7 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <initializer_list> #include <initializer_list>
#include <qcoreapplication.h> #include <qcoreapplication.h>
#include <qtimer.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
@ -224,12 +225,19 @@ TEST_F(TaskListModelTest, deletion) {
// process deleteLater() events // process deleteLater() events
QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete); QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete);
// as m is owned by model, m should be destroyed // 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); EXPECT_EQ(model.rowCount(), 0);
} }
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
rclcpp::init(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();
} }

View File

@ -467,6 +467,7 @@ std::pair<QAbstractItemModel*, QModelIndex> FlatMergeProxyModel::getModel(const
const QModelIndex& src_index = d_ptr->mapToSource(index, data); const QModelIndex& src_index = d_ptr->mapToSource(index, data);
Q_ASSERT(data); Q_ASSERT(data);
// NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker)
return std::make_pair(data->model_, src_index); return std::make_pair(data->model_, src_index);
} }

View File

@ -55,7 +55,7 @@ static QPixmap maskToColorAndAlpha(const QPixmap& mask, const QColor& color) {
using MaskAndColor = QPair<QPixmap, QColor>; using MaskAndColor = QPair<QPixmap, QColor>;
using MasksAndColors = QList<MaskAndColor>; using MasksAndColors = QList<MaskAndColor>;
static MasksAndColors masksAndColors(const Icon& icon, int dpr) { static MasksAndColors masksAndColors(const Icon& icon, int /*dpr*/) {
MasksAndColors result; MasksAndColors result;
for (const IconMaskAndColor& i : icon) { for (const IconMaskAndColor& i : icon) {
const QString& file_name = i.first; const QString& file_name = i.first;

View File

@ -440,6 +440,7 @@ std::pair<QAbstractItemModel*, QModelIndex> TreeMergeProxyModel::getModel(const
const QModelIndex& src_index = d_ptr->mapToSource(index, data); const QModelIndex& src_index = d_ptr->mapToSource(index, data);
Q_ASSERT(data); Q_ASSERT(data);
// NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker)
return std::make_pair(data->model_, src_index); return std::make_pair(data->model_, src_index);
} }