Merge branch master into ros2

This commit is contained in:
Robert Haschke 2023-05-26 01:27:51 +02:00
commit 349e3c6c6c
76 changed files with 832 additions and 473 deletions

41
.github/workflows/prerelease.yaml vendored Normal file
View File

@ -0,0 +1,41 @@
# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
name: pre-release
on:
workflow_dispatch:
permissions:
contents: read # to fetch code (actions/checkout)
jobs:
default:
strategy:
matrix:
distro: [noetic]
env:
# https://github.com/ros-industrial/industrial_ci/issues/666
BUILDER: catkin_make_isolated
ROS_DISTRO: ${{ matrix.distro }}
PRERELEASE: true
BASEDIR: ${{ github.workspace }}/.work
name: "${{ matrix.distro }}"
runs-on: ubuntu-latest
steps:
- name: "Free up disk space"
run: |
sudo apt-get -qq purge build-essential "ghc*"
sudo apt-get clean
# cleanup docker images not used by us
docker system prune -af
# free up a lot of stuff from /usr/local
sudo rm -rf /usr/local
df -h
- uses: actions/checkout@v3
with:
submodules: recursive
- name: industrial_ci
uses: ros-industrial/industrial_ci@master

2
.gitmodules vendored
View File

@ -1,5 +1,5 @@
[submodule "core/python/pybind11"]
path = core/python/pybind11
url = https://github.com/rhaschke/pybind11
url = https://github.com/pybind/pybind11
branch = smart_holder
shallow = true

View File

@ -2,6 +2,15 @@
Changelog for package moveit_task_constructor_capabilities
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
0.1.2 (2023-02-24)
------------------
0.1.1 (2023-02-15)
------------------
0.1.0 (2023-02-02)
------------------
* Initial release

View File

@ -1,6 +1,6 @@
<package format="2">
<name>moveit_task_constructor_capabilities</name>
<version>0.1.0</version>
<version>0.1.3</version>
<description>
MoveGroupCapabilites to interact with MoveIt
</description>

View File

@ -193,5 +193,5 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
} // namespace move_group
#include <class_loader/class_loader.hpp>
CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)

View File

@ -2,6 +2,35 @@
Changelog for package moveit_task_constructor_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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>`_)
* ComputeIK: Limit collision checking to JMG (`#428 <https://github.com/ros-planning/moveit_task_constructor/issues/428>`_)
* Fix: Fetch pybind11 submodule if not yet present
* Contributors: Robert Haschke
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

View File

@ -1,30 +1,47 @@
Basic Concepts
==============
The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stages relating to the result flow: generator, propagator, and connector stages:
The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stage types w.r.t. their data flow: generators, propagators, and connector stages:
.. glossary::
Generators
compute their results independently of their neighbor stages and pass them in both directions, backwards and forwards. An example is an IK sampler for geometric poses where approaching and departing motions (neighbor stages) depend on the solution.
compute their results independently of their neighboring stages and pass them in both directions, backwards and forwards. Examples include pose generators, e.g. for grasping or placing, as well as ``ComputeIK``, which computes IK solutions for Cartesian targets. neighboring stages can continue processing from the generated states. The most important generator stage, however, is ``CurrentState``, which provides the current robot state as the starting point for a planning pipeline.
Propagators
receive the result of one neighbor stage, solve a subproblem and then propagate their result to the neighbor on the opposite site. Depending on the implementation, propagating stages can pass solutions forward, backward or in both directions separately. An example is a stage that computes a Cartesian path based on either a start or a goal state.
receive the result of *one* neighboring stage as input, plan towards a goal state, and then propagate their result to the opposite interface site. Propagating stages can receive their input on either interface, begin or end. The flow of information (forwards or backwards) is determined by the input interface of the stage. An example is a stage that computes a Cartesian path based on either a start or a goal state.
Connectors
do not propagate any results, but rather attempt to bridge the gap between the resulting states of both neighbors. An example is the computation of a free-motion plan from one given state to another.
do not propagate any results, but rather attempt to bridge the gap between the resulting states of both its neighboring stages. It receives input states from both, the begin and end interface and attempts to connect them via a suitable motion plan. Obviously, any pair of input states needs to be *compatible*, i.e. their states (including collision and attached objects as well as joint poses) need to match except for those joints that are part of the given planning group.
Additional to the order types, there are different hierarchy types allowing to encapsulate subordinate stages. Stages without subordinate stages are called primitive stages, higher-level stages are called container stages. There are three container types:
Processing starts from generator stages, expands via propagators, and finally connects partial solution sequences via connector stages.
Obviously, there exist limitations on how stages can be connected to each other. For example, two generator stages cannot occur in sequence as they would both attempt to *write* into their interfaces, but non of them is actually *reading*. Same applies for two connectors in a row: they would both attempt to read, while no stage is actually writing.
The compatibility of stages is automatically checked once before planning by ``Task::init()``.
To compose a planning pipeline from multiple seemingly independent parts, for example grasping an object and placing it at a new location, one needs to break the typical linear pipeline structure: the place pose is another generator stage (additionally to the ``CurrentState`` stage we are starting from) serving as a seed for the placing sub solution. However, this stage is not completely independent from the grasping sub solution: it should continue where grasping left off, namely with the grasped object attached to the gripper. To convey this state information, the place pose generator should inherit from ``MonitoringGenerator``, which monitors the solutions of another stage in the pipeline in fast-forwards them for further processing in the dependent stage.
In order to hierarchically organize planning pipelines and to allow for reuse of sub pipelines, e.g. for grasping or placing, one can encapsulate stages within various containers.
Stages without children are called primitive stages. We provide three types of containers:
.. glossary::
Wrappers
encapsulate a single subordinate stage and modify or filter the results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property.
encapsulate a single subordinate stage and modify or filter its results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property.
Serial Containers
Serial containers
hold a sequence of subordinate stages and only consider end-to-end solutions as results. An example is a picking motion that consists of a sequence of coherent steps.
Parallel Containers
combine set of subordinate stages and can be used for passing the best of alternative results, running fallback solvers or for merging multiple independent solutions. Examples are running alternative planners for a free-motion plan, picking objects with the right hand or with the left hand as a fallback, or moving the arm and opening the gripper at the same time.
Parallel containers
consider the solutions of all their children as feasible. Different sub types are available, namely:
``Alternative`` container
processes all children simultaneously (currently in a round-robin fashion). For example, one could plan a grasping sequence for a left and right arm in parallel if the actual choice of the arm doesn't matter for the task.
``Fallback`` container
processes their children sequentially: only if the current child has exhausted all its solution candidates (and didn't produce any feasible solution), the next child is considered.
Use this container, e.g. if you prefer grasping with the right arm, but allow falling back to the left if really needed.
``Merger`` container
processes their children simultaneously and combine their solutions into an joint solution. It is assumed that children operate on disjoint joint model groups, e.g. the arm and the hand, such that their solution trajectories can be executed in parallel after being merged.
Stages not only support solving motion planning problems. They can also be used for all kinds of state transitions, as for instance modifying the planning scene. Combined with the possibility of using class inheritance it is possible to construct very complex behavior while only relying on a well-structured set of primitive stages.

View File

@ -28,3 +28,4 @@ Organization of the documentation
concepts
howto
api
troubleshooting

View File

@ -0,0 +1,31 @@
.. _sec-troubleshooting:
Troubleshooting
===============
``Task::init()`` reports mismatching interfaces
-----------------------------------------------
Before planning, the planning pipeline is checked for consistency. Each stage type has a specific flow interface, e.g. generator-type stages write into both, their begin and end interface, propagator-type stages read from one and write to the opposite, and connector-type stages read from both sides. Obviously these interfaces need to match. If they don't, an ``InitStageException`` is thrown. Per default, this is not very verbose, but you can use the following code snippet to get some more info:
.. code-block:: c
try {
task.plan();
} catch (const InitStageException& e) {
std::cerr << e << std::endl;
throw;
}
For example, the following pipeline:
- ↕ current
- ⛓ connect
- ↑ moveTo
throws the error: ``task pipeline: end interface (←) of 'moveto' does not match mine (→)``.
The validation process runs sequentially through a ``SerialContainer``. Here, ``current``, as a generator stage is compatible to ``connect``, writing to the interface read by ``connect``.
``moveTo`` as a propagator can operate, in principle, forwards and backwards. By default, the operation direction is inferred automatically. Here, as ``connect`` requires a reading end-interface, moveTo should seemingly operate backwards. However, now the whole pipeline is incompatible to the enclosing container's interface: a task requires a generator-type interface as it generates solutions - there is no input interface to read from. Hence, the *reading* end interface (←) of ``moveto`` conflicts with a *writing* end interface of the task.
Obviously, in a ``ParallelContainer`` all (direct) children need to share a common interface.

View File

@ -1,27 +0,0 @@
#pragma once
#include "ros_types.h"
#include <geometry_msgs/PoseStamped.h>
/** Convienency type casters, also allowing to initialize Stamped geometry msgs from a string */
namespace pybind11 {
namespace detail {
template <>
struct type_caster<geometry_msgs::PoseStamped> : type_caster_ros_msg<geometry_msgs::PoseStamped>
{
// Python -> C++
bool load(handle src, bool convert) {
type_caster<std::string> str_caster;
if (convert && str_caster.load(src, false)) { // string creates identity pose with given frame
value.header.frame_id = static_cast<std::string&>(str_caster);
value.pose.orientation.w = 1.0;
return true;
}
return type_caster_ros_msg<geometry_msgs::PoseStamped>::load(src, convert);
}
};
} // namespace detail
} // namespace pybind11

View File

@ -1,31 +0,0 @@
#pragma once
#include <memory>
#include <boost/thread/mutex.hpp>
#include <ros/spinner.h>
namespace moveit {
namespace python {
/// singleton class to initialize ROS C++ (once) from Python
class InitProxy
{
public:
static void init(const std::string& node_name = "moveit_python_wrapper",
const std::map<std::string, std::string>& remappings = std::map<std::string, std::string>(),
uint32_t options = 0);
static void shutdown();
~InitProxy();
private:
InitProxy(const std::string& node_name, const std::map<std::string, std::string>& remappings, uint32_t options);
static boost::mutex lock_;
static std::unique_ptr<InitProxy> singleton_instance_;
private:
std::unique_ptr<ros::AsyncSpinner> spinner;
};
} // namespace python
} // namespace moveit

View File

@ -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 {

View File

@ -76,6 +76,7 @@ public:
virtual bool canCompute() const = 0;
virtual void compute() = 0;
void explainFailure(std::ostream& os) const override;
/// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(const SolutionBase& s) = 0;

View File

@ -56,8 +56,10 @@ public:
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;

View File

@ -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::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -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 @@ public:
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;

View File

@ -232,6 +232,8 @@ public:
/// Should we generate failure solutions? Note: Always report a failure!
bool storeFailures() const;
virtual void explainFailure(std::ostream& os) const;
/// Get the stage's property map
PropertyMap& properties();
const PropertyMap& properties() const { return const_cast<Stage*>(this)->properties(); }

View File

@ -149,8 +149,9 @@ protected:
protected:
// apply stored modifications to scene
InterfaceState apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::msg::CollisionObject& object);
std::pair<InterfaceState, SubTrajectory> apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::msg::CollisionObject& object,
bool invert);
void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
bool invert);
void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);

