mtc

air_moveit_config/scripts/generate_pose.py

@@ -0,0 +1,51 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from moveit.task_constructor import core, stages
+from geometry_msgs.msg import PoseStamped
+from moveit_commander.roscpp_initializer import roscpp_initialize
+import time
+
+roscpp_initialize("mtc_tutorial_compute_ik")
+
+# Specify the planning group
+group = "air"
+
+# Create a task
+task = core.Task()
+
+# Get the current robot state
+currentState = stages.CurrentState("current state")
+task.add(currentState)
+
+# Create a planner instance that is used to connect
+# the current state to the grasp approach pose
+pipelinePlanner = core.PipelinePlanner()
+pipelinePlanner.planner = "RRTConnectkConfigDefault"
+planners = [(group, pipelinePlanner)]
+
+# Connect the two stages
+connect = stages.Connect("connect1", planners)
+connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT)
+task.add(connect)
+
+# [initAndConfigGeneratePose]
+# create an example pose wrt. the origin of the
+# panda arm link8
+pose = PoseStamped()
+pose.header.frame_id = "qbhand_base_link"
+
+# Calculate the inverse kinematics for the current robot state
+generatePose = stages.GeneratePose("generate pose")
+
+# spwan a pose whenever there is a solution of the monitored stage
+generatePose.setMonitoredStage(task["current state"])
+generatePose.pose = pose
+
+# Add the stage to the task hierarchy
+task.add(generatePose)
+# [initAndConfigGeneratePose]
+
+if task.plan():
+    task.publish(task.solutions[0])
+time.sleep(1)

air_moveit_config/scripts/pickplace.py

@@ -30,14 +30,14 @@
 # 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.55
 objectPose.pose.position.y = -0.25
-objectPose.pose.position.z = 0.7
+objectPose.pose.position.z = 0.82
 # [initCollisionObject]
 
 # Add the grasp object to the planning scene
-psi.add_cylinder(object_name, objectPose, height=0.25, radius=0.02)
+psi.add_cylinder(object_name, objectPose, height=0.2, radius=0.01)
 # [pickAndPlaceTut2]
 
 # [pickAndPlaceTut3]
@@ -54,15 +54,14 @@
 # Create a planner instance that is used to connect
 # the current state to the grasp approach pose
 pipeline = core.PipelinePlanner()
-pipeline.planner = "RRTConnectkConfigDefault"
+pipeline.planner = "RRTConnect"
 planners = [(arm, pipeline)]
 
 # Connect the two stages
 task.add(stages.Connect("connect1", 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")
@@ -118,66 +117,18 @@
 
 # [pickAndPlaceTut9]
 # Connect the Pick stage with the following Place stage
-task.add(stages.Connect("connect2", planners))
-# [pickAndPlaceTut9]
 
-# [pickAndPlaceTut10]
-# [initAndConfigGeneratePlacePose]
-# Define the pose that the object should have after placing
-placePose = objectPose
-placePose.pose.orientation.x = 1
-placePose.pose.position.x += -0.1
-placePose.pose.position.y += 0.15  # shift object by 20cm along y axis
-
-# Generate Cartesian place poses for the object
-place_generator = stages.GeneratePlacePose("Generate Place Pose")
-place_generator.setMonitoredStage(task["Pick"])
-place_generator.object = object_name
-place_generator.pose = placePose
-# [initAndConfigGeneratePlacePose]
-# [pickAndPlaceTut10]
-
-# [initAndConfigSimpleUnGrasp]
-# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
-# [pickAndPlaceTut11]
-simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
-# [pickAndPlaceTut11]
-
-# [pickAndPlaceTut12]
-# [initAndConfigPlace]
-# Place container comprises placing, ungrasping, and retracting
-place = stages.Place(simpleUnGrasp, "Place")
-place.eef = eef
-place.object = object_name
-place.eef_frame = "qbhand_base_link"
-#place.eef_frame = "panda_link8"
-# [initAndConfigSimpleUnGrasp]
-
-# Twist to retract from the object
-retract = TwistStamped()
-retract.header.frame_id = "world"
-retract.twist.linear.y = 1.0
-place.setRetractMotion(retract, 0.00, 0.25) #(retract, 0.03, 0.1)
-
-# Twist to place the object
-placeMotion = TwistStamped()
-placeMotion.header.frame_id = "qbhand_base_link"
-placeMotion.twist.linear.y = 1.0
-place.setPlaceMotion(placeMotion, 0.00, 0.25) #(placeMotion, 0.03, 0.1)
-
-# Add the place pipeline to the task's hierarchy
-task.add(place)
-# [initAndConfigPlace]
-# [pickAndPlaceTut12]
-
-# [pickAndPlaceTut13]
 if task.plan():
-    task.publish(task.solutions[0])
+    try:
+        task.publish(task.solutions[0])
+    except:
+        print("Can't publish task")    
+
 
 # avoid ClassLoader warning
 del pipeline
 del planners
 # [pickAndPlaceTut13]
 
 # Prevent the program from exiting, giving you the opportunity to inspect solutions in rviz
-time.sleep(3600)
+time.sleep(120)

moveit_task_constructor/capabilities/CHANGELOG.rst

@@ -0,0 +1,14 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package moveit_task_constructor_capabilities
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.2 (2023-02-24)
+------------------
+
+0.1.1 (2023-02-15)
+------------------
+
+0.1.0 (2023-02-02)
+------------------
+* Initial release
+* Contributors: Michael Görner, Robert Haschke

moveit_task_constructor/core/CHANGELOG.rst

@@ -0,0 +1,30 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package moveit_task_constructor_core
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.2 (2023-02-24)
+------------------
+* Remove moveit/__init__.py during .deb builds to avoid installation conflicts
+* MultiPlanner solver (`#429 <https://github.com/ros-planning/moveit_task_constructor/issues/429>`_): a planner that tries multiple planners in sequence
+
+  * CartesianPath: Deprecate redundant property setters
+  * PlannerInterface: provide "timeout" property
+  * PlannerInterface: provide setters for properties
+* JointInterpolation: fix timeout handling
+* Contributors: Robert Haschke
+
+0.1.1 (2023-02-15)
+------------------
+* Resort to MoveIt's python tools
+* Provide ComputeIK.ik_frame as full PoseStamped
+* Fixed build farm issues
+
+  * Removed unused eigen_conversions includes
+  * Fixed odr compiler warning on Buster
+  * Fixed missing dependency declarations
+* Contributors: Robert Haschke
+
+0.1.0 (2023-02-02)
+------------------
+* Initial release
+* Contributors: Michael Görner, Robert Haschke, Captain Yoshi, Christian Petersmeier, Henning Kayser, Jafar Abdi, Tyler Weaver

moveit_task_constructor/core/CMakeLists.txt

@@ -25,7 +25,6 @@ catkin_package(
 		${PROJECT_NAME}
 		${PROJECT_NAME}_stages
 		${PROJECT_NAME}_stage_plugins
-		moveit_python_tools
 	INCLUDE_DIRS
 		include
 	CATKIN_DEPENDS

moveit_task_constructor/core/include/moveit/python/task_constructor/properties.h

@@ -1,11 +1,14 @@
 #pragma once
 
-#include <moveit/python/python_tools/ros_types.h>
-#include <moveit/python/python_tools/geometry_msg_types.h>
+#include <pybind11/smart_holder.h>
+#include <moveit/python/pybind_rosmsg_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 {
 

moveit_task_constructor/core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -56,8 +56,10 @@ class CartesianPath : public PlannerInterface
 	void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
 	void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
 
-	void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); }
-	void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); }
+	[[deprecated("Replace with setMaxVelocityScalingFactor")]]  // clang-format off
+	void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); }
+	[[deprecated("Replace with setMaxAccelerationScalingFactor")]]  // clang-format off
+	void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); }
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 

