Merge PRs #412 (fix-ci) and #409 (more cost-terms)

This commit is contained in:
Robert Haschke 2022-12-11 22:38:32 +01:00
commit 26c690c3b6
7 changed files with 165 additions and 20 deletions

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/utils.h>
#include <moveit_msgs/RobotState.h>
namespace moveit {
namespace task_constructor {
@ -69,6 +70,14 @@ public:
class TrajectoryCostTerm : public CostTerm
{
public:
enum class Mode
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
double operator()(const SolutionSequence& s, std::string& comment) const override;
double operator()(const WrappedSolution& s, std::string& comment) const override;
};
@ -113,15 +122,34 @@ public:
double cost;
};
/// trajectory length (interpolated between waypoints)
/// trajectory length with optional weighting for different joints
class PathLength : public TrajectoryCostTerm
{
public:
/// By default, all joints are considered with same weight of 1.0
PathLength() = default;
PathLength(std::vector<std::string> j) : joints{ std::move(j) } {};
/// Limit measurements to given joint names
PathLength(std::vector<std::string> joints);
/// Limit measurements to given joints and use given weighting
PathLength(std::map<std::string, double> j) : joints(std::move(j)) {}
double operator()(const SubTrajectory& s, std::string& comment) const override;
std::vector<std::string> joints;
std::map<std::string, double> joints; //< joint weights
};
/// (weighted) joint-space distance to reference pose
class DistanceToReference : public TrajectoryCostTerm
{
public:
DistanceToReference(const moveit_msgs::RobotState& ref, Mode m = Mode::AUTO,
std::map<std::string, double> w = std::map<std::string, double>());
DistanceToReference(const std::map<std::string, double>& ref, Mode m = Mode::AUTO,
std::map<std::string, double> w = std::map<std::string, double>());
double operator()(const SubTrajectory& s, std::string& comment) const override;
moveit_msgs::RobotState reference;
std::map<std::string, double> weights;
Mode mode;
};
/// execution duration of the whole trajectory
@ -153,14 +181,6 @@ public:
class Clearance : public TrajectoryCostTerm
{
public:
enum class Mode
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
Mode mode = Mode::AUTO);
bool with_world;

View File

@ -73,6 +73,7 @@ class Task : protected WrapperBase
{
public:
PRIVATE_CLASS(Task)
using WrapperBase::setCostTerm;
Task(const std::string& ns = "", bool introspection = true,
ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));

View File

@ -1,10 +1,11 @@
# As we rely on some not yet released pybind11 PRs, we are employing
# our own fork of it, imported via git submodule.
# See https://github.com/pybind/pybind11/pull/2687.
# We rely on pybind11's smart_holder branch imported pybind11 via git submodule
# pybind11 must use the ROS python version
set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING})
# Use minimum-size optimization for pybind11 bindings
add_compile_options("-Os")
# create symlink to grant access to downstream packages in devel space
add_custom_target(pybind11_devel_symlink ALL COMMAND ${CMAKE_COMMAND} -E create_symlink
${CMAKE_CURRENT_SOURCE_DIR}/pybind11

View File

@ -33,6 +33,8 @@
*********************************************************************/
#include "core.h"
#include <pybind11/stl.h>
#include <pybind11/functional.h>
#include <moveit/python/task_constructor/properties.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task.h>
@ -157,6 +159,36 @@ void export_core(pybind11::module& m) {
.def(PYBIND11_BOOL_ATTR,
[](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast<bool>(err)); });
py::classh<CostTerm>(m, "CostTerm", "Base class for cost calculation in stages");
auto tct = py::classh<TrajectoryCostTerm, CostTerm>(m, "TrajectoryCostTerm",
"Base class for cost calculation of trajectories");
py::enum_<TrajectoryCostTerm::Mode>(tct, "Mode", "Specify which states are considered for collision checking")
.value("AUTO", TrajectoryCostTerm::Mode::AUTO, "TRAJECTORY (if available) or START_INTERFACE")
.value("START_INTERFACE", TrajectoryCostTerm::Mode::START_INTERFACE, "Only consider start state")
.value("END_INTERFACE", TrajectoryCostTerm::Mode::END_INTERFACE, "Only consider end state")
.value("TRAJECTORY", TrajectoryCostTerm::Mode::TRAJECTORY, "Consider whole trajectory");
py::classh<cost::PathLength, TrajectoryCostTerm>(m, "PathLength",
"Computes joint-based path length along trajectory")
.def(py::init<>())
.def(py::init<std::vector<std::string>>())
.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>>(),
"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>());
py::classh<cost::TrajectoryDuration, TrajectoryCostTerm>(m, "TrajectoryDuration", "Computes duration of trajectory")
.def(py::init<>());
py::classh<cost::LinkMotion, TrajectoryCostTerm>(m, "LinkMotion",
"Computes Cartesian path length of given link along trajectory")
.def(py::init<std::string>(), "link_name"_a);
py::classh<cost::Clearance, TrajectoryCostTerm>(m, "Clearance", "Computes inverse distance to collision objects")
.def(py::init<bool, bool, std::string, TrajectoryCostTerm::Mode>(), "with_world"_a = true,
"cumulative"_a = false, "group_property"_a = "group", "mode"_a = TrajectoryCostTerm::Mode::AUTO);
auto stage =
properties::class_<Stage, PyStage<>>(m, "Stage", "Abstract base class of all stages.")
.property<double>("timeout", "float: Maximally allowed time [s] per computation step")
@ -168,6 +200,15 @@ void export_core(pybind11::module& m) {
"PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)")
.def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def<void (Stage::*)(const CostTermConstPtr&)>("setCostTerm", &Stage::setCostTerm,
"Specify a CostTerm for calculation of stage costs")
.def(
"setCostTerm", [](Stage& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def(
"setCostTerm",
[](Stage& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties")
.def("init", &Stage::init,
"Initialize the stage once before planning. "
@ -407,6 +448,15 @@ void export_core(pybind11::module& m) {
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def(
"setCostTerm", [](Task& self, const CostTermConstPtr& c) { self.setCostTerm(c); },
"Specify a CostTerm for calculation of stage costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def("reset", &Task::reset, "Reset task (and all its stages)")
.def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)")
.def("plan", &Task::plan, "max_solutions"_a = 0, R"(

View File

@ -37,6 +37,7 @@
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/utils/moveit_error_code.h>
#include <pybind11/smart_holder.h>
@ -119,6 +120,14 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(ordered<moveit::task_constructor::SolutionBas
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::InterfaceState)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::core::MoveItErrorCode)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::CostTerm)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::TrajectoryCostTerm)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::PathLength)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::DistanceToReference)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::TrajectoryDuration)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::LinkMotion)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::Clearance)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Stage)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingEitherWay)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingForward)

