mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch master into ros2
This commit is contained in:
commit
349e3c6c6c
41
.github/workflows/prerelease.yaml
vendored
Normal file
41
.github/workflows/prerelease.yaml
vendored
Normal 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
2
.gitmodules
vendored
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -28,3 +28,4 @@ Organization of the documentation
|
||||
concepts
|
||||
howto
|
||||
api
|
||||
troubleshooting
|
||||
|
||||
31
core/doc/troubleshooting.rst
Normal file
31
core/doc/troubleshooting.rst
Normal 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.
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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 {
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
77
core/include/moveit/task_constructor/solvers/multi_planner.h
Normal file
77
core/include/moveit/task_constructor/solvers/multi_planner.h
Normal 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
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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(); }
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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); }
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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(); }
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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)) {
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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);
|
||||
}
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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",
|
||||
|
||||
@ -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
117
core/python/test/rostest_mps.py
Executable 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)
|
||||
@ -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)
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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
26
core/setup.py
Normal 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
|
||||
@ -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
|
||||
|
||||
@ -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) {}
|
||||
|
||||
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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);
|
||||
|
||||
98
core/src/solvers/multi_planner.cpp
Normal file
98
core/src/solvers/multi_planner.cpp
Normal 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
|
||||
@ -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");
|
||||
|
||||
@ -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>());
|
||||
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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>
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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"
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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"
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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>
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user