new TrajectoryCostTerm: DistanceToReference

This commit is contained in:
Robert Haschke 2022-12-09 09:41:03 +01:00
parent a9b4947845
commit 6de570c7a6
2 changed files with 76 additions and 8 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;
};
@ -128,6 +137,21 @@ public:
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
class TrajectoryDuration : public TrajectoryCostTerm
{
@ -157,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

@ -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>
@ -140,6 +141,57 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/)
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;
}