View File

@ -272,6 +272,12 @@ class TestStages(unittest.TestCase):
print("error in class {}: {}".format(stage, ex))
raise
def test_CostTerm(self):
stage = stages.CurrentState()
weights = {"joint_{}".format(i + 1): 1.0 for i in range(6)}
costs = core.PathLength(weights)
stage.setCostTerm(costs)
class TestContainer(unittest.TestCase):
def __init__(self, *args, **kwargs):

View File

@ -40,6 +40,7 @@
#include <moveit/collision_detection/collision_common.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <Eigen/Geometry>
@ -106,17 +107,23 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment
return cost;
}
PathLength::PathLength(std::vector<std::string> joints) {
for (auto& j : joints)
this->joints.emplace(std::move(j), 1.0);
}
double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
const auto& traj = s.trajectory();
if (traj == nullptr || traj->getWayPointCount() == 0)
return 0.0;
std::vector<const robot_model::JointModel*> joint_models;
joint_models.reserve(joints.size());
std::map<const robot_model::JointModel*, double> weights;
const auto& first_waypoint = traj->getWayPoint(0);
for (auto& joint : joints) {
joint_models.push_back(first_waypoint.getJointModel(joint));
for (auto& joint_weight : joints) {
const robot_model::JointModel* jm = first_waypoint.getJointModel(joint_weight.first);
if (jm)
weights.emplace(jm, joint_weight.second);
}
double path_length{ 0.0 };
@ -126,14 +133,65 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/)
if (joints.empty()) {
path_length += last.distance(curr);
} else {
for (const auto& model : joint_models) {
path_length += last.distance(curr, model);
for (const auto& item : weights) {
path_length += item.second * last.distance(curr, item.first);
}
}
}
return path_length;
}
DistanceToReference::DistanceToReference(const moveit_msgs::RobotState& ref, Mode m, std::map<std::string, double> w)
: reference(ref), weights(std::move(w)), mode(m) {}
DistanceToReference::DistanceToReference(const std::map<std::string, double>& ref, Mode m,
std::map<std::string, double> w)
: weights(std::move(w)), mode(m) {
reference.joint_state.name.reserve(ref.size());
reference.joint_state.position.reserve(ref.size());
for (auto& item : ref) {
reference.joint_state.name.push_back(item.first);
reference.joint_state.position.push_back(item.second);
}
reference.is_diff = true;
}
double DistanceToReference::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
const auto& state = (mode == Mode::END_INTERFACE) ? s.end() : s.start();
const auto& traj = s.trajectory();
moveit::core::RobotState ref_state = state->scene()->getCurrentState();
moveit::core::robotStateMsgToRobotState(reference, ref_state, false);
std::map<const robot_model::JointModel*, double> w;
for (auto& item : weights) {
const robot_model::JointModel* jm = ref_state.getJointModel(item.first);
if (jm)
w.emplace(jm, item.second);
}
auto distance = [this, &ref_state, &w](const moveit::core::RobotState& state) {
if (weights.empty()) {
return ref_state.distance(state);
} else {
double accumulated = 0.0;
for (const auto& item : w)
accumulated += item.second * ref_state.distance(state, item.first);
return accumulated;
}
};
if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE || (mode == Mode::AUTO && (traj == nullptr))) {
return distance(state->scene()->getCurrentState());
} else {
double accumulated = 0.0;
for (size_t i = 0; i < traj->getWayPointCount(); ++i)
accumulated += distance(traj->getWayPoint(i));
accumulated /= traj->getWayPointCount();
return accumulated;
}
}
double TrajectoryDuration::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
}