mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
new TrajectoryCostTerm: DistanceToReference
This commit is contained in:
parent
a9b4947845
commit
6de570c7a6
@ -40,6 +40,7 @@
|
|||||||
|
|
||||||
#include <moveit/task_constructor/storage.h>
|
#include <moveit/task_constructor/storage.h>
|
||||||
#include <moveit/task_constructor/utils.h>
|
#include <moveit/task_constructor/utils.h>
|
||||||
|
#include <moveit_msgs/RobotState.h>
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
@ -69,6 +70,14 @@ public:
|
|||||||
class TrajectoryCostTerm : public CostTerm
|
class TrajectoryCostTerm : public CostTerm
|
||||||
{
|
{
|
||||||
public:
|
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 SolutionSequence& s, std::string& comment) const override;
|
||||||
double operator()(const WrappedSolution& 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
|
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
|
/// execution duration of the whole trajectory
|
||||||
class TrajectoryDuration : public TrajectoryCostTerm
|
class TrajectoryDuration : public TrajectoryCostTerm
|
||||||
{
|
{
|
||||||
@ -157,14 +181,6 @@ public:
|
|||||||
class Clearance : public TrajectoryCostTerm
|
class Clearance : public TrajectoryCostTerm
|
||||||
{
|
{
|
||||||
public:
|
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",
|
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
|
||||||
Mode mode = Mode::AUTO);
|
Mode mode = Mode::AUTO);
|
||||||
bool with_world;
|
bool with_world;
|
||||||
|
|||||||
@ -40,6 +40,7 @@
|
|||||||
#include <moveit/collision_detection/collision_common.h>
|
#include <moveit/collision_detection/collision_common.h>
|
||||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
#include <moveit/robot_state/conversions.h>
|
||||||
|
|
||||||
#include <Eigen/Geometry>
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
@ -140,6 +141,57 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/)
|
|||||||
return path_length;
|
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 {
|
double TrajectoryDuration::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
|
||||||
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
|
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user