View File

@ -66,10 +66,8 @@ public:
void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Isometry3d pose;
pose = p;
setIKFrame(pose, link);
void setIKFrame(const T& pose, const std::string& link) {
setIKFrame(Eigen::Isometry3d(pose), link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }

View File

@ -312,9 +312,11 @@ public:
auto& markers() { return markers_; }
const auto& markers() const { return markers_; }
/// convert solution to message
void toMsg(moveit_task_constructor_msgs::msg::Solution& solution, Introspection* introspection = nullptr) const;
/// append this solution to Solution msg
virtual void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection = nullptr) const = 0;
virtual void appendTo(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection = nullptr) const = 0;
void fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection = nullptr) const;
/// required to dispatch to type-specific CostTerm methods via vtable
@ -355,8 +357,8 @@ public:
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg,
Introspection* introspection = nullptr) const override;
void appendTo(moveit_task_constructor_msgs::msg::Solution& msg,
Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -389,7 +391,7 @@ public:
void push_back(const SolutionBase& solution);
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const override;
void appendTo(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -420,8 +422,8 @@ public:
: SolutionBase(creator, cost), wrapped_(wrapped) {}
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
: WrappedSolution(creator, wrapped, wrapped->cost()) {}
void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection = nullptr) const override;
void appendTo(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;

View File

@ -87,6 +87,9 @@ public:
const std::string& name() const { return stages()->name(); }
void setName(const std::string& name) { stages()->setName(name); }
Stage* findChild(const std::string& name) const { return stages()->findChild(name); }
Stage* operator[](int index) const { return stages()->operator[](index); }
const moveit::core::RobotModelConstPtr& getRobotModel() const;
/// setting the robot model also resets the task
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
@ -131,6 +134,9 @@ public:
/// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream& os = std::cout) const;
/// print an explanation for a planning failure to os
void explainFailure(std::ostream& os = std::cout) const override;
size_t numSolutions() const { return solutions().size(); }
const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
const std::list<SolutionBaseConstPtr>& failures() const { return stages()->failures(); }

View File

@ -1,6 +1,6 @@
<package format="3">
<name>moveit_task_constructor_core</name>
<version>0.1.0</version>
<version>0.1.3</version>
<description>MoveIt Task Pipeline</description>
<url type="website">https://github.com/ros-planning/moveit_task_constructor</url>
@ -34,6 +34,7 @@
<test_depend>launch_testing_ament_cmake</test_depend>
<!-- test_depend>launch_testing_ros</test_depend -->
<!-- test_depend>moveit_resources_fanuc_moveit_config</test_depend -->
<test_depend>moveit_planners</test_depend>
<export>
<build_type>ament_cmake</build_type>

View File

@ -16,9 +16,8 @@ set(PYBIND11_CMAKECONFIG_INSTALL_DIR share/${PROJECT_NAME}/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()
add_subdirectory(pybind11)

View File

@ -124,9 +124,9 @@ void export_core(pybind11::module& m) {
":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)")
.def(
"toMsg",
[](const SolutionBasePtr& s) {
moveit_task_constructor_msgs::Solution msg;
s->fillMessage(msg);
[](const SolutionBase& self) {
moveit_task_constructor_msgs::msg::Solution msg;
self.toMsg(msg);
return msg;
},
"Convert to the ROS message ``Solution``");
@ -175,7 +175,7 @@ void export_core(pybind11::module& m) {
.def(py::init<std::map<std::string, double>>());
py::classh<cost::DistanceToReference, TrajectoryCostTerm>(m, "DistanceToReference",
"Computes joint-based distance to reference pose")
.def(py::init<const moveit_msgs::RobotState&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
.def(py::init<const moveit_msgs::msg::RobotState&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
"reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>())
.def(py::init<const std::map<std::string, double>&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
"reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>());
@ -440,6 +440,7 @@ void export_core(pybind11::module& m) {
t.add(it->cast<Stage::pointer>());
},
"Append stage(s) to the task's top-level container")
.def("insert", &Task::insert, "stage"_a, "before"_a = -1, "Insert stage before given index")
.def("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def(
"__getitem__",
@ -493,10 +494,10 @@ void export_core(pybind11::module& m) {
MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]);
MoveGroupInterface::Plan plan;
moveit_task_constructor_msgs::Solution serialized;
solution->fillMessage(serialized);
moveit_task_constructor_msgs::msg::Solution serialized;
solution->toMsg(serialized);
for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) {
for (const moveit_task_constructor_msgs::msg::SubTrajectory& traj : serialized.sub_trajectory) {
if (!traj.trajectory.joint_trajectory.points.empty()) {
plan.trajectory_ = traj.trajectory;
if (!mgi.execute(plan)) {

View File

@ -109,9 +109,6 @@ public:
} // namespace task_constructor
} // namespace moveit
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::solvers::PlannerInterface)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SolutionBase)

View File

@ -39,9 +39,6 @@ namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap)
namespace moveit {
namespace python {
namespace {

View File

@ -1,53 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, 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.
*********************************************************************/
#include <pybind11/smart_holder.h>
#include <pybind11/stl.h>
#include <moveit/python/python_tools/ros_init.h>
#include <ros/init.h>
namespace py = pybind11;
using namespace moveit::python;
PYBIND11_MODULE(pymoveit_python_tools, m) {
m.doc() = "MoveIt python tools";
m.def("roscpp_init", &InitProxy::init, "Initialize C++ ROS", py::arg("node_name") = "moveit_python_wrapper",
py::arg("remappings") = std::map<std::string, std::string>(), py::arg("options") = 0);
m.def("roscpp_shutdown", &InitProxy::shutdown, "Shutdown C++ ROS");
py::enum_<ros::InitOption>(m, "InitOption")
.value("AnonymousName", ros::init_options::AnonymousName)
.value("NoRosout", ros::init_options::NoRosout);
}

View File

@ -1,68 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, 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.
*********************************************************************/
#include <moveit/python/python_tools/ros_init.h>
#include <ros/init.h>
namespace moveit {
namespace python {
boost::mutex InitProxy::lock_;
std::unique_ptr<InitProxy> InitProxy::singleton_instance_;
void InitProxy::init(const std::string& node_name, const std::map<std::string, std::string>& remappings,
uint32_t options) {
boost::mutex::scoped_lock slock(lock_);
if (!singleton_instance_ && !ros::isInitialized())
singleton_instance_.reset(new InitProxy(node_name, remappings, options));
}
void InitProxy::shutdown() {
boost::mutex::scoped_lock slock(lock_);
singleton_instance_.reset();
}
InitProxy::InitProxy(const std::string& node_name, const std::map<std::string, std::string>& remappings,
uint32_t options) {
ros::init(remappings, node_name, options | ros::init_options::NoSigintHandler);
spinner.reset(new ros::AsyncSpinner(1));
spinner->start();
}
InitProxy::~InitProxy() {
spinner->stop();
spinner.reset();
}
} // namespace python
} // namespace moveit

View File

@ -1,61 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, 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.
*********************************************************************/
#include <moveit/python/python_tools/ros_types.h>
namespace py = pybind11;
namespace moveit {
namespace python {
py::object createMessage(const std::string& ros_msg_name) {
// find delimiting '/' in ros msg name
std::size_t pos = ros_msg_name.find('/');
// import module
py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str());
// retrieve type instance
py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str());
// create message instance
return cls();
}
bool convertible(const pybind11::handle& h, const char* ros_msg_name) {
try {
PyObject* o = h.attr("_type").ptr();
return py::cast<std::string>(o) == ros_msg_name;
} catch (const std::exception& e) {
return false;
}
}
} // namespace python
} // namespace moveit

View File

@ -39,6 +39,7 @@
#include <moveit_msgs/WorkspaceParameters.h>
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
using namespace moveit::task_constructor::solvers;
@ -71,7 +72,7 @@ void export_solvers(py::module& m) {
)")
.property<std::string>("planner", "str: Planner ID")
.property<uint>("num_planning_attempts", "int: Number of planning attempts")
.property<moveit_msgs::WorkspaceParameters>(
.property<moveit_msgs::msg::WorkspaceParameters>(
"workspace_parameters",
":moveit_msgs:`WorkspaceParameters`: Specifies workspace box to be used for Cartesian sampling")
.property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
@ -79,7 +80,7 @@ void export_solvers(py::module& m) {
.property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
.property<bool>("display_motion_plans", "bool: Publish generated solutions via a topic")
.property<bool>("publish_planning_requests", "bool: Publish motion planning requests via a topic")
.def(py::init<>());
.def(py::init<const std::string&>(), "pipeline"_a = std::string("ompl"));
properties::class_<JointInterpolationPlanner, PlannerInterface>(
m, "JointInterpolationPlanner",

View File

@ -40,6 +40,7 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/PlanningScene.h>
#include <pybind11/stl.h>
#include <moveit/python/pybind_rosmsg_typecasters.h>
namespace py = pybind11;
using namespace py::literals;
@ -104,6 +105,8 @@ void export_stages(pybind11::module& m) {
const std::string& attach_link) {
self.attachObjects(elementOrList<std::string>(names), attach_link, false);
}, "Detach multiple objects from a robot link", "names"_a, "attach_link"_a)
.def("allowCollisions", [](ModifyPlanningScene& self, const std::string& object, bool allow) {self.allowCollisions(object, allow);},
"Allow or disable all collisions involving the given object", "object"_a, "enable_collision"_a = true)
.def("allowCollisions", [](ModifyPlanningScene& self,
const py::object& first, const py::object& second, bool enable_collision) {
self.allowCollisions(elementOrList<std::string>(first), elementOrList<std::string>(second), enable_collision);
@ -113,10 +116,14 @@ void export_stages(pybind11::module& m) {
.. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html
)", "collision_object"_a);
)", "collision_object"_a)
.def("removeObject", &ModifyPlanningScene::removeObject,
"Remove a CollisionObject_ from the planning scene", "name"_a)
;
properties::class_<CurrentState, Stage>(m, "CurrentState", R"(
Fetch the current PlanningScene state via the ``get_planning_scene`` service.
Fetch the current PlanningScene / real robot state via the ``get_planning_scene`` service.
Usually, this stage should be used *once* at the beginning of your pipeline to start from the current state.
.. literalinclude:: ./../../../demo/scripts/current_state.py
:language: python
@ -188,14 +195,14 @@ void export_stages(pybind11::module& m) {
bool: Specify if collisions with other members of
the planning scene are allowed.
)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
.property<geometry_msgs::msg::PoseStamped>("ik_frame", R"(
PoseStamped_: Specify the frame with respect
to which the inverse kinematics
should be calculated.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.property<geometry_msgs::PoseStamped>("target_pose", R"(
.property<geometry_msgs::msg::PoseStamped>("target_pose", R"(
PoseStamped_: Specify the pose on which
the inverse kinematics should be
calculated on. Since this property should
@ -220,29 +227,29 @@ void export_stages(pybind11::module& m) {
.property<std::string>("group", R"(
str: Planning group which should be utilized for planning and execution.
)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
.property<geometry_msgs::msg::PoseStamped>("ik_frame", R"(
PoseStamped_: IK reference frame for the goal pose
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.property<moveit_msgs::Constraints>("path_constraints", R"(
.property<moveit_msgs::msg::Constraints>("path_constraints", R"(
Constraints_: Set path constraints via the corresponding moveit message type
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
)")
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>(), "name"_a, "planner"_a)
.def("setGoal", py::overload_cast<const geometry_msgs::PoseStamped&>(&MoveTo::setGoal), R"(
.def("setGoal", py::overload_cast<const geometry_msgs::msg::PoseStamped&>(&MoveTo::setGoal), R"(
Move link to a given PoseStamped_
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)", "goal"_a)
.def("setGoal", py::overload_cast<const geometry_msgs::PointStamped&>(&MoveTo::setGoal), R"(
.def("setGoal", py::overload_cast<const geometry_msgs::msg::PointStamped&>(&MoveTo::setGoal), R"(
Move link to given PointStamped_, keeping current orientation
.. _PointStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html
)", "goal"_a)
.def("setGoal", py::overload_cast<const moveit_msgs::RobotState&>(&MoveTo::setGoal), R"(
.def("setGoal", py::overload_cast<const moveit_msgs::msg::RobotState&>(&MoveTo::setGoal), R"(
Move joints specified in RobotState_ to their target values
.. _RobotState: https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotState.html
@ -266,25 +273,25 @@ void export_stages(pybind11::module& m) {
.property<std::string>("group", R"(
str: Planning group which should be utilized for planning and execution.
)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
.property<geometry_msgs::msg::PoseStamped>("ik_frame", R"(
PoseStamped_: IK reference frame for the goal pose.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.property<double>("min_distance", "float: Set the minimum distance to move")
.property<double>("max_distance", "float: Set the maximum distance to move")
.property<moveit_msgs::Constraints>("path_constraints", R"(
.property<moveit_msgs::msg::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 solvers::PlannerInterfacePtr&>(), "name"_a, "planner"_a)
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&MoveRelative::setDirection), R"(
.def("setDirection", py::overload_cast<const geometry_msgs::msg::TwistStamped&>(&MoveRelative::setDirection), R"(
Perform twist motion on specified link.
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "twist"_a)
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection), R"(
.def("setDirection", py::overload_cast<const geometry_msgs::msg::Vector3Stamped&>(&MoveRelative::setDirection), R"(
Translate link along given direction.
.. _Vector3Stamped: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Vector3Stamped.html
@ -345,7 +352,7 @@ void export_stages(pybind11::module& m) {
str: Name of the object in the planning scene, attached to the robot which should be placed
)")
.property<std::string>("eef", "str: Name of the end effector that should be used for grasping")
.property<geometry_msgs::PoseStamped>("pose", R"(
.property<geometry_msgs::msg::msg::PoseStamped>("pose", R"(
PoseStamped_: The pose where the object should be placed, i.e. states should be sampled
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
@ -381,7 +388,7 @@ void export_stages(pybind11::module& m) {
for an implementation of a task hierarchy that makes use of the
``GeneratePose`` stage.
)")
.property<geometry_msgs::PoseStamped>("pose", R"(
.property<geometry_msgs::msg::msg::msg::PoseStamped>("pose", R"(
PoseStamped_: Set the pose, which should be spawned on each new solution of the monitored stage.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
@ -421,7 +428,7 @@ void export_stages(pybind11::module& m) {
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "motion"_a, "min_distance"_a, "max_distance"_a)
.def("setLiftMotion", py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Pick::setLiftMotion), R"(
.def("setLiftMotion", py::overload_cast<const geometry_msgs::msg::TwistStamped&, double, double>(&Pick::setLiftMotion), R"(
The lifting motion away from the grasping state is represented by a twist message.
Additionally specify the minimum and maximum allowed distances to travel.
@ -462,7 +469,7 @@ void export_stages(pybind11::module& m) {
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "motion"_a, "min_distance"_a, "max_distance"_a)
.def("setPlaceMotion", py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Place::setPlaceMotion), R"(
.def("setPlaceMotion", py::overload_cast<const geometry_msgs::msg::TwistStamped&, double, double>(&Place::setPlaceMotion), R"(
The object-placing motion towards the final state is represented by a twist message.
Additionally specify the minimum and maximum allowed distances to travel.
@ -477,13 +484,13 @@ void export_stages(pybind11::module& m) {
properties::class_<SimpleGraspBase, SerialContainer>(m, "SimpleGraspBase", "Abstract base class for grasping and releasing")
.property<std::string>("eef", "str: The end effector of the robot")
.property<std::string>("object", "str: The object to grasp (Must be present in the planning scene)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
.property<geometry_msgs::msg::PoseStamped>("ik_frame", R"(
PoseStamped_: Set the frame for which the inverse kinematics is calculated
with respect to each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.def<void (SimpleGraspBase::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
.def<void (SimpleGraspBase::*)(const geometry_msgs::msg::PoseStamped&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame as a PoseStamped_ for which the inverse kinematics are calculated with respect to
each pose generated by the pose_generator.

117
core/python/test/rostest_mps.py Executable file
View File

@ -0,0 +1,117 @@
#! /usr/bin/env python
from __future__ import print_function
import unittest
import rostest
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit_commander import PlanningSceneInterface
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
def make_pose(x, y, z):
pose = PoseStamped()
pose.header.frame_id = "base_link"
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.w = 1.0
return pose
class TestModifyPlanningScene(unittest.TestCase):
PLANNING_GROUP = "manipulator"
def setUp(self):
super(TestModifyPlanningScene, self).setUp()
self.psi = PlanningSceneInterface(synchronous=True)
self.make_box = self.psi._PlanningSceneInterface__make_box
# insert a box to collide with
self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2])
self.task = task = core.Task()
task.add(stages.CurrentState("current"))
def insertMove(self, position=-1):
# colliding motion
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP
move.setDirection({"joint_1": 0.3})
self.task.insert(move, position)
def test_collision(self):
self.insertMove()
self.assertFalse(self.task.plan())
def test_allow_collision_list(self):
mps = stages.ModifyPlanningScene("allow specific collisions for box")
mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True)
self.task.add(mps)
self.insertMove()
self.assertTrue(self.task.plan())
def test_allow_collision_all(self):
# insert an extra collision object that is unknown to ACM
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# attach box to end effector
self.psi.attach_box("link_6", "box")
self.insertMove()
self.assertFalse(self.task.plan())
# allow all collisions for attached "box" object
mps = stages.ModifyPlanningScene("allow all collisions for box")
mps.allowCollisions("box", True)
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
# restore original state
self.psi.remove_attached_object("link_6", "box")
self.psi.remove_world_object("block")
def test_fw_add_object(self):
mps = stages.ModifyPlanningScene("addObject(block)")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.add(mps)
self.insertMove()
self.assertFalse(self.task.plan())
def test_fw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box)")
mps.removeObject("box")
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
s = self.task.solutions[0].toMsg()
self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box")
def DISABLED_test_bw_add_object(self):
# add object to move_group's planning scene
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# backward operation will actually remove the object
mps = stages.ModifyPlanningScene("addObject(block) backwards")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.insert(mps, 0)
self.insertMove(0)
self.assertTrue(self.task.plan())
self.psi.remove_world_object("block") # restore original state
s = self.task.solutions[0].toMsg()
# block shouldn't be in start scene
objects = [o.id for o in s.start_scene.world.collision_objects]
self.assertTrue(objects == ["box"])
# only addObject(block) should add it
objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects]
self.assertTrue(objects == ["block", "box"])
def DISABLED_test_bw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box) backwards")
mps.removeObject("box")
self.task.insert(mps, 0)
self.insertMove(0)
self.assertFalse(self.task.plan())
if __name__ == "__main__":
roscpp_initialize("test_mtc")
rostest.rosrun("mtc", "mps", TestModifyPlanningScene)

View File

@ -3,7 +3,7 @@
from __future__ import print_function
import unittest
import rostest
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped, Pose
from geometry_msgs.msg import Vector3Stamped, Vector3
@ -14,31 +14,21 @@ import rospy
class Test(unittest.TestCase):
PLANNING_GROUP = "manipulator"
@classmethod
def setUpClass(self):
pass
def test_MoveAndExecute(self):
moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner())
moveRel.group = self.PLANNING_GROUP
moveRel.setDirection({"joint_1": 0.2, "joint_2": 0.4})
@classmethod
def tearDown(self):
pass
moveTo = stages.MoveTo("moveTo", core.JointInterpolationPlanner())
moveTo.group = self.PLANNING_GROUP
moveTo.setGoal("all-zeros")
def test_MoveRelative(self):
task = core.Task()
task.add(stages.CurrentState("current"))
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP
move.setDirection({"joint_1": 0.2, "joint_2": 0.4})
task.add(move)
task.enableIntrospection()
task.init()
task.plan()
task.add(stages.CurrentState("current"), moveRel, moveTo)
self.assertTrue(task.plan())
self.assertEqual(len(task.solutions), 1)
for s in task.solutions:
print(s)
s = task.solutions[0]
task.execute(s)
task.execute(task.solutions[0])
def test_Merger(self):
cartesian = core.CartesianPath()
@ -65,5 +55,5 @@ class Test(unittest.TestCase):
if __name__ == "__main__":
roscpp_init("test_mtc")
roscpp_initialize("test_mtc")
rostest.rosrun("mtc", "base", Test)

View File

@ -1,5 +1,6 @@
<launch>
<include file="$(find moveit_resources_fanuc_moveit_config)/launch/test_environment.launch" />
<test pkg="moveit_task_constructor_core" type="rostest_mtc.py" test-name="rostest_mtc" time-limit="60" args="" />
<test pkg="moveit_task_constructor_core" type="rostest_mps.py" test-name="rostest_mps" time-limit="60" args="" />
<test pkg="moveit_task_constructor_core" type="rostest_trampoline.py" test-name="rostest_trampoline" time-limit="60" args="" />
</launch>

View File

@ -4,10 +4,10 @@
from __future__ import print_function
import unittest
import rostest
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.task_constructor import core, stages
from moveit.core.planning_scene import PlanningScene
from geometry_msgs.msg import Vector3Stamped, Vector3
from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped
from std_msgs.msg import Header
PLANNING_GROUP = "manipulator"
@ -75,7 +75,7 @@ class PyMoveRelX(stages.MoveRelative):
def __init__(self, x, planner, name="Move ±x"):
stages.MoveRelative.__init__(self, name, planner)
self.group = PLANNING_GROUP
self.ik_frame = "tool0"
self.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
self.setDirection(
Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0))
)
@ -131,5 +131,5 @@ class TestTrampolines(unittest.TestCase):
if __name__ == "__main__":
roscpp_init("test_mtc")
roscpp_initialize("test_mtc")
rostest.rosrun("mtc", "trampoline", TestTrampolines)

26
core/setup.py Normal file
View File

@ -0,0 +1,26 @@
#!/usr/bin/env python
from setuptools import find_packages, setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
# list of packages to setup
packages=find_packages("python/src"),
# specify location of root ("") package dir
package_dir={"": "python/src"},
)
dist = setup(**d)
# Remove moveit/__init__.py when building .deb packages
# Otherwise, the installation procedure will complain about conflicting files (with moveit_core)
try:
# installation dir (.../lib/python3/dist-packages)
libdir = dist.command_obj["install_lib"].install_dir
if "/debian/ros-" in libdir and "moveit-task-constructor-core/opt/ros/" in libdir:
import os
import shutil
os.remove(os.path.join(libdir, "moveit", "__init__.py"))
shutil.rmtree(os.path.join(libdir, "moveit", "__pycache__"))
except AttributeError as e:
pass

View File

@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} SHARED
${PROJECT_INCLUDE}/solvers/cartesian_path.h
${PROJECT_INCLUDE}/solvers/joint_interpolation.h
${PROJECT_INCLUDE}/solvers/pipeline_planner.h
${PROJECT_INCLUDE}/solvers/multi_planner.h
container.cpp
cost_terms.cpp
@ -32,8 +33,9 @@ add_library(${PROJECT_NAME} SHARED
solvers/planner_interface.cpp
solvers/cartesian_path.cpp
solvers/pipeline_planner.cpp
solvers/joint_interpolation.cpp
solvers/pipeline_planner.cpp
solvers/multi_planner.cpp
)
ament_target_dependencies(${PROJECT_NAME}
moveit_core

View File

@ -447,6 +447,20 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
throw errors;
}
void ContainerBase::explainFailure(std::ostream& os) const {
for (const auto& stage : pimpl()->children()) {
if (!stage->solutions().empty())
continue; // skip deeper traversal, this stage produced solutions
if (stage->numFailures()) {
os << stage->name() << " (0/" << stage->numFailures() << ")";
stage->explainFailure(os);
os << std::endl;
break;
}
stage->explainFailure(os); // recursively process children
}
}
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
ContainerBase::StageCallback processor = [&os](const Stage& stage, unsigned int depth) -> bool {
os << std::string(2 * depth, ' ') << *stage.pimpl() << std::endl;
@ -692,6 +706,7 @@ void SerialContainer::compute() {
}
}
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
: ContainerBasePrivate(me, name) {}

View File

@ -48,17 +48,21 @@
#include <sstream>
#include <boost/bimap.hpp>
#include <rcutils/isalnum_no_locale.h>
namespace moveit {
namespace task_constructor {
namespace {
std::string getTaskId(const TaskPrivate* task) {
static const std::string ALLOWED = "_/";
std::ostringstream oss;
char our_hostname[256] = { 0 };
gethostname(our_hostname, sizeof(our_hostname) - 1);
// Hostname could have `-` as a character but this is an invalid character in ROS so we replace it with `_`
std::replace(std::begin(our_hostname), std::end(our_hostname), '-', '_');
// Replace all invalid ROS-name chars with an underscore
std::replace_if(
our_hostname, our_hostname + strlen(our_hostname),
[](const char ch) { return !rcutils_isalnum_no_locale(ch) && ALLOWED.find(ch) == std::string::npos; }, '_');
oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast<std::size_t>(task);
return oss.str();
}
@ -186,9 +190,7 @@ void Introspection::registerSolution(const SolutionBase& s) {
}
void Introspection::fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s) {
s.fillMessage(msg, this);
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
s.toMsg(msg, this);
msg.task_id = impl->task_id_;
}

View File

@ -123,6 +123,8 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa
auto element_handler = [&](const T& element) {
if (element && element->geometry) {
createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element)));
if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0)
return; // skip zero-size marker
m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose);
callback(m, name);
valid_found = true;

View File

@ -75,7 +75,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
// reach pose of forward kinematics
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
timeout, result, path_constraints);
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,

View File

@ -53,6 +53,8 @@ using namespace trajectory_processing;
JointInterpolationPlanner::JointInterpolationPlanner() {
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
// allow passing max_effort to GripperCommand actions via
p.declare<double>("max_effort", "max_effort for GripperCommand actions");
}
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
@ -97,6 +99,20 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
// set max_effort on first and last waypoint (first, because we might reverse the trajectory)
const auto& max_effort = properties().get("max_effort");
if (!max_effort.empty()) {
double effort = boost::any_cast<double>(max_effort);
for (const auto* jm : jmg->getActiveJointModels()) {
if (jm->getVariableCount() != 1)
continue;
result->getFirstWayPointPtr()->dropAccelerations();
result->getFirstWayPointPtr()->setJointEfforts(jm, &effort);
result->getLastWayPointPtr()->dropAccelerations();
result->getLastWayPointPtr()->setJointEfforts(jm, &effort);
}
}
return true;
}
@ -105,7 +121,8 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now();
timeout = std::min(timeout, properties().get<double>("timeout"));
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
auto to{ from->diff() };
@ -127,8 +144,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
}
to->getCurrentStateNonConst().update();
timeout = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
if (timeout <= 0.0)
if (std::chrono::steady_clock::now() >= deadline)
return false;
return plan(from, to, jmg, timeout, result, path_constraints);

View File

@ -0,0 +1,98 @@
/*********************************************************************
* 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: Michael Goerner, Robert Haschke
Desc: generate and validate a straight-line Cartesian path
*/
#include <moveit/task_constructor/solvers/multi_planner.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <chrono>
namespace moveit {
namespace task_constructor {
namespace solvers {
void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) {
for (const auto& p : *this)
p->init(robot_model);
}
bool MultiPlanner::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::msg::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
for (const auto& p : *this) {
if (remaining_time < 0)
return false; // timeout
if (result)
result->clear();
if (p->plan(from, to, jmg, remaining_time, result, path_constraints))
return true;
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return false;
}
bool MultiPlanner::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::msg::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
for (const auto& p : *this) {
if (remaining_time < 0)
return false; // timeout
if (result)
result->clear();
if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints))
return true;
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return false;
}
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -156,7 +156,7 @@ void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const Prope
const moveit::core::JointModelGroup* jmg, double timeout) {
req.group_name = jmg->getName();
req.planner_id = p.get<std::string>("planner");
req.allowed_planning_time = timeout;
req.allowed_planning_time = std::min(timeout, p.get<double>("timeout"));
req.start_state.is_diff = true; // we don't specify an extra start state
req.num_planning_attempts = p.get<uint>("num_planning_attempts");

View File

@ -47,6 +47,7 @@ namespace solvers {
PlannerInterface::PlannerInterface() {
auto& p = properties();
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<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());

View File

@ -428,6 +428,11 @@ bool Stage::storeFailures() const {
return pimpl()->storeFailures();
}
void Stage::explainFailure(std::ostream& os) const {
if (!failures().empty())
os << ": " << failures().front()->comment();
}
PropertyMap& Stage::properties() {
return pimpl()->properties_;
}

View File

@ -105,7 +105,7 @@ namespace {
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
moveit::core::RobotState& robot_state, Eigen::Isometry3d pose,
const moveit::core::LinkModel* link,
const moveit::core::LinkModel* link, const moveit::core::JointModelGroup* jmg = nullptr,
collision_detection::CollisionResult* collision_result = nullptr) {
// consider all rigidly connected parent links as well
const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link);
@ -136,6 +136,8 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
collision_detection::CollisionRequest req;
collision_detection::CollisionResult result;
req.contacts = (collision_result != nullptr);
if (jmg)
req.group_name = jmg->getName();
collision_detection::CollisionResult& res = collision_result ? *collision_result : result;
scene->checkCollision(req, res, robot_state, acm);
return res.collision;
@ -316,7 +318,7 @@ void ComputeIK::compute() {
collision_detection::CollisionResult collisions;
moveit::core::RobotState sandbox_state{ scene->getCurrentState() };
bool colliding =
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions);
// frames at target pose and ik frame
std::deque<visualization_msgs::msg::Marker> frame_markers;
@ -376,6 +378,7 @@ void ComputeIK::compute() {
collision_detection::CollisionResult res;
req.contacts = true;
req.max_contacts = 1;
req.group_name = jmg->getName();
scene->checkCollision(req, res, *state);
solution.feasible = ignore_collisions || !res.collision;
if (!res.contacts.empty()) {
@ -390,8 +393,10 @@ void ComputeIK::compute() {
double remaining_time = timeout();
auto start_time = std::chrono::steady_clock::now();
while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) {
if (tried_current_state_as_seed)
if (tried_current_state_as_seed) {
sandbox_state.setToRandomPositions(jmg);
sandbox_state.update();
}
tried_current_state_as_seed = true;
size_t previous = ik_solutions.size();

View File

@ -86,11 +86,13 @@ void ModifyPlanningScene::allowCollisions(const std::string& first, const moveit
}
void ModifyPlanningScene::computeForward(const InterfaceState& from) {
sendForward(from, apply(from, false), SubTrajectory());
auto result = apply(from, false);
sendForward(from, std::move(result.first), std::move(result.second));
}
void ModifyPlanningScene::computeBackward(const InterfaceState& to) {
sendBackward(apply(to, true), to, SubTrajectory());
auto result = apply(to, true);
sendBackward(std::move(result.first), to, std::move(result.second));
}
void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene,
@ -113,38 +115,55 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene,
collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst();
bool allow = invert ? !pairs.allow : pairs.allow;
if (pairs.second.empty()) {
for (const auto& name : pairs.first)
for (const auto& name : pairs.first) {
acm.setDefaultEntry(name, allow);
acm.setEntry(name, allow);
}
} else
acm.setEntry(pairs.first, pairs.second, allow);
}
// invert indicates, whether to detach instead of attach (and vice versa)
// as well as to forbid instead of allow collision (and vice versa)
InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
planning_scene::PlanningScenePtr scene = from.scene()->diff();
InterfaceState result(scene);
// add/remove objects
for (const auto& collision_object : collision_objects_)
processCollisionObject(*scene, collision_object);
InterfaceState state(scene);
SubTrajectory traj;
try {
// add/remove objects
for (auto& collision_object : collision_objects_)
processCollisionObject(*scene, collision_object, invert);
// attach/detach objects
for (const auto& pair : attach_objects_)
attachObjects(*scene, pair, invert);
// attach/detach objects
for (const auto& pair : attach_objects_)
attachObjects(*scene, pair, invert);
// allow/forbid collisions
for (const auto& pairs : collision_matrix_edits_)
allowCollisions(*scene, pairs, invert);
// allow/forbid collisions
for (const auto& pairs : collision_matrix_edits_)
allowCollisions(*scene, pairs, invert);
if (callback_)
callback_(scene, properties());
return result;
if (callback_)
callback_(scene, properties());
} catch (const std::exception& e) {
traj.markAsFailure(e.what());
}
return std::make_pair(state, traj);
}
void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
const moveit_msgs::msg::CollisionObject& object) {
const moveit_msgs::msg::CollisionObject& object, bool invert) {
const auto op = object.operation;
if (invert) {
if (op == moveit_msgs::msg::CollisionObject::ADD)
// (temporarily) change operation to REMOVE to revert adding the object
const_cast<moveit_msgs::msg::CollisionObject&>(object).operation = moveit_msgs::msg::CollisionObject::REMOVE;
else if (op == moveit_msgs::msg::CollisionObject::REMOVE)
throw std::runtime_error("cannot apply removeObject() backwards");
}
scene.processCollisionObjectMsg(object);
// restore previous operation (for next call)
const_cast<moveit_msgs::msg::CollisionObject&>(object).operation = op;
}
} // namespace stages
} // namespace task_constructor

View File

@ -85,11 +85,12 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) {
planner_->init(robot_model);
}
static bool getJointStateFromOffset(const boost::any& direction, const moveit::core::JointModelGroup* jmg,
moveit::core::RobotState& robot_state) {
static bool getJointStateFromOffset(const boost::any& direction, const Interface::Direction dir,
const moveit::core::JointModelGroup* jmg, moveit::core::RobotState& robot_state) {
try {
const auto& accepted = jmg->getActiveJointModels();
const auto& joints = boost::any_cast<std::map<std::string, double>>(direction);
double sign = dir == Interface::Direction::FORWARD ? +1.0 : -1.0;
for (const auto& j : joints) {
auto jm = robot_state.getRobotModel()->getJointModel(j.first);
if (!jm || std::find(accepted.begin(), accepted.end(), jm) == accepted.end())
@ -98,7 +99,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
if (jm->getVariableCount() != 1)
throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'");
auto index = jm->getFirstVariableIndex();
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + sign * j.second);
robot_state.enforceBounds(jm);
}
robot_state.update();
@ -195,7 +196,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false;
if (getJointStateFromOffset(direction, jmg, scene->getCurrentStateNonConst())) {
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
// plan to joint-space target
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else {
@ -287,35 +288,37 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
success =
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms();
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms();
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
double distance = 0.0;
if (use_rotation_distance) {
Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
distance = rotation.angle();
} else
distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
double distance = 0.0;
if (use_rotation_distance) {
Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
distance = rotation.angle();
} else
distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
// min_distance reached?
if (min_distance > 0.0) {
success = distance >= min_distance;
if (!success) {
char msg[100];
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
solution.setComment(msg);
// min_distance reached?
if (min_distance > 0.0) {
success = distance >= min_distance;
if (!success) {
char msg[100];
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
solution.setComment(msg);
}
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
success = true;
} else if (!success)
solution.setComment("failed to move full distance");
// visualize plan
auto ns = props.get<std::string>("marker_ns");
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
linear, distance);
}
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
success = true;
} else if (!success)
solution.setComment("failed to move full distance");
// visualize plan
auto ns = props.get<std::string>("marker_ns");
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
linear, distance);
}
}

View File

@ -206,6 +206,11 @@ void SolutionBase::markAsFailure(const std::string& msg) {
}
}
void SolutionBase::toMsg(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const {
appendTo(msg, introspection);
start()->scene()->getPlanningSceneMsg(msg.start_scene);
}
void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection) const {
info.id = introspection ? introspection->solutionId(*this) : 0;
info.cost = this->cost();
@ -218,7 +223,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& inf
std::copy(markers.begin(), markers.end(), info.markers.begin());
}
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const {
void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const {
msg.sub_trajectory.emplace_back();
moveit_task_constructor_msgs::msg::SubTrajectory& t = msg.sub_trajectory.back();
SolutionBase::fillInfo(t.info, introspection);
@ -237,8 +242,7 @@ void SolutionSequence::push_back(const SolutionBase& solution) {
subsolutions_.push_back(&solution);
}
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg,
Introspection* introspection) const {
void SolutionSequence::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const {
moveit_task_constructor_msgs::msg::SubSolution sub_msg;
SolutionBase::fillInfo(sub_msg.info, introspection);
@ -260,7 +264,7 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::msg::Solution&
msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
for (const SolutionBase* s : subsolutions_) {
size_t current = msg.sub_trajectory.size();
s->fillMessage(msg, introspection);
s->appendTo(msg, introspection);
// zero IDs of sub solutions with same creator as this
if (s->creator() == this->creator()) {
@ -277,9 +281,9 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co
return f(*this, comment);
}
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection) const {
wrapped_->fillMessage(solution, introspection);
void WrappedSolution::appendTo(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection) const {
wrapped_->appendTo(solution, introspection);
// prepend this solutions info as a SubSolution msg
moveit_task_constructor_msgs::msg::SubSolution sub_msg;

View File

@ -239,9 +239,12 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
init();
// Print state and return success if there are solutions otherwise the input error_code
const auto success_or = [this](const int32_t error_code) {
const auto success_or = [this](const int32_t error_code) -> int32_t {
if (numSolutions() > 0)
return moveit::core::MoveItErrorCode::SUCCESS;
printState();
return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code;
explainFailure();
return error_code;
};
impl->preempt_requested_ = false;
const double available_time = timeout();
@ -276,8 +279,7 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
ac->wait_for_action_server();
moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal;
s.fillMessage(goal.solution, pimpl()->introspection_.get());
s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene);
s.toMsg(goal.solution, pimpl()->introspection_.get());
moveit_msgs::msg::MoveItErrorCodes error_code;
error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
@ -346,5 +348,10 @@ const core::RobotModelConstPtr& Task::getRobotModel() const {
void Task::printState(std::ostream& os) const {
os << *stages();
}
void Task::explainFailure(std::ostream& os) const {
os << "Failing stage(s):\n";
stages()->explainFailure(os);
}
} // namespace task_constructor
} // namespace moveit

View File

@ -79,20 +79,27 @@ TEST(ContainerBase, positionForInsert) {
/* TODO: remove interface as it returns raw pointers */
TEST(ContainerBase, findChild) {
SerialContainer s;
auto s = std::make_unique<SerialContainer>();
Stage *a, *b, *c1, *d;
s.add(Stage::pointer(a = new NamedStage("a")));
s.add(Stage::pointer(b = new NamedStage("b")));
s.add(Stage::pointer(c1 = new NamedStage("c")));
s->add(Stage::pointer(a = new NamedStage("a")));
s->add(Stage::pointer(b = new NamedStage("b")));
s->add(Stage::pointer(c1 = new NamedStage("c")));
auto sub = ContainerBase::pointer(new SerialContainer("c"));
sub->add(Stage::pointer(d = new NamedStage("d")));
s.add(std::move(sub));
s->add(std::move(sub));
EXPECT_EQ(s.findChild("a"), a);
EXPECT_EQ(s.findChild("b"), b);
EXPECT_EQ(s.findChild("c"), c1);
EXPECT_EQ(s.findChild("d"), nullptr);
EXPECT_EQ(s.findChild("c/d"), d);
EXPECT_EQ(s->findChild("a"), a);
EXPECT_EQ(s->findChild("b"), b);
EXPECT_EQ(s->findChild("c"), c1);
EXPECT_EQ(s->findChild("d"), nullptr);
EXPECT_EQ(s->findChild("c/d"), d);
Task t("", false, std::move(s));
EXPECT_EQ(t.findChild("a"), a);
EXPECT_EQ(t.findChild("b"), b);
EXPECT_EQ(t.findChild("c"), c1);
EXPECT_EQ(t.findChild("d"), nullptr);
EXPECT_EQ(t.findChild("c/d"), d);
}
template <typename Container>

View File

@ -2,6 +2,26 @@
Changelog for package moveit_task_constructor_demo
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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>`_)
* Contributors: Robert Haschke
0.1.2 (2023-02-24)
------------------
* CartesianPath: Deprecate redundant property setters (`#429 <https://github.com/ros-planning/moveit_task_constructor/issues/429>`_)
* 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
* Fixed missing dependency declarations
* pick_place_task: monitor last state before Connect
... to prune solutions as much as possible
* Contributors: Robert Haschke
0.1.0 (2023-02-02)
------------------
* Initial release

View File

@ -59,12 +59,6 @@
#include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
#include "pick_place_demo_parameters.hpp"
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif
#pragma once
namespace moveit_task_constructor_demo {

View File

@ -1,12 +1,13 @@
<?xml version="1.0"?>
<package format="2">
<name>moveit_task_constructor_demo</name>
<version>0.1.0</version>
<version>0.1.3</version>
<description>demo tasks illustrating various capabilities of MTC.</description>
<author email="simongold@picknik.ai">simon Goldstein</author>
<author email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</author>
<author email="simongold@picknik.ai">Simon Goldstein</author>
<author email="henningkayser@picknik.ai">Henning Kayser</author>
<maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<license>BSD</license>

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_alternatives")
roscpp_initialize("mtc_tutorial_alternatives")
# Use the joint interpolation planner
jointPlanner = core.JointInterpolationPlanner()

View File

@ -7,9 +7,9 @@ from moveit.task_constructor import core, stages
from math import pi
import time
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
roscpp_init("mtc_tutorial")
roscpp_initialize("mtc_tutorial")
# [cartesianTut1]
group = "panda_arm"

View File

@ -6,9 +6,9 @@ from geometry_msgs.msg import PoseStamped, Pose, Vector3
from std_msgs.msg import Header
import time
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
roscpp_init("mtc_tutorial_compute_ik")
roscpp_initialize("mtc_tutorial_compute_ik")
# Specify the planning group
group = "panda_arm"
@ -40,7 +40,8 @@ generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose)
# Wrap Cartesian generator into a ComputeIK stage to yield a joint pose
computeIK = stages.ComputeIK("compute IK", generator)
computeIK.group = group # Use the group-specific IK solver
computeIK.ik_frame = "panda_link8" # Which end-effector frame should reach the target?
# Which end-effector frame should reach the target?
computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_link8"))
computeIK.max_ik_solutions = 4 # Limit the number of IK solutions
# [propertyTut14]
props = computeIK.properties

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_initialize("mtc_tutorial_current_state")
# Create a task
task = core.Task()

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_fallbacks")
roscpp_initialize("mtc_tutorial_fallbacks")
# use cartesian and joint interpolation planners
cartesianPlanner = core.CartesianPath()

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_initialize("mtc_tutorial_current_state")
# Create a task
task = core.Task()

View File

@ -3,11 +3,11 @@
from moveit.core import planning_scene
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.core.planning_scene import PlanningScene
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_initialize("mtc_tutorial_current_state")
# Create a task

View File

@ -3,10 +3,10 @@
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_compute_ik")
roscpp_initialize("mtc_tutorial_compute_ik")
# Specify the planning group
group = "panda_arm"

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_merger")
roscpp_initialize("mtc_tutorial_merger")
# use the joint interpolation planner
planner = core.JointInterpolationPlanner()

View File

@ -5,10 +5,10 @@ from moveit.task_constructor import core, stages
from moveit_msgs.msg import CollisionObject
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import PoseStamped
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_modify_planning_scene")
roscpp_initialize("mtc_tutorial_modify_planning_scene")
# Create a task
task = core.Task()

View File

@ -1,13 +1,13 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.task_constructor import core, stages
from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, TwistStamped
import time
roscpp_init("pickplace")
roscpp_initialize("pickplace")
# [pickAndPlaceTut1]
# Specify robot parameters

View File

@ -5,9 +5,9 @@ from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
import time
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
roscpp_init("mtc_tutorial")
roscpp_initialize("mtc_tutorial")
# Create a task container
task = core.Task()

View File

@ -37,6 +37,7 @@
#include <Eigen/Geometry>
#include <moveit_task_constructor_demo/pick_place_task.h>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include "pick_place_demo_parameters.hpp"
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
@ -117,8 +118,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
// Cartesian planner
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
cartesian_planner->setMaxVelocityScaling(1.0);
cartesian_planner->setMaxAccelerationScaling(1.0);
cartesian_planner->setMaxVelocityScalingFactor(1.0);
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
cartesian_planner->setStepSize(.01);
// Set task properties
@ -133,7 +134,6 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
* Current State *
* *
***************************************************/
Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator
{
auto current_state = std::make_unique<stages::CurrentState>("current state");
@ -147,8 +147,6 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
}
return true;
});
current_state_ptr = applicability_filter.get();
t.add(std::move(applicability_filter));
}
@ -157,10 +155,12 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
* Open Hand *
* *
***************************************************/
Stage* initial_state_ptr = nullptr;
{ // Open Hand
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->setGroup(params.hand_group_name);
stage->setGoal(params.hand_open_pose);
initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator
t.add(std::move(stage));
}
@ -182,7 +182,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
* Pick Object *
* *
***************************************************/
Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
Stage* pick_stage_ptr = nullptr;
{
auto grasp = std::make_unique<SerialContainer>("pick object");
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
@ -217,7 +217,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
stage->setPreGraspPose(params.hand_open_pose);
stage->setObject(params.object_name);
stage->setAngleDelta(M_PI / 12);
stage->setMonitoredStage(current_state_ptr); // Hook into current state
stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
// Compute IK
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
@ -257,7 +257,6 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
stage->attachObject(params.object_name, params.hand_frame);
attach_object_stage = stage.get();
grasp->insert(std::move(stage));
}
@ -297,6 +296,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
grasp->insert(std::move(stage));
}
pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator
// Add grasp container to task
t.add(std::move(grasp));
}
@ -358,7 +359,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
p.pose = vectorToPose(params.place_pose);
p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset;
stage->setPose(p);
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions
// Compute IK
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
@ -456,8 +457,8 @@ bool PickPlaceTask::execute() {
// // If you want to inspect the goal message, use this instead:
// actionlib::SimpleActionClient<moveit_task_constructor_msgs::msg::ExecuteTaskSolutionAction>
// execute("execute_task_solution", true); execute.waitForServer();
// moveit_task_constructor_msgs::msg::ExecuteTaskSolutionGoal execute_goal;
// task_->solutions().front()->fillMessage(execute_goal.solution);
// moveit_task_constructor_msgs::msg::ExecuteTaskSolution::Goal execute_goal;
// task_->solutions().front()->toMsg(execute_goal.solution);
// execute.sendGoalAndWait(execute_goal);
// execute_result = execute.getResult()->error_code;

View File

@ -2,6 +2,15 @@
Changelog for package moveit_task_constructor_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
0.1.2 (2023-02-24)
------------------
0.1.1 (2023-02-15)
------------------
0.1.0 (2023-02-02)
------------------
* Initial release

View File

@ -1,6 +1,6 @@
<package format="3">
<name>moveit_task_constructor_msgs</name>
<version>0.1.0</version>
<version>0.1.3</version>
<description>Messages for MoveIt Task Pipeline</description>
<license>BSD</license>

View File

@ -2,6 +2,17 @@
Changelog for package rviz_marker_tools
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
0.1.2 (2023-02-24)
------------------
* Fix marker creation: allow zero scale for geometric shapes (`#430 <https://github.com/ros-planning/moveit_task_constructor/issues/430>`_)
* Contributors: Robert Haschke
0.1.1 (2023-02-15)
------------------
0.1.0 (2023-02-02)
------------------
* Initial release

View File

@ -1,9 +1,10 @@
<package format="2">
<name>rviz_marker_tools</name>
<version>0.1.0</version>
<version>0.1.3</version>
<description>Tools for marker creation / handling</description>
<license>BSD</license>
<author email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</author>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<buildtool_depend>ament_cmake</buildtool_depend>

View File

@ -165,12 +165,6 @@ void prepareMarker(vm::msg::Marker& m, int marker_type) {
m.points.clear();
m.colors.clear();
// ensure valid scale
if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) {
m.scale.x = 1.0;
m.scale.y = 1.0;
m.scale.z = 1.0;
}
// ensure non-null orientation
if (m.pose.orientation.w == 0 && m.pose.orientation.x == 0 && m.pose.orientation.y == 0 && m.pose.orientation.z == 0)
m.pose.orientation.w = 1.0;
@ -195,6 +189,7 @@ vm::msg::Marker& makeXYPlane(vm::msg::Marker& m) {
p[3].y = -1.0;
p[3].z = 0.0;
m.scale.x = m.scale.y = m.scale.z = 1.0;
prepareMarker(m, vm::msg::Marker::TRIANGLE_LIST);
m.points.push_back(p[0]);
m.points.push_back(p[1]);
@ -224,6 +219,7 @@ vm::msg::Marker& makeYZPlane(vm::msg::Marker& m) {
/// create a cone of given angle along the x-axis
vm::msg::Marker makeCone(double angle, vm::msg::Marker& m) {
m.scale.x = m.scale.y = m.scale.z = 1.0;
prepareMarker(m, vm::msg::Marker::TRIANGLE_LIST);
geometry_msgs::msg::Point p[3];
p[0].x = p[0].y = p[0].z = 0.0;
@ -303,6 +299,7 @@ vm::msg::Marker& makeArrow(vm::msg::Marker& m, double scale, bool tip_at_origin)
}
vm::msg::Marker& makeText(vm::msg::Marker& m, const std::string& text) {
m.scale.x = m.scale.y = m.scale.z = 1.0;
prepareMarker(m, vm::msg::Marker::TEXT_VIEW_FACING);
m.text = text;
return m;

View File

@ -2,6 +2,17 @@
Changelog for package moveit_task_constructor_visualization
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
0.1.2 (2023-02-24)
------------------
0.1.1 (2023-02-15)
------------------
* Remove unused eigen_conversions includes
* Contributors: Robert Haschke
0.1.0 (2023-02-02)
------------------
* Initial release

View File

@ -80,7 +80,7 @@ TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_d
task_solution_topic_property_ = new rviz_common::properties::RosTopicProperty(
"Task Solution Topic", "", rosidl_generator_traits::data_type<moveit_task_constructor_msgs::msg::Solution>(),
"The topic on which task solutions (moveit_msgs::Solution messages) are received", this,
"The topic on which task solutions (moveit_msgs::msg::Solution messages) are received", this,
SLOT(changedTaskSolutionTopic()), this);
trajectory_visual_.reset(new TaskSolutionVisualization(this, this));

View File

@ -1,9 +1,10 @@
<package format="2">
<name>moveit_task_constructor_visualization</name>
<version>0.1.0</version>
<version>0.1.3</version>
<description>Visualization tools for MoveIt Task Pipeline</description>
<license>BSD</license>
<author email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</author>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<maintainer email="me@v4hn.de">Michael Goerner</maintainer>