mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Compare commits
21 Commits
ros2-0.1.3
...
master
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
ef1cdeea86 | ||
|
|
377d09046f | ||
|
|
1655762a63 | ||
|
|
7384702448 | ||
|
|
ac4b92fff9 | ||
|
|
e9eab62f5f | ||
|
|
336ad4456c | ||
|
|
ed99e6b865 | ||
|
|
bf001bd093 | ||
|
|
87b3701223 | ||
|
|
86f0083566 | ||
|
|
f3659da82b | ||
|
|
24f22484ca | ||
|
|
717771e25a | ||
|
|
d19b77782d | ||
|
|
c6521551a4 | ||
|
|
580dac9151 | ||
|
|
5ec63045e8 | ||
|
|
0cc398797f | ||
|
|
5a0059dc83 | ||
|
|
a0da41a4aa |
10
.github/workflows/ci.yaml
vendored
10
.github/workflows/ci.yaml
vendored
@ -22,20 +22,21 @@ jobs:
|
||||
- IMAGE: noble-ci
|
||||
NAME: ccov
|
||||
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
|
||||
- IMAGE: noble-ci
|
||||
- IMAGE: noble-ci-testing
|
||||
CLANG_TIDY: true
|
||||
TARGET_CMAKE_ARGS: >-
|
||||
-DCMAKE_BUILD_TYPE=Release
|
||||
-DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wold-style-cast"
|
||||
- IMAGE: jammy-ci
|
||||
- IMAGE: noetic-source
|
||||
- IMAGE: noble-ci-testing
|
||||
NAME: asan
|
||||
DOCKER_RUN_OPTS: >-
|
||||
-e PRELOAD=libasan.so.5
|
||||
-e PRELOAD=libasan.so.8
|
||||
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
|
||||
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
|
||||
|
||||
env:
|
||||
PIP_BREAK_SYSTEM_PACKAGES: 1
|
||||
CATKIN_LINT: true
|
||||
CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
|
||||
DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
|
||||
@ -104,9 +105,10 @@ jobs:
|
||||
with:
|
||||
docker: $DOCKER_IMAGE
|
||||
workdir: ${{ env.BASEDIR }}/target_ws
|
||||
lcov_capture_args: --ignore-errors=gcov,gcov,mismatch,mismatch,negative,negative,source
|
||||
ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
|
||||
- name: Upload codecov report
|
||||
uses: codecov/codecov-action@v4
|
||||
uses: codecov/codecov-action@v5
|
||||
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
|
||||
with:
|
||||
files: ${{ env.BASEDIR }}/target_ws/coverage.info
|
||||
|
||||
2
.github/workflows/format.yaml
vendored
2
.github/workflows/format.yaml
vendored
@ -19,8 +19,6 @@ jobs:
|
||||
- name: Install clang-format-14
|
||||
run: sudo apt-get install clang-format-14
|
||||
- uses: rhaschke/install-catkin_lint-action@main
|
||||
with:
|
||||
distro: noetic
|
||||
- uses: pre-commit/action@v3.0.1
|
||||
id: precommit
|
||||
- name: Upload pre-commit changes
|
||||
|
||||
5
.gitmodules
vendored
5
.gitmodules
vendored
@ -1,8 +1,3 @@
|
||||
[submodule "core/python/pybind11"]
|
||||
path = core/python/pybind11
|
||||
url = https://github.com/pybind/pybind11
|
||||
branch = smart_holder
|
||||
shallow = true
|
||||
[submodule "core/src/scope_guard"]
|
||||
path = core/src/scope_guard
|
||||
url = https://github.com/ricab/scope_guard
|
||||
|
||||
@ -29,7 +29,7 @@ repos:
|
||||
- id: trailing-whitespace
|
||||
|
||||
- repo: https://github.com/psf/black
|
||||
rev: 24.10.0
|
||||
rev: 25.1.0
|
||||
hooks:
|
||||
- id: black
|
||||
args: ["--line-length", "100"]
|
||||
|
||||
@ -2,6 +2,21 @@
|
||||
Changelog for package moveit_task_constructor_capabilities
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.4 (2025-10-15)
|
||||
------------------
|
||||
* Add missing include fmt/ranges.h (`#712 <https://github.com/moveit/moveit_task_constructor/issues/712>`_)
|
||||
* Provide action feedback during task execution (`#653 <https://github.com/moveit/moveit_task_constructor/issues/653>`_)
|
||||
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
|
||||
* Silent error "Found empty JointState message"
|
||||
* Simplify formatting code with https://github.com/fmtlib (`#499 <https://github.com/moveit/moveit_task_constructor/issues/499>`_)
|
||||
* Drop Melodic support
|
||||
* Fix Solution::fillMessage() (`#432 <https://github.com/moveit/moveit_task_constructor/issues/432>`_)
|
||||
* Add property trajectory_execution_info (`#355 <https://github.com/moveit/moveit_task_constructor/issues/355>`_, `#502 <https://github.com/moveit/moveit_task_constructor/issues/502>`_)
|
||||
* ExecuteTaskSolutionCapability: Rename goalCallback() -> execCallback()
|
||||
* Replace namespace robot\_[model|state] with moveit::core
|
||||
* Use pluginlib consistently (`#463 <https://github.com/moveit/moveit_task_constructor/issues/463>`_)
|
||||
* Contributors: Dhruv Patel, Michael Görner, Robert Haschke
|
||||
|
||||
0.1.3 (2023-03-06)
|
||||
------------------
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<package format="2">
|
||||
<name>moveit_task_constructor_capabilities</name>
|
||||
<version>0.1.3</version>
|
||||
<version>0.1.4</version>
|
||||
<description>
|
||||
MoveGroupCapabilites to interact with MoveIt
|
||||
</description>
|
||||
|
||||
@ -44,6 +44,7 @@
|
||||
#include <moveit/utils/message_checks.h>
|
||||
#include <moveit/utils/moveit_error_code.h>
|
||||
#include <fmt/format.h>
|
||||
#include <fmt/ranges.h>
|
||||
|
||||
namespace {
|
||||
|
||||
|
||||
@ -2,6 +2,106 @@
|
||||
Changelog for package moveit_task_constructor_core
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.4 (2025-10-15)
|
||||
------------------
|
||||
* Avoid duplicate scenes in Solution.msg from generator stages (`#639 <https://github.com/moveit/moveit_task_constructor/issues/639>`_)
|
||||
* Allow max Cartesian link speed in PlannerInterface (`#277 <https://github.com/moveit/moveit_task_constructor/issues/277>`_)
|
||||
* Enable collisions visualizations (`#708 <https://github.com/moveit/moveit_task_constructor/issues/708>`_)
|
||||
* LimitSolutions wrapper stage (`#710 <https://github.com/moveit/moveit_task_constructor/issues/710>`_)
|
||||
* Improve code documentation
|
||||
* CI: Fix Noble builds
|
||||
* Pick with custom max_velocity_scaling_factor during approach+lift
|
||||
* Rework pybind11 ABI compatibility checks
|
||||
* Remove pybind11 submodule
|
||||
* Upgrade pybind11 to v3
|
||||
* Modernize declaration of compile options
|
||||
* Factor out Python property handling to allow for reuse in custom Python wrappers
|
||||
* Fix clamping of joint constraints (`#665 <https://github.com/moveit/moveit_task_constructor/issues/665>`_)
|
||||
* Correctly report failures instead of issueing console warnings
|
||||
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
|
||||
* Python API: Allow passing a task's introspection object to SolutionBase::toMsg()
|
||||
* clang-format-14
|
||||
* Add support for GenerateRandomPose
|
||||
* python: Add Task::setRobotModel
|
||||
* Add path_constraints property to Connect stage
|
||||
* provide a fmt wrapper (`#615 <https://github.com/moveit/moveit_task_constructor/issues/615>`_)
|
||||
* Update API: JumpThreshold -> CartesianPrecision (`#611 <https://github.com/moveit/moveit_task_constructor/issues/611>`_)
|
||||
* Reduce stop time due to preempt (`#598 <https://github.com/moveit/moveit_task_constructor/issues/598>`_)
|
||||
* Add unittest for `#581 <https://github.com/moveit/moveit_task_constructor/issues/581>`_
|
||||
* Fix early planning preemption (`#597 <https://github.com/moveit/moveit_task_constructor/issues/597>`_)
|
||||
* MoveRelative: fix segfault on empty trajectory (`#595 <https://github.com/moveit/moveit_task_constructor/issues/595>`_)
|
||||
* MoveRelative: handle equal min/max distance (`#593 <https://github.com/moveit/moveit_task_constructor/issues/593>`_)
|
||||
* Cleanup unit tests and allow them to run via both, cmdline and pytest
|
||||
* Connect: Relax validity check of reached end state
|
||||
* Unify Python demo scripts
|
||||
* Switch shebang to python3
|
||||
* Silence gcc's overloaded-virtual warnings
|
||||
* Add property to enable/disable pruning at runtime (`#590 <https://github.com/moveit/moveit_task_constructor/issues/590>`_)
|
||||
* Disable pruning by default
|
||||
* test_pruning.cpp: Add new test
|
||||
* test_pruning.cpp: Extend test to ParallelContainer
|
||||
* PassThrough: cleanup unused headers
|
||||
* Avoid segfault if TimeParameterization is not set
|
||||
* CartesianPath: allow ik_frame definition if start and end are given as joint-space poses
|
||||
* Generalize utils::getRobotTipForFrame() to return error_msg instead of calling markAsFailure() on a solution
|
||||
* ComputeIK: Allow additional constraints for filtering solutions (`#464 <https://github.com/moveit/moveit_task_constructor/issues/464>`_)
|
||||
* Expose MultiPlanner to Python (`#474 <https://github.com/moveit/moveit_task_constructor/issues/474>`_)
|
||||
* Add unittest cartesianCollisionMinMaxDistance (`#538 <https://github.com/moveit/moveit_task_constructor/issues/538>`_)
|
||||
* Simplify formatting code with https://github.com/fmtlib (`#499 <https://github.com/moveit/moveit_task_constructor/issues/499>`_)
|
||||
* Add NoOp stage (`#534 <https://github.com/moveit/moveit_task_constructor/issues/534>`_)
|
||||
* ModifyPlanningScene: check state for collisions
|
||||
* Improve TypeError exceptions
|
||||
* Drop Melodic support
|
||||
* Switch to package py_binding_tools
|
||||
* Add ability to move CollisionObjects (`#567 <https://github.com/moveit/moveit_task_constructor/issues/567>`_)
|
||||
* Improve description of max_distance property of Connect stage (`#564 <https://github.com/moveit/moveit_task_constructor/issues/564>`_)
|
||||
* Add Generator::spawn(from, to, trajectory) variant (`#546 <https://github.com/moveit/moveit_task_constructor/issues/546>`_)
|
||||
* Cosmetic fixes
|
||||
* Fix Solution::fillMessage() (`#432 <https://github.com/moveit/moveit_task_constructor/issues/432>`_)
|
||||
* Fix generation of Solution msg: consider backward operation
|
||||
* Propagate errors from planners to solution comment (`#525 <https://github.com/moveit/moveit_task_constructor/issues/525>`_)
|
||||
* JointInterpolationPlanner: Check joint bounds (`#505 <https://github.com/moveit/moveit_task_constructor/issues/505>`_)
|
||||
* Add property trajectory_execution_info (`#355 <https://github.com/moveit/moveit_task_constructor/issues/355>`_, `#502 <https://github.com/moveit/moveit_task_constructor/issues/502>`_)
|
||||
* Clear JointStates in scene diff (`#504 <https://github.com/moveit/moveit_task_constructor/issues/504>`_)
|
||||
* Set a non-infinite default timeout in CurrentState stage (`#491 <https://github.com/moveit/moveit_task_constructor/issues/491>`_)
|
||||
* Add GenerateRandomPose stage (`#166 <https://github.com/moveit/moveit_task_constructor/issues/166>`_)
|
||||
* GenerateGraspPose: Expose rotation_axis as property (`#535 <https://github.com/moveit/moveit_task_constructor/issues/535>`_)
|
||||
* Connect: ensure end-state matches goal state (`#532 <https://github.com/moveit/moveit_task_constructor/issues/532>`_)
|
||||
* Fix discontinuity in trajectory (`#485 <https://github.com/moveit/moveit_task_constructor/issues/485>`_)
|
||||
* Adaptions for https://github.com/ros-planning/moveit/pull/3534
|
||||
* Cleanup debug output
|
||||
* Fix duplicate solutions
|
||||
* printPendingPairs(os) -> os<<pendingPairsPrinter()
|
||||
* Fix leaking of failures into enumerated solutions
|
||||
* Add more debugging output
|
||||
* Unit tests for `#485 <https://github.com/moveit/moveit_task_constructor/issues/485>`_
|
||||
* DelayingWrapper stage to delay solution shipping in unit tests
|
||||
* Simplify tests
|
||||
* Skip Fallbacks::replaceImpl() when already correctly initialized (`#494 <https://github.com/moveit/moveit_task_constructor/issues/494>`_)
|
||||
* Fix demos (`#493 <https://github.com/moveit/moveit_task_constructor/issues/493>`_)
|
||||
* Limit time to wait for execute_task_solution action server
|
||||
* Replace namespace robot\_[model|state] with moveit::core
|
||||
* MPS: fixup processCollisionObject
|
||||
* Merge PR `#460 <https://github.com/moveit/moveit_task_constructor/issues/460>`_: improvements to ModifyPlanningScene stage
|
||||
* Gracefully handle NULL robot_trajectory (`#469 <https://github.com/moveit/moveit_task_constructor/issues/469>`_)
|
||||
* introspection: remove any invalid ROS-name chars from hostname (`#465 <https://github.com/moveit/moveit_task_constructor/issues/465>`_)
|
||||
* Fix SolutionBase::fillMessage(): also write start_scene
|
||||
* Fix add/remove object in backward operation
|
||||
* Add python binding for ModifyPlanningScene::removeObject
|
||||
* ComputeIK: update RobotState before calling setFromIK()
|
||||
* Use pluginlib consistently (`#463 <https://github.com/moveit/moveit_task_constructor/issues/463>`_)
|
||||
* Expose argument of PipelinePlanner's constructor to Python (`#462 <https://github.com/moveit/moveit_task_constructor/issues/462>`_)
|
||||
* Fix allowCollisions(object, enable_collision)
|
||||
* TestModifyPlanningScene
|
||||
* Basic Move test: MoveRelative + MoveTo
|
||||
* Add python binding for ModifyPlanningScene::allowCollisions(std::string, bool)
|
||||
* Add python binding for Task::insert
|
||||
* Add Stage::explainFailure() (`#445 <https://github.com/moveit/moveit_task_constructor/issues/445>`_)
|
||||
* Improve documentation (`#431 <https://github.com/moveit/moveit_task_constructor/issues/431>`_)
|
||||
* JointInterpolationPlanner: pass optional max_effort property along to GripperCommand (`#458 <https://github.com/moveit/moveit_task_constructor/issues/458>`_)
|
||||
* Task: findChild() and operator[] should directly operate on stages() (`#435 <https://github.com/moveit/moveit_task_constructor/issues/435>`_)
|
||||
* Contributors: Abishalini, Ali Haider, Captain Yoshi, Daniel García López, Gauthier Hentz, Henning Kayser, JafarAbdi, Michael Görner, Michael Wiznitzer, Paul Gesel, Robert Haschke, Sebastian Castro, Sebastian Jahr, VideoSystemsTech
|
||||
|
||||
0.1.3 (2023-03-06)
|
||||
------------------
|
||||
* MoveRelative: Allow backwards operation for joint-space delta (`#436 <https://github.com/ros-planning/moveit_task_constructor/issues/436>`_)
|
||||
|
||||
@ -27,6 +27,7 @@ catkin_package(
|
||||
${PROJECT_NAME}
|
||||
${PROJECT_NAME}_stages
|
||||
${PROJECT_NAME}_stage_plugins
|
||||
${PROJECT_NAME}_python_tools
|
||||
INCLUDE_DIRS
|
||||
include
|
||||
CATKIN_DEPENDS
|
||||
@ -35,10 +36,9 @@ catkin_package(
|
||||
moveit_task_constructor_msgs
|
||||
rviz_marker_tools
|
||||
visualization_msgs
|
||||
CFG_EXTRAS pybind11.cmake
|
||||
)
|
||||
|
||||
add_compile_options(-fvisibility-inlines-hidden)
|
||||
set(CMAKE_VISIBILITY_INLINES_HIDDEN TRUE)
|
||||
|
||||
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
|
||||
|
||||
|
||||
@ -1,9 +0,0 @@
|
||||
# pybind11 must use the ROS python version
|
||||
set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION})
|
||||
|
||||
if(@INSTALLSPACE@)
|
||||
include(${CMAKE_CURRENT_LIST_DIR}/pybind11Config.cmake)
|
||||
else()
|
||||
# in build space, directly include pybind11 directory
|
||||
add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../pybind11 ${CMAKE_CURRENT_BINARY_DIR}/pybind11)
|
||||
endif()
|
||||
@ -1,14 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <pybind11/smart_holder.h>
|
||||
#include <py_binding_tools/ros_msg_typecasters.h>
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <boost/any.hpp>
|
||||
#include <typeindex>
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap)
|
||||
|
||||
namespace moveit {
|
||||
namespace python {
|
||||
|
||||
@ -59,7 +55,7 @@ class class_ : public pybind11::classh<type_, options...> // NOLINT(readability
|
||||
|
||||
public:
|
||||
// forward all constructors
|
||||
using base_class_::classh;
|
||||
using base_class_::base_class_;
|
||||
|
||||
template <typename PropertyType, typename... Extra>
|
||||
class_& property(const char* name, const Extra&... extra) {
|
||||
|
||||
@ -89,6 +89,10 @@ public:
|
||||
void setTimeout(double timeout) { properties_.set("timeout", timeout); }
|
||||
void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
|
||||
void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); }
|
||||
|
||||
void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); }
|
||||
void setCartesianSpeedLimitedLink(const std::string& link) { setProperty("cartesian_speed_limited_link", link); }
|
||||
|
||||
void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
|
||||
properties_.set("time_parameterization", tp);
|
||||
}
|
||||
|
||||
@ -48,7 +48,9 @@
|
||||
#include "stages/generate_place_pose.h"
|
||||
#include "stages/generate_pose.h"
|
||||
#include "stages/generate_random_pose.h"
|
||||
#include "stages/limit_solutions.h"
|
||||
#include "stages/modify_planning_scene.h"
|
||||
#include "stages/move_relative.h"
|
||||
#include "stages/move_to.h"
|
||||
#include "stages/passthrough.h"
|
||||
#include "stages/predicate_filter.h"
|
||||
|
||||
@ -0,0 +1,65 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holders nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
/* Authors: Joseph Moore */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
/** A wrapper which lazily passes a limited number of solutions
|
||||
*
|
||||
* The best solution at each call to compute is passed on
|
||||
*
|
||||
*/
|
||||
class LimitSolutions : public WrapperBase
|
||||
{
|
||||
public:
|
||||
LimitSolutions(const std::string& name = "LimitSolutions", Stage::pointer&& child = Stage::pointer());
|
||||
|
||||
void reset() override;
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
|
||||
void setMaxSolutions(uint32_t max_solutions) { setProperty("max_solutions", max_solutions); }
|
||||
|
||||
private:
|
||||
ordered<const SolutionBase*> upstream_solutions_;
|
||||
uint32_t forwarded_solutions;
|
||||
};
|
||||
} // namespace stages
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
@ -335,7 +335,7 @@ private:
|
||||
// comment for this solution, e.g. explanation of failure
|
||||
std::string comment_;
|
||||
// markers for this solution, e.g. target frame or collision indicators
|
||||
std::deque<visualization_msgs::Marker> markers_;
|
||||
std::vector<visualization_msgs::Marker> markers_;
|
||||
|
||||
// begin and end InterfaceState of this solution/trajectory
|
||||
const InterfaceState* start_ = nullptr;
|
||||
|
||||
@ -45,6 +45,9 @@
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
#include <moveit/collision_detection/collision_common.h>
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
|
||||
namespace planning_scene {
|
||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||
@ -139,9 +142,23 @@ private:
|
||||
Int i;
|
||||
};
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
/** For a PoseStamped property, lookup the associated LinkModel* and yield the pose in global frame */
|
||||
bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, std::string& error_msg,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
|
||||
|
||||
/** Add sphere markers to visualize given collisions */
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers, const std::string& frame_id,
|
||||
const collision_detection::CollisionResult::ContactMap& contacts, double radius = 0.035);
|
||||
|
||||
/** Add sphere markers to visualize collisions within the trajectory */
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers,
|
||||
const robot_trajectory::RobotTrajectory& trajectory,
|
||||
const planning_scene::PlanningSceneConstPtr& planning_scene);
|
||||
|
||||
/** Returns true if the result provides hints that planning failed due to collisions */
|
||||
bool hints_at_collisions(const solvers::PlannerInterface::Result& result);
|
||||
|
||||
} // namespace utils
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<package format="3">
|
||||
<name>moveit_task_constructor_core</name>
|
||||
<version>0.1.3</version>
|
||||
<version>0.1.4</version>
|
||||
<description>MoveIt Task Pipeline</description>
|
||||
|
||||
<url type="website">https://github.com/moveit/moveit_task_constructor</url>
|
||||
|
||||
@ -1,31 +1,6 @@
|
||||
# We rely on pybind11's smart_holder branch imported pybind11 via git submodule
|
||||
|
||||
# pybind11 must use the ROS python version
|
||||
set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION})
|
||||
|
||||
# Use minimum-size optimization for pybind11 bindings
|
||||
add_compile_options("-Os")
|
||||
|
||||
# create symlink to grant access to downstream packages in devel space
|
||||
add_custom_target(pybind11_devel_symlink ALL COMMAND ${CMAKE_COMMAND} -E create_symlink
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/pybind11
|
||||
${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/pybind11)
|
||||
|
||||
# configure pybind11 install for use by downstream packages in install space
|
||||
set(PYBIND11_INSTALL ON CACHE INTERNAL "Install pybind11")
|
||||
set(CMAKE_INSTALL_INCLUDEDIR include/moveit/python)
|
||||
set(PYBIND11_CMAKECONFIG_INSTALL_DIR ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake
|
||||
CACHE INTERNAL "install path for pybind11 cmake files")
|
||||
|
||||
# source pybind11 folder, which exposes its targets and installs them
|
||||
if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/pybind11/CMakeLists.txt")
|
||||
message("Missing content of submodule pybind11: Use 'git clone --recurse-submodule' in future.\n"
|
||||
"Checking out content automatically")
|
||||
execute_process(COMMAND git submodule init WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
execute_process(COMMAND git submodule update WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
endif()
|
||||
#catkin_lint: ignore_once subproject duplicate_cmd
|
||||
add_subdirectory(pybind11)
|
||||
find_package(pybind11 3.0 REQUIRED)
|
||||
|
||||
# C++ wrapper code
|
||||
add_subdirectory(bindings)
|
||||
|
||||
@ -1,22 +1,28 @@
|
||||
set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor)
|
||||
add_library(${PROJECT_NAME}_python_tools SHARED
|
||||
${INCLUDES}/properties.h
|
||||
src/properties.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC ${PROJECT_NAME} pybind11::pybind11)
|
||||
# Use minimum-size optimization for pybind11 bindings
|
||||
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC pybind11::opt_size)
|
||||
|
||||
# catkin_lint cannot detect target declarations in functions, here in pybind11_add_module
|
||||
#catkin_lint: ignore undefined_target
|
||||
|
||||
# moveit.task_constructor
|
||||
set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor)
|
||||
pybind11_add_module(pymoveit_mtc
|
||||
${INCLUDES}/properties.h
|
||||
|
||||
src/properties.cpp
|
||||
src/solvers.cpp
|
||||
src/core.cpp
|
||||
src/stages.cpp
|
||||
src/module.cpp
|
||||
)
|
||||
target_include_directories(pymoveit_mtc PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>)
|
||||
target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${TOOLS_LIB_NAME})
|
||||
target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${PROJECT_NAME}_python_tools)
|
||||
set_target_properties(pymoveit_mtc PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION})
|
||||
|
||||
# install python libs
|
||||
# install libs
|
||||
install(TARGETS ${PROJECT_NAME}_python_tools
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
install(TARGETS pymoveit_mtc
|
||||
LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}
|
||||
)
|
||||
LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION})
|
||||
|
||||
@ -39,7 +39,7 @@
|
||||
#include <moveit/task_constructor/cost_queue.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <moveit/utils/moveit_error_code.h>
|
||||
#include <pybind11/smart_holder.h>
|
||||
#include <pybind11/pybind11.h>
|
||||
|
||||
/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */
|
||||
|
||||
@ -108,36 +108,3 @@ public:
|
||||
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::solvers::PlannerInterface)
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SolutionBase)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SubTrajectory)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(ordered<moveit::task_constructor::SolutionBaseConstPtr>)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::InterfaceState)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::core::MoveItErrorCode)
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::CostTerm)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::TrajectoryCostTerm)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::PathLength)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::DistanceToReference)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::TrajectoryDuration)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::LinkMotion)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::Clearance)
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Stage)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingEitherWay)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingForward)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingBackward)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Generator)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::MonitoringGenerator)
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ContainerBase)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SerialContainer)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ParallelContainerBase)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Alternatives)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Fallbacks)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Merger)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::WrapperBase)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Task)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Introspection)
|
||||
|
||||
@ -32,7 +32,7 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
#include <pybind11/smart_holder.h>
|
||||
#include <pybind11/pybind11.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace python {
|
||||
|
||||
@ -155,6 +155,7 @@ bool PropertyConverterBase::insert(const std::type_index& type_index, const std:
|
||||
return REGISTRY_SINGLETON.insert(type_index, ros_msg_name, to, from);
|
||||
}
|
||||
|
||||
__attribute__((visibility("default"))) // export this symbol as visible in the shared library
|
||||
void export_properties(py::module& m) {
|
||||
// clang-format off
|
||||
py::classh<Property>(m, "Property", "Holds an arbitrarily typed value and a default value")
|
||||
|
||||
@ -46,12 +46,6 @@ using namespace py::literals;
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace moveit::task_constructor::solvers;
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(PlannerInterface)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(PipelinePlanner)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(JointInterpolationPlanner)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(CartesianPath)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(MultiPlanner)
|
||||
|
||||
namespace moveit {
|
||||
namespace python {
|
||||
|
||||
@ -60,6 +54,8 @@ void export_solvers(py::module& m) {
|
||||
.property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
|
||||
.property<double>("max_acceleration_scaling_factor",
|
||||
"float: Reduce the maximum acceleration by scaling between (0,1]")
|
||||
.property<double>("max_cartesian_speed", "float: maximum cartesian speed")
|
||||
.property<std::string>("cartesian_speed_limited_link", "str: link with limited cartesian speed")
|
||||
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
|
||||
py::return_value_policy::reference_internal, "Properties of the planner");
|
||||
|
||||
|
||||
@ -37,6 +37,7 @@
|
||||
#include <moveit/task_constructor/stages.h>
|
||||
#include <moveit/task_constructor/stages/pick.h>
|
||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit_msgs/PlanningScene.h>
|
||||
#include <pybind11/stl.h>
|
||||
@ -47,24 +48,6 @@ using namespace py::literals;
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace moveit::task_constructor::stages;
|
||||
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(ModifyPlanningScene)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(CurrentState)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixedState)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(ComputeIK)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveTo)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveRelative)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePlacePose)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateRandomPose)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGraspBase)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp)
|
||||
|
||||
namespace moveit {
|
||||
namespace python {
|
||||
|
||||
@ -188,10 +171,6 @@ void export_stages(pybind11::module& m) {
|
||||
str: Default joint pose of the active group
|
||||
(defines cost of the inverse kinematics).
|
||||
)")
|
||||
.property<uint32_t>("max_ik_solutions", R"(
|
||||
int: Set the maximum number of inverse
|
||||
kinematic solutions thats should be generated.
|
||||
)")
|
||||
.property<uint32_t>("max_ik_solutions", "uint: max number of solutions to return")
|
||||
.property<bool>("ignore_collisions", R"(
|
||||
bool: Specify if collisions with other members of
|
||||
@ -452,6 +431,7 @@ void export_stages(pybind11::module& m) {
|
||||
.property<std::string>("eef_parent_group", "str: Joint model group of the eef's parent")
|
||||
.def(py::init<Stage::pointer&&, const std::string&>(), "grasp_generator"_a,
|
||||
"name"_a = std::string("pick"))
|
||||
.def_property_readonly("cartesian_solver", &Pick::cartesianSolver)
|
||||
.def("setApproachMotion", &Pick::setApproachMotion, R"(
|
||||
The approaching motion towards the grasping state is represented
|
||||
by a twist message.
|
||||
@ -564,6 +544,23 @@ void export_stages(pybind11::module& m) {
|
||||
.property<std::string>("grasp", "str: Name of the grasp pose")
|
||||
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
|
||||
"name"_a = std::string("ungrasp"));
|
||||
|
||||
properties::class_<PassThrough, Stage>(m, "PassThrough", R"(
|
||||
Wrapper which simply forwards all solutions of a child stage,
|
||||
allowing the wrapper to redefine costs, w/o loosing original costs.
|
||||
)")
|
||||
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
|
||||
|
||||
properties::class_<LimitSolutions, Stage>(m, "LimitSolutions", R"(
|
||||
Wrapper for any stage to limit the total number of solutions returned.
|
||||
|
||||
The wrapper stores solutions of its child stage, and on each compute will
|
||||
pass on the lowest cost solution available, until the maximum number of solutions
|
||||
is reached.
|
||||
)")
|
||||
.property<uint32_t>("max_solutions", "uint: maximum number of solutions that should be passed on")
|
||||
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
|
||||
|
||||
}
|
||||
} // namespace python
|
||||
} // namespace moveit
|
||||
|
||||
@ -1 +0,0 @@
|
||||
Subproject commit f4bc71f981d4eb2dd780215fd3c5a7420f1f03aa
|
||||
@ -16,19 +16,6 @@ def setUpModule():
|
||||
roscpp_init("test_mtc")
|
||||
|
||||
|
||||
def pybind11_versions():
|
||||
try:
|
||||
keys = __builtins__.keys() # for use with pytest
|
||||
except AttributeError:
|
||||
keys = __builtins__.__dict__.keys() # use from cmdline
|
||||
return [k for k in keys if k.startswith("__pybind11_internals_v")]
|
||||
|
||||
|
||||
incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions: " + "\n- ".join(
|
||||
pybind11_versions()
|
||||
)
|
||||
|
||||
|
||||
class PyGenerator(core.Generator):
|
||||
"""Implements a custom 'Generator' stage."""
|
||||
|
||||
@ -109,18 +96,20 @@ class TestTrampolines(unittest.TestCase):
|
||||
return task
|
||||
|
||||
def plan(self, task, expected_solutions=None, wait=False):
|
||||
task.plan()
|
||||
try:
|
||||
task.plan()
|
||||
except TypeError as e:
|
||||
self.fail(f"{e}\nMoveIt and MTC use ABI-incompatible pybind11 versions")
|
||||
|
||||
if expected_solutions is not None:
|
||||
self.assertEqual(len(task.solutions), expected_solutions)
|
||||
if wait:
|
||||
input("Waiting for any key (allows inspection in rviz)")
|
||||
|
||||
@unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg)
|
||||
def test_generator(self):
|
||||
task = self.create(PyGenerator())
|
||||
self.plan(task, expected_solutions=PyGenerator.max_calls)
|
||||
|
||||
@unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg)
|
||||
def test_monitoring_generator(self):
|
||||
task = self.create(
|
||||
stages.CurrentState("current"),
|
||||
|
||||
@ -228,6 +228,7 @@ class TestStages(unittest.TestCase):
|
||||
self._check(stage, "eef_frame", "eef_frame")
|
||||
self._check(stage, "eef_group", "eef_group")
|
||||
self._check(stage, "eef_parent_group", "eef_parent_group")
|
||||
self._check(stage.cartesian_solver, "max_velocity_scaling_factor", 0.1)
|
||||
|
||||
def test_Place(self):
|
||||
generator_stage = stages.GeneratePose("generator")
|
||||
|
||||
@ -40,6 +40,7 @@
|
||||
#include <moveit/task_constructor/utils.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||
#include <moveit/trajectory_processing/limit_cartesian_speed.h>
|
||||
#include <moveit/kinematics_base/kinematics_base.h>
|
||||
#include <moveit/robot_state/cartesian_interpolator.h>
|
||||
#include <tf2_eigen/tf2_eigen.h>
|
||||
@ -121,8 +122,19 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
|
||||
for (const auto& waypoint : trajectory)
|
||||
result->addSuffixWayPoint(waypoint, 0.0);
|
||||
|
||||
double max_cartesian_speed = props.get<double>("max_cartesian_speed");
|
||||
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
|
||||
if (timing)
|
||||
// compute timing to move the eef with constant speed
|
||||
if (max_cartesian_speed > 0.0) {
|
||||
if (trajectory_processing::limitMaxCartesianLinkSpeed(*result, max_cartesian_speed,
|
||||
props.get<std::string>("cartesian_speed_limited_link"))) {
|
||||
ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link "
|
||||
<< props.get<std::string>("cartesian_speed_limited_link"));
|
||||
} else {
|
||||
ROS_ERROR_STREAM("failed to set max speed for link_ "
|
||||
<< props.get<std::string>("cartesian_speed_limited_link"));
|
||||
}
|
||||
} else if (timing)
|
||||
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
||||
props.get<double>("max_acceleration_scaling_factor"));
|
||||
|
||||
|
||||
@ -149,6 +149,8 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
|
||||
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
|
||||
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
|
||||
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
|
||||
req.cartesian_speed_limited_link = p.get<std::string>("cartesian_speed_limited_link");
|
||||
req.max_cartesian_speed = p.get<double>("max_cartesian_speed");
|
||||
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
|
||||
}
|
||||
|
||||
|
||||
@ -50,6 +50,8 @@ PlannerInterface::PlannerInterface() {
|
||||
p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
|
||||
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
|
||||
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
|
||||
p.declare<double>("max_cartesian_speed", 0.0, "maximum cartesian speed");
|
||||
p.declare<std::string>("cartesian_speed_limited_link", "", "link with limited cartesian speed");
|
||||
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
|
||||
}
|
||||
} // namespace solvers
|
||||
|
||||
@ -13,6 +13,7 @@ add_library(${PROJECT_NAME}_stages
|
||||
${PROJECT_INCLUDE}/stages/passthrough.h
|
||||
${PROJECT_INCLUDE}/stages/noop.h
|
||||
${PROJECT_INCLUDE}/stages/predicate_filter.h
|
||||
${PROJECT_INCLUDE}/stages/limit_solutions.h
|
||||
|
||||
${PROJECT_INCLUDE}/stages/connect.h
|
||||
${PROJECT_INCLUDE}/stages/move_to.h
|
||||
@ -34,6 +35,7 @@ add_library(${PROJECT_NAME}_stages
|
||||
compute_ik.cpp
|
||||
passthrough.cpp
|
||||
predicate_filter.cpp
|
||||
limit_solutions.cpp
|
||||
|
||||
connect.cpp
|
||||
move_to.cpp
|
||||
|
||||
@ -89,7 +89,7 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
|
||||
struct IKSolution
|
||||
{
|
||||
std::vector<double> joint_positions;
|
||||
collision_detection::Contact contact;
|
||||
collision_detection::CollisionResult::ContactMap contacts;
|
||||
bool collision_free;
|
||||
bool satisfies_constraints;
|
||||
};
|
||||
@ -141,7 +141,7 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
|
||||
}
|
||||
|
||||
std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap& contacts,
|
||||
const std::string& separator) {
|
||||
const std::string& separator = ", ") {
|
||||
std::string result;
|
||||
for (const auto& contact : contacts) {
|
||||
if (!result.empty())
|
||||
@ -345,8 +345,8 @@ void ComputeIK::compute() {
|
||||
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
|
||||
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
|
||||
solution.markAsFailure();
|
||||
// TODO: visualize collisions
|
||||
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
|
||||
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts));
|
||||
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), collisions.contacts);
|
||||
auto colliding_scene{ scene->diff() };
|
||||
colliding_scene->setCurrentState(sandbox_state);
|
||||
spawn(InterfaceState(colliding_scene), std::move(solution));
|
||||
@ -395,9 +395,7 @@ void ComputeIK::compute() {
|
||||
req.group_name = jmg->getName();
|
||||
scene->checkCollision(req, res, *state);
|
||||
solution.collision_free = ignore_collisions || !res.collision;
|
||||
if (!res.contacts.empty()) {
|
||||
solution.contact = res.contacts.begin()->second.front();
|
||||
}
|
||||
solution.contacts = std::move(res.contacts);
|
||||
|
||||
return solution.satisfies_constraints && solution.collision_free;
|
||||
};
|
||||
@ -433,10 +431,8 @@ void ComputeIK::compute() {
|
||||
// compute cost as distance to compare_pose
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data()));
|
||||
else if (!ik_solutions[i].collision_free) { // solution was in collision
|
||||
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());
|
||||
solution.markAsFailure("Collision between " + listCollisionPairs(ik_solutions[i].contacts));
|
||||
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), ik_solutions[i].contacts);
|
||||
} else if (!ik_solutions[i].satisfies_constraints) { // solution was violating constraints
|
||||
solution.markAsFailure("Constraints violated");
|
||||
}
|
||||
|
||||
@ -150,6 +150,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
intermediate_scenes.push_back(start);
|
||||
|
||||
bool success = false;
|
||||
bool has_potential_collisions = false;
|
||||
std::string comment = "No planners specified";
|
||||
std::vector<double> positions;
|
||||
for (const GroupPlannerVector::value_type& pair : planner_) {
|
||||
@ -169,6 +170,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = trajectory && utils::hints_at_collisions(result);
|
||||
break;
|
||||
}
|
||||
|
||||
@ -187,8 +189,15 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
||||
if (!solution) // success == false or merging failed: store sequentially
|
||||
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
|
||||
if (!success) // error during sequential planning
|
||||
if (!success) { // error already during sequential planning
|
||||
solution->markAsFailure(comment);
|
||||
if (has_potential_collisions) {
|
||||
// add collision markers for last (failed) trajectory segment
|
||||
auto sequence = std::dynamic_pointer_cast<SolutionSequence>(solution);
|
||||
auto trajectory = dynamic_cast<const SubTrajectory*>(sequence->solutions().back())->trajectory();
|
||||
utils::addCollisionMarkers(solution->markers(), *trajectory, start);
|
||||
}
|
||||
}
|
||||
connect(from, to, solution);
|
||||
}
|
||||
|
||||
|
||||
78
core/src/stages/limit_solutions.cpp
Normal file
78
core/src/stages/limit_solutions.cpp
Normal file
@ -0,0 +1,78 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Bielefeld University nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Joseph Moore */
|
||||
|
||||
#include <moveit/task_constructor/stages/limit_solutions.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <fmt/format.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
LimitSolutions::LimitSolutions(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {
|
||||
auto& p = properties();
|
||||
p.declare<uint32_t>("max_solutions", "maximum number of solutions returned by this wrapper");
|
||||
forwarded_solutions = 0;
|
||||
}
|
||||
|
||||
void LimitSolutions::reset() {
|
||||
upstream_solutions_.clear();
|
||||
forwarded_solutions = 0;
|
||||
WrapperBase::reset();
|
||||
}
|
||||
|
||||
void LimitSolutions::onNewSolution(const SolutionBase& s) {
|
||||
uint32_t max_solutions = properties().get<uint32_t>("max_solutions");
|
||||
if (forwarded_solutions + upstream_solutions_.size() < max_solutions)
|
||||
upstream_solutions_.push(&s);
|
||||
}
|
||||
|
||||
bool LimitSolutions::canCompute() const {
|
||||
return !upstream_solutions_.empty() || WrapperBase::canCompute();
|
||||
}
|
||||
|
||||
void LimitSolutions::compute() {
|
||||
if (WrapperBase::canCompute())
|
||||
WrapperBase::compute();
|
||||
|
||||
if (upstream_solutions_.empty())
|
||||
return;
|
||||
|
||||
++forwarded_solutions;
|
||||
liftSolution(*upstream_solutions_.pop());
|
||||
}
|
||||
} // namespace stages
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
@ -163,6 +163,7 @@ std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const Interf
|
||||
if (res.collision) {
|
||||
const auto contact = res.contacts.begin()->second.front();
|
||||
traj.markAsFailure(contact.body_name_1 + " colliding with " + contact.body_name_2);
|
||||
utils::addCollisionMarkers(traj.markers(), scene->getPlanningFrame(), res.contacts);
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
traj.markAsFailure(e.what());
|
||||
|
||||
@ -94,7 +94,6 @@ static bool getJointStateFromOffset(const boost::any& direction, const Interface
|
||||
throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'");
|
||||
auto index = jm->getFirstVariableIndex();
|
||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + sign * j.second);
|
||||
robot_state.enforceBounds(jm);
|
||||
}
|
||||
robot_state.update();
|
||||
return true;
|
||||
@ -106,7 +105,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const Interface
|
||||
}
|
||||
|
||||
// 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::Marker>& markers, Interface::Direction dir, bool success,
|
||||
static void visualizePlan(std::vector<visualization_msgs::Marker>& markers, Interface::Direction dir, bool success,
|
||||
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();
|
||||
@ -194,14 +193,17 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||
bool success = false;
|
||||
bool has_potential_collisions = false;
|
||||
std::string comment = "";
|
||||
|
||||
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
||||
// plan to joint-space target
|
||||
auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
} else {
|
||||
// Cartesian targets require an IK reference frame
|
||||
const moveit::core::LinkModel* link;
|
||||
@ -293,8 +295,10 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
auto result =
|
||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { // the following requires a robot_trajectory
|
||||
// returned from planning
|
||||
@ -338,8 +342,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
robot_trajectory->reverse();
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
|
||||
if (!success)
|
||||
if (!success) {
|
||||
solution.markAsFailure(comment);
|
||||
if (has_potential_collisions)
|
||||
utils::addCollisionMarkers(solution.markers(), *robot_trajectory, scene);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
||||
@ -197,14 +197,17 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||
bool success = false;
|
||||
bool has_potential_collisions = false;
|
||||
std::string comment = "";
|
||||
|
||||
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
|
||||
// plan to joint-space target
|
||||
auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
} else { // Cartesian goal
|
||||
// Where to go?
|
||||
Eigen::Isometry3d target;
|
||||
@ -243,8 +246,10 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
const auto result =
|
||||
planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
}
|
||||
|
||||
// store result
|
||||
@ -259,9 +264,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
robot_trajectory->reverse();
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
|
||||
if (!success)
|
||||
if (!success) {
|
||||
solution.markAsFailure(comment);
|
||||
|
||||
if (has_potential_collisions)
|
||||
utils::addCollisionMarkers(solution.markers(), *robot_trajectory, scene);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
||||
@ -231,7 +231,8 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros
|
||||
if (trajectory())
|
||||
trajectory()->getRobotTrajectoryMsg(t.trajectory);
|
||||
|
||||
if (this->end()->scene()->getParent() == this->start()->scene())
|
||||
if (this->end()->scene()->getParent() == this->start()->scene() || // diff
|
||||
this->end()->scene() == this->start()->scene()) // identical (from generator)
|
||||
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
|
||||
else
|
||||
this->end()->scene()->getPlanningSceneMsg(t.scene_diff);
|
||||
|
||||
@ -39,6 +39,8 @@
|
||||
|
||||
#include <moveit/robot_state/robot_state.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
#include <moveit/utils/moveit_error_code.h>
|
||||
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
@ -47,7 +49,7 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace utils {
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, std::string& error_msg,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
|
||||
auto get_tip = [&jmg]() -> const moveit::core::LinkModel* {
|
||||
@ -62,7 +64,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
|
||||
error_msg = "";
|
||||
|
||||
if (property.value().empty()) { // property undefined
|
||||
if (tip_pose.value().empty()) { // property undefined
|
||||
robot_link = get_tip();
|
||||
if (!robot_link) {
|
||||
error_msg = "missing ik_frame";
|
||||
@ -70,7 +72,7 @@ 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::PoseStamped>(property.value());
|
||||
auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(tip_pose.value());
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
|
||||
robot_link = nullptr;
|
||||
@ -82,7 +84,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
error_msg = ss.str();
|
||||
return false;
|
||||
}
|
||||
if (!robot_link)
|
||||
if (!robot_link) // if ik_pose_msg.header.frame_id was empty, use default tip frame
|
||||
robot_link = get_tip();
|
||||
if (!robot_link) {
|
||||
error_msg = "ik_frame doesn't specify a link frame";
|
||||
@ -97,6 +99,59 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
return true;
|
||||
}
|
||||
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers, const std::string& frame_id,
|
||||
const collision_detection::CollisionResult::ContactMap& contacts, double radius) {
|
||||
visualization_msgs::Marker m;
|
||||
m.header.frame_id = frame_id;
|
||||
m.ns = "collisions";
|
||||
m.type = visualization_msgs::Marker::SPHERE;
|
||||
m.action = visualization_msgs::Marker::ADD;
|
||||
m.pose.orientation.x = 0.0;
|
||||
m.pose.orientation.y = 0.0;
|
||||
m.pose.orientation.z = 0.0;
|
||||
m.pose.orientation.w = 1.0;
|
||||
m.scale.x = m.scale.y = m.scale.z = radius * 2.0;
|
||||
m.color.r = m.color.a = 1.0;
|
||||
|
||||
for (const auto& collision : contacts) {
|
||||
for (const auto& contact : collision.second) {
|
||||
m.pose.position.x = contact.pos.x();
|
||||
m.pose.position.y = contact.pos.y();
|
||||
m.pose.position.z = contact.pos.z();
|
||||
markers.push_back(m);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers,
|
||||
const robot_trajectory::RobotTrajectory& trajectory,
|
||||
const planning_scene::PlanningSceneConstPtr& planning_scene) {
|
||||
std::size_t n_wp = trajectory.getWayPointCount();
|
||||
for (std::size_t it = 0; it < n_wp; ++it) {
|
||||
const moveit::core::RobotState& robot_state = trajectory.getWayPoint(it);
|
||||
|
||||
// Collision contact check
|
||||
collision_detection::CollisionRequest req;
|
||||
collision_detection::CollisionResult res;
|
||||
req.contacts = true;
|
||||
req.max_contacts = 10;
|
||||
|
||||
planning_scene->checkCollision(req, res, robot_state);
|
||||
|
||||
if (res.contact_count > 0)
|
||||
addCollisionMarkers(markers, planning_scene->getPlanningFrame(), res.contacts);
|
||||
}
|
||||
}
|
||||
|
||||
bool hints_at_collisions(const solvers::PlannerInterface::Result& result) {
|
||||
static const std::string INVALID_MOTION_PLAN_MSG = []() {
|
||||
moveit_msgs::MoveItErrorCodes err;
|
||||
err.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
return moveit::core::MoveItErrorCode::toString(err);
|
||||
}();
|
||||
return result.message == INVALID_MOTION_PLAN_MSG;
|
||||
}
|
||||
|
||||
} // namespace utils
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
|
||||
@ -36,11 +36,13 @@ if (CATKIN_ENABLE_TESTING)
|
||||
|
||||
mtc_add_gmock(test_mockups.cpp)
|
||||
mtc_add_gtest(test_stage.cpp)
|
||||
mtc_add_gtest(test_limit_solutions.cpp)
|
||||
mtc_add_gtest(test_container.cpp)
|
||||
mtc_add_gmock(test_serial.cpp)
|
||||
mtc_add_gmock(test_pruning.cpp)
|
||||
mtc_add_gtest(test_properties.cpp)
|
||||
mtc_add_gtest(test_cost_terms.cpp)
|
||||
mtc_add_gtest(test_storage.cpp)
|
||||
|
||||
mtc_add_gmock(test_fallback.cpp)
|
||||
mtc_add_gmock(test_cost_queue.cpp)
|
||||
|
||||
39
core/test/test_limit_solutions.cpp
Normal file
39
core/test/test_limit_solutions.cpp
Normal file
@ -0,0 +1,39 @@
|
||||
#include <moveit/task_constructor/stages/limit_solutions.h>
|
||||
|
||||
#include "stage_mockups.h"
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
using LimitSolutionsTest = TaskTestBase;
|
||||
|
||||
TEST_F(LimitSolutionsTest, returnsMaxSolutions) {
|
||||
auto g = std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 }));
|
||||
auto ls = std::make_unique<stages::LimitSolutions>("LimitSolutions", std::move(g));
|
||||
ls->setMaxSolutions(2);
|
||||
t.add(std::move(ls));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_EQ(t.solutions().size(), 2u);
|
||||
}
|
||||
|
||||
TEST_F(LimitSolutionsTest, returnsAllChildSolutionsIfFewerThanMax) {
|
||||
auto g = std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 }));
|
||||
auto ls = std::make_unique<stages::LimitSolutions>("LimitSolutions", std::move(g));
|
||||
ls->setMaxSolutions(5);
|
||||
t.add(std::move(ls));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_EQ(t.solutions().size(), 3u);
|
||||
}
|
||||
|
||||
TEST_F(LimitSolutionsTest, returnsBestSolutions) {
|
||||
// generator can create solutions with costs 1-4 in a single call to compute
|
||||
auto g = std::make_unique<GeneratorMockup>(PredefinedCosts({ 4.0, 2.0, 3.0, 1.0 }), 4);
|
||||
auto ls = std::make_unique<stages::LimitSolutions>("LimitSolutions", std::move(g));
|
||||
ls->setMaxSolutions(3); // however, only the first 3 solutions will be considered
|
||||
t.add(std::move(ls));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_EQ(t.solutions().size(), 3u); // only 3 solutions should have been generated
|
||||
EXPECT_EQ(t.solutions().front()->cost(), 2.0); // solution with cost 1.0 was not considered anymore
|
||||
}
|
||||
@ -4,6 +4,7 @@
|
||||
#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/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
@ -32,6 +33,10 @@ solvers::CartesianPathPtr create<solvers::CartesianPath>() {
|
||||
return std::make_shared<solvers::CartesianPath>();
|
||||
}
|
||||
template <>
|
||||
solvers::JointInterpolationPlannerPtr create<solvers::JointInterpolationPlanner>() {
|
||||
return std::make_shared<solvers::JointInterpolationPlanner>();
|
||||
}
|
||||
template <>
|
||||
solvers::PipelinePlannerPtr create<solvers::PipelinePlanner>() {
|
||||
auto p = std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner");
|
||||
p->setPlannerId("LIN");
|
||||
@ -68,6 +73,7 @@ struct PandaMoveRelative : public testing::Test
|
||||
}
|
||||
};
|
||||
using PandaMoveRelativeCartesian = PandaMoveRelative<solvers::CartesianPath>;
|
||||
using PandaMoveRelativeJoint = PandaMoveRelative<solvers::JointInterpolationPlanner>;
|
||||
|
||||
moveit_msgs::CollisionObject createObject(const std::string& id, const geometry_msgs::Pose& pose) {
|
||||
moveit_msgs::CollisionObject co;
|
||||
@ -163,6 +169,25 @@ TEST_F(PandaMoveRelativeCartesian, cartesianRotateAttachedIKFrame) {
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), attached_object);
|
||||
}
|
||||
|
||||
// https://github.com/moveit/moveit_task_constructor/issues/664
|
||||
TEST_F(PandaMoveRelativeJoint, jointOutsideBound) {
|
||||
// move joint inside limit
|
||||
auto initial_jpos = scene->getCurrentState().getJointPositions("panda_joint7");
|
||||
move->setDirection([initial_jpos] {
|
||||
return std::map<std::string, double>{ { "panda_joint7", 2.0 - *initial_jpos } };
|
||||
}());
|
||||
EXPECT_TRUE(this->t.plan()) << "Plan should succeed, joint inside limit";
|
||||
|
||||
this->t.reset();
|
||||
|
||||
// move joint outside limit: 2.8973
|
||||
move->setDirection([initial_jpos] {
|
||||
return std::map<std::string, double>{ { "panda_joint7", 3.0 - *initial_jpos } };
|
||||
}());
|
||||
|
||||
EXPECT_FALSE(this->t.plan()) << "Plan should fail, joint outside limit";
|
||||
}
|
||||
|
||||
using PlannerTypes = ::testing::Types<solvers::CartesianPath, solvers::PipelinePlanner>;
|
||||
TYPED_TEST_SUITE(PandaMoveRelative, PlannerTypes);
|
||||
TYPED_TEST(PandaMoveRelative, cartesianCollisionMinMaxDistance) {
|
||||
|
||||
35
core/test/test_storage.cpp
Normal file
35
core/test/test_storage.cpp
Normal file
@ -0,0 +1,35 @@
|
||||
#include "models.h"
|
||||
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace planning_scene;
|
||||
using namespace moveit::core;
|
||||
|
||||
// https://github.com/moveit/moveit_task_constructor/issues/638
|
||||
TEST(SolutionMsg, DuplicateScenes) {
|
||||
Task t;
|
||||
PlanningScenePtr scene;
|
||||
|
||||
t.setRobotModel(getModel());
|
||||
scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
||||
t.add(std::make_unique<stages::FixedState>("start", scene));
|
||||
|
||||
EXPECT_TRUE(t.plan(1));
|
||||
EXPECT_EQ(t.solutions().size(), 1u);
|
||||
|
||||
// create solution
|
||||
moveit_task_constructor_msgs::Solution solution_msg;
|
||||
t.solutions().front()->toMsg(solution_msg);
|
||||
|
||||
// all sub trajectories `scene_diff` should be a diff
|
||||
EXPECT_EQ(solution_msg.sub_trajectory.size(), 1u);
|
||||
EXPECT_EQ(solution_msg.start_scene.is_diff, false);
|
||||
EXPECT_EQ(solution_msg.sub_trajectory.front().scene_diff.is_diff, true);
|
||||
}
|
||||
@ -2,6 +2,28 @@
|
||||
Changelog for package moveit_task_constructor_demo
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.4 (2025-10-15)
|
||||
------------------
|
||||
* Allow max Cartesian link speed in PlannerInterface (`#277 <https://github.com/moveit/moveit_task_constructor/issues/277>`_)
|
||||
* Add missing dependency
|
||||
* CI: Fix Noble builds
|
||||
* Pick with custom max_velocity_scaling_factor during approach+lift
|
||||
* Fix pick+place: connect should plan both, arm and hand motion
|
||||
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
|
||||
* clang-tidy fixes: std::endl -> '\n'
|
||||
* examples: add orientation path constraint
|
||||
* Update API: JumpThreshold -> CartesianPrecision (`#611 <https://github.com/moveit/moveit_task_constructor/issues/611>`_)
|
||||
* Unify Python demo scripts
|
||||
* Switch shebang to python3
|
||||
* Improve comments for pick-and-place task (`#238 <https://github.com/moveit/moveit_task_constructor/issues/238>`_)
|
||||
* Expose MultiPlanner to Python (`#474 <https://github.com/moveit/moveit_task_constructor/issues/474>`_)
|
||||
* Example of constrained orientation planning
|
||||
* Switch to package py_binding_tools
|
||||
* Fix demos (`#493 <https://github.com/moveit/moveit_task_constructor/issues/493>`_)
|
||||
* Merge PR `#460 <https://github.com/moveit/moveit_task_constructor/issues/460>`_: improvements to ModifyPlanningScene stage
|
||||
* Fix SolutionBase::fillMessage(): also write start_scene
|
||||
* Contributors: Fabian Schuetze, Gauthier Hentz, Michael Görner, Robert Haschke, VideoSystemsTech
|
||||
|
||||
0.1.3 (2023-03-06)
|
||||
------------------
|
||||
* Use const reference instead of reference for ros::NodeHandle (`#437 <https://github.com/ros-planning/moveit_task_constructor/issues/437>`_)
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>moveit_task_constructor_demo</name>
|
||||
<version>0.1.3</version>
|
||||
<version>0.1.4</version>
|
||||
<description>demo tasks illustrating various capabilities of MTC.</description>
|
||||
|
||||
<author email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</author>
|
||||
@ -22,6 +22,7 @@
|
||||
<depend>moveit_task_constructor_core</depend>
|
||||
|
||||
<exec_depend>moveit_task_constructor_capabilities</exec_depend>
|
||||
<exec_depend>moveit_task_constructor_visualization</exec_depend>
|
||||
<exec_depend>moveit_resources_panda_moveit_config</exec_depend>
|
||||
<exec_depend>py_binding_tools</exec_depend>
|
||||
|
||||
|
||||
@ -18,6 +18,8 @@ group = "panda_arm"
|
||||
# [cartesianTut2]
|
||||
# Cartesian and joint-space interpolation planners
|
||||
cartesian = core.CartesianPath()
|
||||
cartesian.max_cartesian_speed = 0.1 # m/s
|
||||
cartesian.cartesian_speed_limited_link = "panda_hand"
|
||||
jointspace = core.JointInterpolationPlanner()
|
||||
# [cartesianTut2]
|
||||
|
||||
|
||||
@ -98,6 +98,8 @@ approach.header.frame_id = "world"
|
||||
approach.twist.linear.z = -1.0
|
||||
pick.setApproachMotion(approach, 0.03, 0.1)
|
||||
|
||||
pick.cartesian_solver.max_velocity_scaling_factor = 0.1
|
||||
|
||||
# Twist to lift the object
|
||||
lift = TwistStamped()
|
||||
lift.header.frame_id = "panda_hand"
|
||||
|
||||
@ -220,8 +220,9 @@ bool PickPlaceTask::init() {
|
||||
***************************************************/
|
||||
// Connect initial open-hand state with pre-grasp pose defined in the following
|
||||
{
|
||||
auto stage = std::make_unique<stages::Connect>(
|
||||
"move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
|
||||
stages::Connect::GroupPlannerVector planners = { { arm_group_name_, sampling_planner },
|
||||
{ hand_group_name_, sampling_planner } };
|
||||
auto stage = std::make_unique<stages::Connect>("move to pick", planners);
|
||||
stage->setTimeout(5.0);
|
||||
stage->properties().configureInitFrom(Stage::PARENT);
|
||||
t.add(std::move(stage));
|
||||
|
||||
@ -10,7 +10,8 @@
|
||||
<!-- use MTC execution capability -->
|
||||
<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
|
||||
|
||||
<test pkg="moveit_task_constructor_demo" type="pick_place_test" test-name="pick_place_test">
|
||||
<test pkg="moveit_task_constructor_demo" type="pick_place_test" test-name="pick_place_test"
|
||||
if="$(eval bool(optenv('PRELOAD')))">
|
||||
<rosparam command="load" file="$(find moveit_task_constructor_demo)/config/panda_config.yaml" />
|
||||
</test>
|
||||
</launch>
|
||||
|
||||
@ -2,6 +2,12 @@
|
||||
Changelog for package moveit_task_constructor_msgs
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.4 (2025-10-15)
|
||||
------------------
|
||||
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
|
||||
* Add property trajectory_execution_info (`#355 <https://github.com/moveit/moveit_task_constructor/issues/355>`_, `#502 <https://github.com/moveit/moveit_task_constructor/issues/502>`_)
|
||||
* Contributors: Robert Haschke, Luca Lach
|
||||
|
||||
0.1.3 (2023-03-06)
|
||||
------------------
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<package format="2">
|
||||
<name>moveit_task_constructor_msgs</name>
|
||||
<version>0.1.3</version>
|
||||
<version>0.1.4</version>
|
||||
<description>Messages for MoveIt Task Pipeline</description>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
@ -2,6 +2,16 @@
|
||||
Changelog for package rviz_marker_tools
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.4 (2025-10-15)
|
||||
------------------
|
||||
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
|
||||
* clang-tidy fixes: use uint8_t enums
|
||||
* Ignore Debian-specific catkin_lint error around urdfdom_headers (`#614 <https://github.com/moveit/moveit_task_constructor/issues/614>`_)
|
||||
* clean up dependencies for rviz_marker_tools (`#610 <https://github.com/moveit/moveit_task_constructor/issues/610>`_)
|
||||
* rviz_marker_tools: add missing dependency on urdfdom
|
||||
* rviz_marker_tools: drop rviz dependency
|
||||
* Contributors: Michael Görner, Robert Haschke
|
||||
|
||||
0.1.3 (2023-03-06)
|
||||
------------------
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<package format="2">
|
||||
<name>rviz_marker_tools</name>
|
||||
<version>0.1.3</version>
|
||||
<version>0.1.4</version>
|
||||
<description>Tools for marker creation / handling</description>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
@ -2,6 +2,24 @@
|
||||
Changelog for package moveit_task_constructor_visualization
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.4 (2025-10-15)
|
||||
------------------
|
||||
* Mandatory yaml dependency
|
||||
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
|
||||
* clang-format-14
|
||||
* clang-tidy fixes: use uint8_t enums
|
||||
* Simplify formatting code with https://github.com/fmtlib (`#499 <https://github.com/moveit/moveit_task_constructor/issues/499>`_)
|
||||
* Drop Kinetic support
|
||||
* Fix discontinuity in trajectory (`#485 <https://github.com/moveit/moveit_task_constructor/issues/485>`_)
|
||||
* Cleanup debug output
|
||||
* Add more debugging output
|
||||
* Hide button to show rviz-based task construction (`#492 <https://github.com/moveit/moveit_task_constructor/issues/492>`_)
|
||||
* Fix Qt 5.15 deprecation warnings
|
||||
* Limit time to wait for execute_task_solution action server
|
||||
* Replace namespace robot\_[model|state] with moveit::core
|
||||
* Use pluginlib consistently (`#463 <https://github.com/moveit/moveit_task_constructor/issues/463>`_)
|
||||
* Contributors: Michael Görner, Robert Haschke
|
||||
|
||||
0.1.3 (2023-03-06)
|
||||
------------------
|
||||
|
||||
|
||||
@ -2,16 +2,13 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_properties)
|
||||
|
||||
set(SOURCES
|
||||
property_factory.cpp
|
||||
property_from_yaml.cpp
|
||||
)
|
||||
|
||||
find_package(PkgConfig REQUIRED)
|
||||
pkg_check_modules(YAML yaml-0.1)
|
||||
if (YAML_FOUND)
|
||||
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually
|
||||
find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS})
|
||||
list(APPEND SOURCES property_from_yaml.cpp)
|
||||
add_definitions(-DHAVE_YAML)
|
||||
endif()
|
||||
pkg_check_modules(YAML REQUIRED yaml-0.1)
|
||||
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually
|
||||
find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS})
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES})
|
||||
|
||||
|
||||
@ -147,21 +147,4 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property
|
||||
new rviz::Property("no properties", QVariant(), QString(), root);
|
||||
}
|
||||
|
||||
#ifndef HAVE_YAML
|
||||
rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/,
|
||||
const std::string& description, const std::string& value,
|
||||
rviz::Property* old) {
|
||||
if (old) { // reuse existing Property?
|
||||
assert(old->getNameStd() == name);
|
||||
old->setDescription(QString::fromStdString(description));
|
||||
old->setValue(QString::fromStdString(value));
|
||||
return old;
|
||||
} else { // create new Property?
|
||||
rviz::Property* result = new rviz::StringProperty(QString::fromStdString(name), QString::fromStdString(value),
|
||||
QString::fromStdString(description));
|
||||
result->setReadOnly(true);
|
||||
return result;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
} // namespace moveit_rviz_plugin
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<package format="2">
|
||||
<name>moveit_task_constructor_visualization</name>
|
||||
<version>0.1.3</version>
|
||||
<version>0.1.4</version>
|
||||
<description>Visualization tools for MoveIt Task Pipeline</description>
|
||||
|
||||
<license>BSD</license>
|
||||
@ -18,6 +18,7 @@
|
||||
<depend>moveit_ros_visualization</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>rviz</depend>
|
||||
<depend>yaml</depend>
|
||||
|
||||
<test_depend>rosunit</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user