diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index cd387f276..845e3266b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -59,14 +59,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 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 0952d4b50..7c6a12f49 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/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index e86ef4de6..cee81dcd6 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); diff --git a/core/src/container.cpp b/core/src/container.cpp index afa0bc9bf..826ed005b 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -47,7 +48,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 04dc6b8d8..4afeffe97 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 { @@ -242,8 +242,8 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto& state_properties{ state->properties() }; auto& stage_properties{ s.creator()->properties() }; request.group_name = state_properties.hasProperty(group_property) ? - state_properties.get(group_property) : - stage_properties.get(group_property); + state_properties.get(group_property) : + stage_properties.get(group_property); // look at all forbidden collisions involving group_name request.enableGroup(state->scene()->getRobotModel()); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 2c504bf55..8c001de97 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -37,8 +37,7 @@ */ #include -#include -#include +#include #include #include #include diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 733f61a7a..97e519321 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 224044a9c..6644fc95f 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 @@ -52,7 +53,6 @@ #include #include #include -#include namespace moveit { namespace task_constructor { @@ -295,7 +295,7 @@ void ComputeIK::compute() { if (value.empty()) { // property undefined // determine IK link from eef/group if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) : - jmg->getOnlyOneEndEffectorTip())) { + jmg->getOnlyOneEndEffectorTip())) { RCLCPP_WARN_STREAM(LOGGER, "Failed to derive IK target link"); return; } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 509b28f81..3e1923503 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 #if FMT_VERSION >= 90000 template <> diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index f9580c567..8b4042ca6 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 @@ -50,7 +51,6 @@ #endif #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 5c126f4fd..21b86a4e5 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 { @@ -105,7 +105,7 @@ void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, if (invert) attach = !attach; obj.object.operation = attach ? static_cast(moveit_msgs::msg::CollisionObject::ADD) : - static_cast(moveit_msgs::msg::CollisionObject::REMOVE); + static_cast(moveit_msgs::msg::CollisionObject::REMOVE); for (const std::string& name : pair.second.first) { obj.object.id = name; scene.processAttachedCollisionObjectMsg(obj); diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py index 8f0abd3f9..82d7319e6 100755 --- a/demo/scripts/constrained.py +++ b/demo/scripts/constrained.py @@ -40,22 +40,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(x=0.0, y=0.4, z=0.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.orientation.w = 0.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 e33503495..be32302e7 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 rclcpp.init() node = rclcpp.Node("mtc_tutorial") @@ -18,7 +19,7 @@ # [pickAndPlaceTut2] # Specify object parameters -object_name = "grasp_object" +object_name = "object" object_radius = 0.02 # Start with a clear planning scene @@ -29,7 +30,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")