From d50c846a7bbe50a30a1b2fbd5ab34e59d8819ea9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 24 Sep 2024 20:32:42 +0200 Subject: [PATCH 1/5] Ignore Debian-specific catkin_lint error around urdfdom_headers (#614) Without the patch: $ catkin_lint rviz_marker_tools/ rviz_marker_tools: package.xml: error: missing build_depend on 'urdfdom_headers' requires catkin_lint 1.6.24 for successful error suppression. --- rviz_marker_tools/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index ed931f8e6..3878ba199 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -10,6 +10,8 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(Eigen3 REQUIRED) +# lint ignore is needed to support ROS distributions which also define urdfdom_headers in rosdep (e.g., Debian ROS packages) +#catkin_lint: ignore missing_depend[pkg=urdfdom_headers] find_package(urdfdom_headers REQUIRED) catkin_package( From 721ff356b79bf23163b0e4d9091b69e756ff0ddd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 24 Sep 2024 20:41:12 +0200 Subject: [PATCH 2/5] provide a fmt wrapper (#615) for special eigen formatter, which is not available in fmt9 https://github.com/fmtlib/fmt/issues/3465 https://stackoverflow.com/a/73755864/21260084 --- core/include/moveit/task_constructor/fmt_p.h | 49 ++++++++++++++++++++ core/python/bindings/src/solvers.cpp | 2 +- core/src/container.cpp | 2 +- core/src/cost_terms.cpp | 2 +- core/src/properties.cpp | 3 +- core/src/stage.cpp | 2 +- core/src/stages/compute_ik.cpp | 2 +- core/src/stages/connect.cpp | 3 +- core/src/stages/fix_collision_objects.cpp | 2 +- core/src/stages/modify_planning_scene.cpp | 2 +- 10 files changed, 58 insertions(+), 11 deletions(-) create mode 100644 core/include/moveit/task_constructor/fmt_p.h diff --git a/core/include/moveit/task_constructor/fmt_p.h b/core/include/moveit/task_constructor/fmt_p.h new file mode 100644 index 000000000..618c3d66e --- /dev/null +++ b/core/include/moveit/task_constructor/fmt_p.h @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, University of Hamburg + * All rights reserved. + * + * 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 holder 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: Michael Goerner + Desc: Thin wrapper around fmt includes to provide Eigen formatters for fmt9 +*/ + +#pragma once + +#include +#include +#include + +#if FMT_VERSION >= 90000 +template +struct fmt::formatter, T>, char>> : ostream_formatter +{}; +#endif diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 572cf71e2..29c87b316 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -37,8 +37,8 @@ #include #include #include +#include #include -#include #include "utils.h" namespace py = pybind11; diff --git a/core/src/container.cpp b/core/src/container.cpp index 265d8d00f..9d8197515 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -46,7 +47,6 @@ #include #include #include -#include #include using namespace std::placeholders; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 8ba3aac91..4bd6ebadc 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -44,7 +45,6 @@ #include -#include #include namespace moveit { diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 95bd2570c..4f727b8e9 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -37,8 +37,7 @@ */ #include -#include -#include +#include #include #include diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 9fbf4d27e..896668e13 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -38,11 +38,11 @@ #include #include #include +#include #include #include -#include #include #include diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 29cf79afd..bf0fa4a45 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -48,7 +49,6 @@ #include #include #include -#include namespace moveit { namespace task_constructor { diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index da6e3406b..c300a2617 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -39,11 +39,10 @@ #include #include #include +#include #include #include -#include -#include using namespace trajectory_processing; diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index f371191f0..4f68fe6e6 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -46,7 +47,6 @@ #include #include #include -#include namespace vm = visualization_msgs; namespace cd = collision_detection; diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 3a5389b35..dd24c7594 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -39,9 +39,9 @@ #include #include #include +#include #include -#include namespace moveit { namespace task_constructor { From 0a502ce7fecacfd802752667421027e2d45d5e34 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 11 Oct 2024 22:11:33 +0200 Subject: [PATCH 3/5] Add path_constraints property to Connect stage --- core/python/bindings/src/stages.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index fc8c5949e..b341ebf09 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -328,6 +328,11 @@ void export_stages(pybind11::module& m) { )") .property("merge_mode", "Defines the merge strategy to use") .property("max_distance", "maximally accepted distance between end and goal sate") + .property("path_constraints", R"( + Constraints_: These are the path constraints. + + .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html + )") .def(py::init(), "name"_a = std::string("connect"), "planners"_a); From 69b4606bca5a5c117a5bcad1a60ee94dfe920730 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 13 Oct 2024 23:53:06 +0200 Subject: [PATCH 4/5] CI: Return to custom cache action --- .github/workflows/ci.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1feef010e..1ee865b53 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -56,14 +56,15 @@ jobs: submodules: recursive - name: Cache ccache - uses: actions/cache@v4 + uses: rhaschke/cache@main with: - save-always: true 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 From 1acf72e0b4347e0d4fe89a379de73f6d03a09633 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 15 Oct 2024 15:30:00 +0200 Subject: [PATCH 5/5] examples: add orientation path constraint constrained.py: constrain orientation of attached object pickplace.py: keep object upright during transport --- demo/scripts/constrained.py | 24 +++++++++++++++++++----- demo/scripts/pickplace.py | 37 +++++++++++++++++++++++++++++-------- 2 files changed, 48 insertions(+), 13 deletions(-) diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py index 4efc53c2a..1849f0fd2 100755 --- a/demo/scripts/constrained.py +++ b/demo/scripts/constrained.py @@ -38,21 +38,35 @@ mps = stages.ModifyPlanningScene("modify planning scene") mps.addObject(co) + +co.id = "object" +co.primitives[0].type = SolidPrimitive.BOX +co.primitives[0].dimensions = [0.1, 0.05, 0.03] +pose = co.primitive_poses[0] +pose.position.x = 0.30702 +pose.position.y = 0.0 +pose.position.z = 0.485 +pose.orientation.x = pose.orientation.w = 0.70711 # 90° about x +mps.addObject(co) +mps.attachObjects(["object"], "panda_hand") + task.add(mps) move = stages.MoveRelative("y +0.4", planner) +move.timeout = 5 move.group = group header = Header(frame_id="world") move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0))) constraints = Constraints() oc = OrientationConstraint() +oc.parameterization = oc.ROTATION_VECTOR oc.header.frame_id = "world" -oc.link_name = "panda_hand" -oc.orientation.x = 1.0 -oc.absolute_x_axis_tolerance = 0.01 -oc.absolute_y_axis_tolerance = 0.01 -oc.absolute_z_axis_tolerance = 0.01 +oc.link_name = "object" +oc.orientation.x = oc.orientation.w = 0.70711 # 90° about x +oc.absolute_x_axis_tolerance = 0.1 +oc.absolute_y_axis_tolerance = 0.1 +oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 constraints.orientation_constraints.append(oc) move.path_constraints = constraints diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index 5105c6d12..709b786c9 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -5,7 +5,8 @@ from moveit.task_constructor import core, stages from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped, TwistStamped -import time +from moveit_msgs.msg import Constraints, OrientationConstraint +import math, time roscpp_init("mtc_tutorial") @@ -17,7 +18,7 @@ # [pickAndPlaceTut2] # Specify object parameters -object_name = "grasp_object" +object_name = "object" object_radius = 0.02 # Start with a clear planning scene @@ -28,7 +29,7 @@ # Grasp object properties objectPose = PoseStamped() objectPose.header.frame_id = "world" -objectPose.pose.orientation.x = 1.0 +objectPose.pose.orientation.w = 1.0 objectPose.pose.position.x = 0.30702 objectPose.pose.position.y = 0.0 objectPose.pose.position.z = 0.285 @@ -56,7 +57,7 @@ planners = [(arm, pipeline)] # Connect the two stages -task.add(stages.Connect("connect1", planners)) +task.add(stages.Connect("connect", planners)) # [initAndConfigConnect] # [pickAndPlaceTut4] @@ -64,7 +65,7 @@ # [initAndConfigGenerateGraspPose] # The grasp generator spawns a set of possible grasp poses around the object grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose") -grasp_generator.angle_delta = 0.2 +grasp_generator.angle_delta = math.pi / 2 grasp_generator.pregrasp = "open" grasp_generator.grasp = "close" grasp_generator.setMonitoredStage(task["current"]) # Generate solutions for all initial states @@ -78,7 +79,8 @@ # Set frame for IK calculation in the center between the fingers ik_frame = PoseStamped() ik_frame.header.frame_id = "panda_hand" -ik_frame.pose.position.z = 0.1034 +ik_frame.pose.position.z = 0.1034 # tcp between fingers +ik_frame.pose.orientation.x = 1.0 # grasp from top simpleGrasp.setIKFrame(ik_frame) # [initAndConfigSimpleGrasp] # [pickAndPlaceTut6] @@ -109,16 +111,35 @@ # [initAndConfigPick] # [pickAndPlaceTut8] +# Define orientation constraint to keep the object upright +oc = OrientationConstraint() +oc.parameterization = oc.ROTATION_VECTOR +oc.header.frame_id = "world" +oc.link_name = "object" +oc.orientation.w = 1 +oc.absolute_x_axis_tolerance = 0.1 +oc.absolute_y_axis_tolerance = 0.1 +oc.absolute_z_axis_tolerance = math.pi +oc.weight = 1.0 + +constraints = Constraints() +constraints.name = "object:upright" +constraints.orientation_constraints.append(oc) + # [pickAndPlaceTut9] # Connect the Pick stage with the following Place stage -task.add(stages.Connect("connect2", planners)) +con = stages.Connect("connect", planners) +con.path_constraints = constraints +task.add(con) # [pickAndPlaceTut9] # [pickAndPlaceTut10] # [initAndConfigGeneratePlacePose] # Define the pose that the object should have after placing placePose = objectPose -placePose.pose.position.y += 0.2 # shift object by 20cm along y axis +placePose.pose.position.x = -0.2 +placePose.pose.position.y = -0.6 +placePose.pose.position.z = 0.0 # Generate Cartesian place poses for the object place_generator = stages.GeneratePlacePose("Generate Place Pose")