Merge branch 'master' into ros2

.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

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 <Eigen/Core>
+#include <fmt/core.h>
+#include <fmt/ostream.h>
+
+#if FMT_VERSION >= 90000
+template <typename T>
+struct fmt::formatter<T, std::enable_if_t<std::is_base_of_v<Eigen::DenseBase<T>, T>, char>> : ostream_formatter
+{};
+#endif

core/python/bindings/src/solvers.cpp

@@ -37,8 +37,8 @@
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/task_constructor/solvers/multi_planner.h>
+#include <moveit/task_constructor/fmt_p.h>
 #include <moveit_msgs/msg/workspace_parameters.hpp>
-#include <fmt/core.h>
 #include "utils.h"
 
 namespace py = pybind11;

core/python/bindings/src/stages.cpp

@@ -328,6 +328,11 @@ void export_stages(pybind11::module& m) {
 		)")
 	    .property<stages::Connect::MergeMode>("merge_mode", "Defines the merge strategy to use")
 	    .property<double>("max_distance", "maximally accepted distance between end and goal sate")
+	    .property<moveit_msgs::Constraints>("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<const std::string&, const Connect::GroupPlannerVector&>(),
 	         "name"_a = std::string("connect"), "planners"_a);
 

core/src/container.cpp

@@ -37,6 +37,7 @@
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/introspection.h>
 #include <moveit/task_constructor/merge.h>
+#include <moveit/task_constructor/fmt_p.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 
@@ -47,7 +48,6 @@
 #include <iostream>
 #include <algorithm>
 #include <boost/range/adaptor/reversed.hpp>
-#include <fmt/core.h>
 #include <functional>
 
 using namespace std::placeholders;

core/src/cost_terms.cpp

@@ -36,6 +36,7 @@
 
 #include <moveit/task_constructor/cost_terms.h>
 #include <moveit/task_constructor/stage.h>
+#include <moveit/task_constructor/fmt_p.h>
 
 #include <moveit/collision_detection/collision_common.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
@@ -44,7 +45,6 @@
 
 #include <Eigen/Geometry>
 
-#include <fmt/core.h>
 #include <utility>
 
 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<std::string>(group_property) :
-                            stage_properties.get<std::string>(group_property);
+	                         state_properties.get<std::string>(group_property) :
+	                         stage_properties.get<std::string>(group_property);
 
 	// look at all forbidden collisions involving group_name
 	request.enableGroup(state->scene()->getRobotModel());

core/src/properties.cpp

@@ -37,8 +37,7 @@
 */
 
 #include <moveit/task_constructor/properties.h>
-#include <fmt/core.h>
-#include <fmt/ostream.h>
+#include <moveit/task_constructor/fmt_p.h>
 #include <functional>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/clock.hpp>

core/src/stage.cpp

@@ -38,6 +38,7 @@
 #include <moveit/task_constructor/stage_p.h>
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/introspection.h>
+#include <moveit/task_constructor/fmt_p.h>
 
 #include <moveit/planning_scene/planning_scene.h>
 

core/src/stages/compute_ik.cpp

@@ -37,6 +37,7 @@
 #include <moveit/task_constructor/stages/compute_ik.h>
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/marker_tools.h>
+#include <moveit/task_constructor/fmt_p.h>
 
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/robot_state/conversions.h>
@@ -52,7 +53,6 @@
 #include <functional>
 #include <iterator>
 #include <rclcpp/logging.hpp>
-#include <fmt/core.h>
 
 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;
 		}

core/src/stages/connect.cpp

@@ -39,11 +39,10 @@
 #include <moveit/task_constructor/stages/connect.h>
 #include <moveit/task_constructor/merge.h>
 #include <moveit/task_constructor/cost_terms.h>
+#include <moveit/task_constructor/fmt_p.h>
 
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
-#include <fmt/core.h>
-#include <fmt/ostream.h>
 
 #if FMT_VERSION >= 90000
 template <>

core/src/stages/fix_collision_objects.cpp

@@ -38,6 +38,7 @@
 
 #include <moveit/task_constructor/stages/fix_collision_objects.h>
 #include <moveit/task_constructor/storage.h>
+#include <moveit/task_constructor/fmt_p.h>
 
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/task_constructor/cost_terms.h>
@@ -50,7 +51,6 @@
 #endif
 #include <Eigen/Geometry>
 #include <rclcpp/logging.hpp>
-#include <fmt/core.h>
 
 namespace vm = visualization_msgs;
 namespace cd = collision_detection;

core/src/stages/modify_planning_scene.cpp

@@ -39,9 +39,9 @@
 #include <moveit/task_constructor/stages/modify_planning_scene.h>
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/cost_terms.h>
+#include <moveit/task_constructor/fmt_p.h>
 
 #include <moveit/planning_scene/planning_scene.h>
-#include <fmt/core.h>
 
 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<int8_t>(moveit_msgs::msg::CollisionObject::ADD) :
-                                   static_cast<int8_t>(moveit_msgs::msg::CollisionObject::REMOVE);
+	                                static_cast<int8_t>(moveit_msgs::msg::CollisionObject::REMOVE);
 	for (const std::string& name : pair.second.first) {
 		obj.object.id = name;
 		scene.processAttachedCollisionObjectMsg(obj);

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

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,15 +57,15 @@
 planners = [(arm, pipeline)]
 
 # Connect the two stages
-task.add(stages.Connect("connect1", planners))
+task.add(stages.Connect("connect", planners))
 # [initAndConfigConnect]
 # [pickAndPlaceTut4]
 
 # [pickAndPlaceTut5]
 # [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")