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-*,
-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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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:

View File

@ -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;
};

View File

@ -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;

View File

@ -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 <typename T>

View File

@ -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);

View File

@ -37,7 +37,6 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.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) {
// 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_);

View File

@ -36,7 +36,6 @@
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/collision_detection/collision_common.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/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_parameterization.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
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<double>("step_size")),
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
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
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/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.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,
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");

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,
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(

View File

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

View File

@ -38,7 +38,6 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
@ -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;

View File

@ -37,7 +37,6 @@
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
@ -90,7 +89,15 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
}
// 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 {
@ -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<uint32_t>("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);

View File

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

View File

@ -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<cost::Constant>(0.0));
}

View File

@ -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<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) {
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());
// 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<visualization_msgs::msg::Marker>& 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<visualization_msgs::msg::Marker>& 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<geometry_msgs::msg::TwistStamped>(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<std::string>("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);
}
}

View File

@ -41,7 +41,6 @@
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/utils.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(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

View File

@ -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<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);
compute();
for (const auto& cb : impl->task_cbs_)

View File

@ -44,7 +44,6 @@
#include <moveit/robot_state/robot_state.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/storage.h>
@ -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<geometry_msgs::msg::PoseStamped>(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;

View File

@ -4,9 +4,9 @@
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/fixed_state.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/robot_trajectory/robot_trajectory.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
@ -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<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) {
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) {

View File

@ -5,7 +5,6 @@
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
@ -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;
}

View File

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

View File

@ -3,8 +3,6 @@
#include <moveit/robot_model/robot_model.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/container.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
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) {

View File

@ -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");
}

View File

@ -44,6 +44,7 @@
#include <gtest/gtest.h>
#include <initializer_list>
#include <qcoreapplication.h>
#include <qtimer.h>
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();
}

View File

@ -467,6 +467,7 @@ std::pair<QAbstractItemModel*, QModelIndex> 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);
}

View File

@ -55,7 +55,7 @@ static QPixmap maskToColorAndAlpha(const QPixmap& mask, const QColor& color) {
using MaskAndColor = QPair<QPixmap, QColor>;
using MasksAndColors = QList<MaskAndColor>;
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;

View File

@ -440,6 +440,7 @@ std::pair<QAbstractItemModel*, QModelIndex> 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);
}