moveit_task_constructor/core/include/moveit/task_constructor/solvers/multi_planner.h

@@ -0,0 +1,77 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, Bielefeld University
+ *  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 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: Robert Haschke
+   Desc:    meta planner, running multiple planners in parallel or sequence
+*/
+
+#pragma once
+
+#include <moveit/task_constructor/solvers/planner_interface.h>
+#include <vector>
+
+namespace moveit {
+namespace task_constructor {
+namespace solvers {
+
+MOVEIT_CLASS_FORWARD(MultiPlanner);
+
+/** A meta planner that runs multiple alternative planners in sequence and returns the first found solution.
+ *
+ * This is useful to sequence different planning strategies of increasing complexity,
+ * e.g. Cartesian or joint-space interpolation first, then OMPL, ...
+ * This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each
+ * individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before
+ * switching to the next child, which possibly applies a different planning strategy.
+ */
+class MultiPlanner : public PlannerInterface, public std::vector<solvers::PlannerInterfacePtr>
+{
+public:
+	using PlannerList = std::vector<solvers::PlannerInterfacePtr>;
+	using PlannerList::PlannerList;  // inherit all std::vector constructors
+
+	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
+
+	bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	          const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	          const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
+
+	bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
+	          double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	          const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
+};
+}  // namespace solvers
+}  // namespace task_constructor
+}  // namespace moveit

moveit_task_constructor/core/include/moveit/task_constructor/solvers/planner_interface.h

@@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningScene);
 namespace robot_trajectory {
 MOVEIT_CLASS_FORWARD(RobotTrajectory);
 }
+namespace trajectory_processing {
+MOVEIT_CLASS_FORWARD(TimeParameterization);
+}
 namespace moveit {
 namespace core {
 MOVEIT_CLASS_FORWARD(LinkModel);
@@ -75,6 +78,12 @@ class PlannerInterface
 	const PropertyMap& properties() const { return properties_; }
 
 	void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); }
+	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 setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
+		properties_.set("time_parameterization", tp);
+	}
 
 	virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
 

moveit_task_constructor/core/package.xml

@@ -1,6 +1,6 @@
 <package format="3">
 	<name>moveit_task_constructor_core</name>
-	<version>0.0.0</version>
+	<version>0.1.2</version>
 	<description>MoveIt Task Pipeline</description>
 
 	<url type="website">https://github.com/ros-planning/moveit_task_constructor</url>
@@ -31,6 +31,8 @@
 	<test_depend>rosunit</test_depend>
 	<test_depend>rostest</test_depend>
 	<test_depend>moveit_resources_fanuc_moveit_config</test_depend>
+	<test_depend>moveit_planners</test_depend>
+	<test_depend>moveit_commander</test_depend>
 
 	<export>
 		<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml" />

moveit_task_constructor/core/python/CMakeLists.txt

@@ -21,9 +21,8 @@ set(PYBIND11_CMAKECONFIG_INSTALL_DIR ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake
 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
-	                COMMAND git submodule update
-	                WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
+	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)