mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
commit
26c690c3b6
@ -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;
|
||||
|
||||
@ -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"));
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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"(
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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):
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user