mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cleanup
This commit is contained in:
parent
9a7aa4dad7
commit
40ca35085a
@ -287,15 +287,16 @@ void PropertyMap::set<boost::any>(const std::string& name, const boost::any& val
|
|||||||
// provide a serialization method for std::map
|
// provide a serialization method for std::map
|
||||||
template <typename T>
|
template <typename T>
|
||||||
std::ostream& operator<<(std::ostream& os, const std::map<std::string, T>& m) {
|
std::ostream& operator<<(std::ostream& os, const std::map<std::string, T>& m) {
|
||||||
os << "{";
|
os << "{";
|
||||||
bool first = true;
|
bool first = true;
|
||||||
for (const auto& pair : m) {
|
for (const auto& pair : m) {
|
||||||
if (!first)
|
if (!first)
|
||||||
os << ", ";
|
os << ", ";
|
||||||
os << pair.first << " : " << pair.second;
|
os << pair.first << " : " << pair.second;
|
||||||
first = false;
|
first = false;
|
||||||
}
|
}
|
||||||
os << "}";
|
os << "}";
|
||||||
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -55,24 +55,42 @@ public:
|
|||||||
bool computeForward(const InterfaceState& from) override;
|
bool computeForward(const InterfaceState& from) override;
|
||||||
bool computeBackward(const InterfaceState& to) override;
|
bool computeBackward(const InterfaceState& to) override;
|
||||||
|
|
||||||
void setGroup(const std::string& group);
|
void setGroup(const std::string& group) {
|
||||||
void setLink(const std::string& link);
|
setProperty("group", group);
|
||||||
|
}
|
||||||
|
void setLink(const std::string& link) {
|
||||||
|
setProperty("link", link);
|
||||||
|
}
|
||||||
|
|
||||||
/// set minimum / maximum distance to move
|
/// set minimum / maximum distance to move
|
||||||
void setMinDistance(double distance);
|
void setMinDistance(double distance) {
|
||||||
void setMaxDistance(double distance);
|
setProperty("min_distance", distance);
|
||||||
void setMinMaxDistance(double min_distance, double max_distance);
|
}
|
||||||
|
void setMaxDistance(double distance) {
|
||||||
|
setProperty("max_distance", distance);
|
||||||
|
}
|
||||||
|
void setMinMaxDistance(double min_distance, double max_distance) {
|
||||||
|
setProperty("min_distance", min_distance);
|
||||||
|
setProperty("max_distance", max_distance);
|
||||||
|
}
|
||||||
|
|
||||||
void setRelativeJointSpaceGoal(const std::map<std::string, double>& goal);
|
void setPathConstraints(moveit_msgs::Constraints path_constraints) {
|
||||||
|
|
||||||
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
|
||||||
setProperty("path_constraints", std::move(path_constraints));
|
setProperty("path_constraints", std::move(path_constraints));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// perform twist motion on specified link
|
/// perform twist motion on specified link
|
||||||
void along(const geometry_msgs::TwistStamped& twist);
|
void along(const geometry_msgs::TwistStamped& twist) {
|
||||||
|
setProperty("twist", twist);
|
||||||
|
}
|
||||||
/// translate link along given direction
|
/// translate link along given direction
|
||||||
void along(const geometry_msgs::Vector3Stamped& direction);
|
void along(const geometry_msgs::Vector3Stamped& direction) {
|
||||||
|
setProperty("direction", direction);
|
||||||
|
}
|
||||||
|
/// move specified joint variables by given amount
|
||||||
|
void about(const std::map<std::string, double>& goal) {
|
||||||
|
setProperty("joints", goal);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
||||||
@ -80,7 +98,6 @@ protected:
|
|||||||
|
|
||||||
protected:
|
protected:
|
||||||
solvers::PlannerInterfacePtr planner_;
|
solvers::PlannerInterfacePtr planner_;
|
||||||
bool joint_space_goal_ = false;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -86,10 +86,10 @@ public:
|
|||||||
|
|
||||||
void setApproachRetract(const geometry_msgs::TwistStamped& motion,
|
void setApproachRetract(const geometry_msgs::TwistStamped& motion,
|
||||||
double min_distance, double max_distance);
|
double min_distance, double max_distance);
|
||||||
|
|
||||||
void setLiftPlace(const geometry_msgs::TwistStamped& motion,
|
void setLiftPlace(const geometry_msgs::TwistStamped& motion,
|
||||||
double min_distance, double max_distance);
|
double min_distance, double max_distance);
|
||||||
|
void setLiftPlace(const std::map<std::string, double>& joints);
|
||||||
void setRelativeJointSpaceGoal(const std::map<std::string, double>& relative_joint_space_goal);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -109,10 +109,9 @@ public:
|
|||||||
double min_distance, double max_distance) {
|
double min_distance, double max_distance) {
|
||||||
setLiftPlace(motion, min_distance, max_distance);
|
setLiftPlace(motion, min_distance, max_distance);
|
||||||
}
|
}
|
||||||
|
void setLiftMotion(const std::map<std::string, double>& joints) {
|
||||||
void setRelativeJointSpaceLiftGoal(const std::map<std::string, double>& relative_joint_space_goal) {
|
setLiftPlace(joints);
|
||||||
setRelativeJointSpaceGoal(relative_joint_space_goal);
|
}
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -132,6 +131,9 @@ public:
|
|||||||
double min_distance, double max_distance) {
|
double min_distance, double max_distance) {
|
||||||
setLiftPlace(motion, min_distance, max_distance);
|
setLiftPlace(motion, min_distance, max_distance);
|
||||||
}
|
}
|
||||||
|
void setPlaceMotion(const std::map<std::string, double>& joints) {
|
||||||
|
setLiftPlace(joints);
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -52,52 +52,17 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
|
|||||||
p.declare<std::string>("marker_ns", "", "marker namespace");
|
p.declare<std::string>("marker_ns", "", "marker namespace");
|
||||||
p.declare<std::string>("group", "name of planning group");
|
p.declare<std::string>("group", "name of planning group");
|
||||||
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
|
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
|
||||||
p.declare<double>("min_distance", "minimum distance to move");
|
p.declare<double>("min_distance", -1.0, "minimum distance to move");
|
||||||
p.declare<double>("max_distance", "maximum distance to move");
|
p.declare<double>("max_distance", 0.0, "maximum distance to move");
|
||||||
|
|
||||||
p.declare<geometry_msgs::TwistStamped>("twist", "Cartesian twist transform");
|
p.declare<geometry_msgs::TwistStamped>("twist", "Cartesian twist transform");
|
||||||
p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction");
|
p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction");
|
||||||
p.declare<std::map<std::string, double>>("relative_joint_space_goal", "Relative joint space goal");
|
p.declare<std::map<std::string, double>>("joints", "Relative joint space goal");
|
||||||
|
|
||||||
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
|
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
|
||||||
"constraints to maintain during trajectory");
|
"constraints to maintain during trajectory");
|
||||||
}
|
}
|
||||||
|
|
||||||
void MoveRelative::setRelativeJointSpaceGoal(const std::map<std::string, double>& goal){
|
|
||||||
setProperty("relative_joint_space_goal", goal);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::setGroup(const std::string& group){
|
|
||||||
setProperty("group", group);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::setLink(const std::string& link){
|
|
||||||
setProperty("link", link);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::setMinDistance(double distance){
|
|
||||||
setProperty("min_distance", distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::setMaxDistance(double distance){
|
|
||||||
setProperty("max_distance", distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::setMinMaxDistance(double min_distance, double max_distance){
|
|
||||||
setProperty("min_distance", min_distance);
|
|
||||||
setProperty("max_distance", max_distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::along(const geometry_msgs::TwistStamped &twist)
|
|
||||||
{
|
|
||||||
setProperty("twist", twist);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction)
|
|
||||||
{
|
|
||||||
setProperty("direction", direction);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(robot_model);
|
PropagatingEitherWay::init(robot_model);
|
||||||
@ -114,154 +79,172 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
const std::string& group = props.get<std::string>("group");
|
const std::string& group = props.get<std::string>("group");
|
||||||
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
|
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
|
||||||
if (!jmg) {
|
if (!jmg) {
|
||||||
ROS_WARN_STREAM("MoveRelative: invalid joint model group: " << group);
|
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// only allow single target
|
// only allow single target
|
||||||
size_t count_goals = props.countDefined({"twist", "direction", "relative_joint_space_goal"});
|
size_t count_goals = props.countDefined({"twist", "direction", "joints"});
|
||||||
if (count_goals != 1) {
|
if (count_goals != 1) {
|
||||||
if (count_goals == 0) ROS_WARN("MoveRelative: no goal defined");
|
if (count_goals == 0) ROS_WARN_NAMED("MoveRelative", "No goal defined");
|
||||||
else ROS_WARN("MoveRelative: cannot plan to multiple goals");
|
else ROS_WARN_NAMED("MoveRelative", "Cannot plan to multiple goals");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Cartesian targets require the link name
|
double max_distance = props.get<double>("max_distance");
|
||||||
// TODO: use ik_frame property as in ComputeIK
|
double min_distance = props.get<double>("min_distance");
|
||||||
std::string link_name = props.get<std::string>("link");
|
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||||
const moveit::core::LinkModel* link;
|
|
||||||
if (link_name.empty())
|
|
||||||
link_name = solvers::getEndEffectorLink(jmg);
|
|
||||||
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
|
||||||
ROS_WARN_STREAM("MoveRelative: no or invalid link name specified: " << link_name);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
Property prop = props.property("max_distance");
|
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||||
double max_distance = prop.value().empty() ? 0.0 : std::abs(boost::any_cast<double>(prop.value()));
|
bool success = false;
|
||||||
prop = props.property("min_distance");
|
|
||||||
double min_distance = prop.value().empty() ? -1.0 : boost::any_cast<double>(prop.value());
|
|
||||||
|
|
||||||
bool use_rotation_distance = false; // measure achieved distance as rotation?
|
boost::any goal = props.get("joints");
|
||||||
Eigen::Vector3d linear; // linear translation
|
|
||||||
Eigen::Vector3d angular; // angular rotation
|
|
||||||
double linear_norm = 0.0, angular_norm = 0.0;
|
|
||||||
|
|
||||||
Eigen::Affine3d target_eigen;
|
|
||||||
Eigen::Affine3d link_pose = scene->getFrameTransform(link_name); // take a copy here, pose will change on success
|
|
||||||
|
|
||||||
boost::any goal = props.get("twist");
|
|
||||||
if (!goal.empty()) {
|
if (!goal.empty()) {
|
||||||
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(goal);
|
const auto& accepted = jmg->getJointModelNames();
|
||||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
auto& robot_state = scene->getCurrentStateNonConst();
|
||||||
tf::vectorMsgToEigen(target.twist.linear, linear);
|
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
|
||||||
tf::vectorMsgToEigen(target.twist.angular, angular);
|
for (auto j : joints) {
|
||||||
|
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
||||||
linear_norm = linear.norm();
|
auto jm = scene->getRobotModel()->getJointModel(index);
|
||||||
angular_norm = angular.norm();
|
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
|
||||||
if (angular_norm > std::numeric_limits<double>::epsilon())
|
ROS_WARN_STREAM_NAMED("MoveRelative", "Ignoring joint " << j.first << " that is not part of group " << group);
|
||||||
angular /= angular_norm; // normalize angular
|
continue;
|
||||||
use_rotation_distance = linear_norm < std::numeric_limits<double>::epsilon();
|
}
|
||||||
|
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||||
// use max distance?
|
robot_state.enforceBounds(jm);
|
||||||
if (max_distance > 0.0) {
|
}
|
||||||
double scale;
|
robot_state.update();
|
||||||
if (!use_rotation_distance) // non-zero linear motion defines distance
|
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
scale = max_distance / linear_norm;
|
} else {
|
||||||
else if (angular_norm > std::numeric_limits<double>::epsilon())
|
// Cartesian targets require the link name
|
||||||
scale = max_distance / angular_norm;
|
// TODO: use ik_frame property as in ComputeIK
|
||||||
linear *= scale;
|
std::string link_name = props.get<std::string>("link");
|
||||||
linear_norm *= scale;
|
const moveit::core::LinkModel* link;
|
||||||
angular_norm *= scale;
|
if (link_name.empty())
|
||||||
|
link_name = solvers::getEndEffectorLink(jmg);
|
||||||
|
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
||||||
|
ROS_WARN_STREAM_NAMED("MoveRelative", "No or invalid link name specified: " << link_name);
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// invert direction?
|
bool use_rotation_distance = false; // measure achieved distance as rotation?
|
||||||
if (dir == BACKWARD) {
|
Eigen::Vector3d linear; // linear translation
|
||||||
linear *= -1.0;
|
Eigen::Vector3d angular; // angular rotation
|
||||||
angular *= -1.0;
|
double linear_norm = 0.0, angular_norm = 0.0;
|
||||||
|
|
||||||
|
Eigen::Affine3d target_eigen;
|
||||||
|
Eigen::Affine3d link_pose = scene->getFrameTransform(link_name); // take a copy here, pose will change on success
|
||||||
|
|
||||||
|
boost::any goal = props.get("twist");
|
||||||
|
if (!goal.empty()) {
|
||||||
|
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(goal);
|
||||||
|
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||||
|
tf::vectorMsgToEigen(target.twist.linear, linear);
|
||||||
|
tf::vectorMsgToEigen(target.twist.angular, angular);
|
||||||
|
|
||||||
|
linear_norm = linear.norm();
|
||||||
|
angular_norm = angular.norm();
|
||||||
|
if (angular_norm > std::numeric_limits<double>::epsilon())
|
||||||
|
angular /= angular_norm; // normalize angular
|
||||||
|
use_rotation_distance = linear_norm < std::numeric_limits<double>::epsilon();
|
||||||
|
|
||||||
|
// use max distance?
|
||||||
|
if (max_distance > 0.0) {
|
||||||
|
double scale;
|
||||||
|
if (!use_rotation_distance) // non-zero linear motion defines distance
|
||||||
|
scale = max_distance / linear_norm;
|
||||||
|
else if (angular_norm > std::numeric_limits<double>::epsilon())
|
||||||
|
scale = max_distance / angular_norm;
|
||||||
|
linear *= scale;
|
||||||
|
linear_norm *= scale;
|
||||||
|
angular_norm *= scale;
|
||||||
|
}
|
||||||
|
|
||||||
|
// invert direction?
|
||||||
|
if (dir == BACKWARD) {
|
||||||
|
linear *= -1.0;
|
||||||
|
angular *= -1.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute absolute transform for link
|
||||||
|
linear = frame_pose.linear() * linear;
|
||||||
|
angular = frame_pose.linear() * angular;
|
||||||
|
target_eigen = link_pose;
|
||||||
|
target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
|
||||||
|
target_eigen.translation() += linear;
|
||||||
}
|
}
|
||||||
|
|
||||||
// compute absolute transform for link
|
goal = props.get("direction");
|
||||||
linear = frame_pose.linear() * linear;
|
if (!goal.empty()) {
|
||||||
angular = frame_pose.linear() * angular;
|
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(goal);
|
||||||
target_eigen = link_pose;
|
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||||
target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
|
tf::vectorMsgToEigen(target.vector, linear);
|
||||||
target_eigen.translation() += linear;
|
|
||||||
}
|
|
||||||
|
|
||||||
goal = props.get("direction");
|
// use max distance?
|
||||||
if (!goal.empty()) {
|
if (max_distance > 0.0) {
|
||||||
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(goal);
|
linear.normalize();
|
||||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
linear *= max_distance;
|
||||||
tf::vectorMsgToEigen(target.vector, linear);
|
}
|
||||||
|
linear_norm = linear.norm();
|
||||||
|
|
||||||
// use max distance?
|
// invert direction?
|
||||||
if (max_distance > 0.0) {
|
if (dir == BACKWARD)
|
||||||
linear.normalize();
|
linear *= -1.0;
|
||||||
linear *= max_distance;
|
|
||||||
|
// compute absolute transform for link
|
||||||
|
linear = frame_pose.linear() * linear;
|
||||||
|
target_eigen = link_pose;
|
||||||
|
target_eigen.translation() += linear;
|
||||||
}
|
}
|
||||||
linear_norm = linear.norm();
|
|
||||||
|
|
||||||
// invert direction?
|
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
if (dir == BACKWARD)
|
|
||||||
linear *= -1.0;
|
|
||||||
|
|
||||||
// compute absolute transform for link
|
// min_distance reached?
|
||||||
linear = frame_pose.linear() * linear;
|
if (min_distance > 0.0) {
|
||||||
target_eigen = link_pose;
|
double distance = 0.0;
|
||||||
target_eigen.translation() += linear;
|
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||||
}
|
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
|
||||||
|
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
|
||||||
goal = props.get("relative_joint_space_goal");
|
if (use_rotation_distance) {
|
||||||
if (!goal.empty()) {
|
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||||
|
distance = rotation.angle();
|
||||||
const auto &start_state = scene->getCurrentState();
|
} else
|
||||||
auto& current_state = scene->getCurrentStateNonConst();
|
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
||||||
joint_space_goal_ = true;
|
}
|
||||||
|
success = distance >= min_distance;
|
||||||
const std::map<std::string, double>& relative_joint_position_map = boost::any_cast<std::map<std::string, double>>(goal);
|
if (!success) {
|
||||||
|
char msg[100];
|
||||||
for (auto j : relative_joint_position_map) {
|
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
|
||||||
double new_pos = (start_state.getVariablePosition(j.first) + j.second);
|
trajectory.setName(msg);
|
||||||
current_state.setVariablePosition(j.first, new_pos);
|
}
|
||||||
|
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
||||||
auto jm = scene->getRobotModel()->getJointModel(j.first);
|
success = true;
|
||||||
current_state.enforceBounds(jm);
|
|
||||||
current_state.update(true);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
|
||||||
bool success = false;
|
|
||||||
|
|
||||||
if (joint_space_goal_) {
|
|
||||||
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory,
|
|
||||||
props.get<moveit_msgs::Constraints>("path_constraints"));
|
|
||||||
} else {
|
|
||||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory,
|
|
||||||
props.get<moveit_msgs::Constraints>("path_constraints"));
|
|
||||||
}
|
|
||||||
|
|
||||||
// min_distance reached?
|
|
||||||
if (min_distance > 0.0) {
|
|
||||||
double distance = 0.0;
|
|
||||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
|
||||||
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
|
|
||||||
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
|
|
||||||
if (use_rotation_distance) {
|
|
||||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
|
||||||
distance = rotation.angle();
|
|
||||||
} else
|
|
||||||
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
|
||||||
}
|
}
|
||||||
success = distance >= min_distance;
|
|
||||||
if (!success) {
|
// add an arrow marker
|
||||||
char msg[100];
|
visualization_msgs::Marker m;
|
||||||
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
|
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
|
||||||
trajectory.setName(msg);
|
m.ns = props.get<std::string>("marker_ns");
|
||||||
|
if (!m.ns.empty()) {
|
||||||
|
m.header.frame_id = scene->getPlanningFrame();
|
||||||
|
if (linear_norm > 1e-3) {
|
||||||
|
// +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder)
|
||||||
|
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
|
||||||
|
: rviz_marker_tools::RED);
|
||||||
|
rviz_marker_tools::makeArrow(m, linear_norm);
|
||||||
|
auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear);
|
||||||
|
Eigen::Vector3d pos(link_pose.translation());
|
||||||
|
if (dir == BACKWARD) {
|
||||||
|
// flip arrow direction
|
||||||
|
quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
|
||||||
|
// arrow tip at goal_pose
|
||||||
|
pos += quat * Eigen::Vector3d(-linear_norm, 0, 0);
|
||||||
|
}
|
||||||
|
tf::pointEigenToMsg(pos, m.pose.position);
|
||||||
|
tf::quaternionEigenToMsg(quat, m.pose.orientation);
|
||||||
|
trajectory.markers().push_back(m);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
|
||||||
success = true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// store result
|
// store result
|
||||||
@ -271,31 +254,6 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
trajectory.setTrajectory(robot_trajectory);
|
trajectory.setTrajectory(robot_trajectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
// add an arrow marker
|
|
||||||
visualization_msgs::Marker m;
|
|
||||||
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
|
|
||||||
m.ns = props.get<std::string>("marker_ns");
|
|
||||||
if (!m.ns.empty()) {
|
|
||||||
m.header.frame_id = scene->getPlanningFrame();
|
|
||||||
if (linear_norm > 1e-3) {
|
|
||||||
// +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder)
|
|
||||||
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
|
|
||||||
: rviz_marker_tools::RED);
|
|
||||||
rviz_marker_tools::makeArrow(m, linear_norm);
|
|
||||||
auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear);
|
|
||||||
Eigen::Vector3d pos(link_pose.translation());
|
|
||||||
if (dir == BACKWARD) {
|
|
||||||
// flip arrow direction
|
|
||||||
quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
|
|
||||||
// arrow tip at goal_pose
|
|
||||||
pos += quat * Eigen::Vector3d(-linear_norm, 0, 0);
|
|
||||||
}
|
|
||||||
tf::pointEigenToMsg(pos, m.pose.position);
|
|
||||||
tf::quaternionEigenToMsg(quat, m.pose.orientation);
|
|
||||||
trajectory.markers().push_back(m);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -83,10 +83,10 @@ void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, doub
|
|||||||
p.set("max_distance", max_distance);
|
p.set("max_distance", max_distance);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PickPlaceBase::setRelativeJointSpaceGoal(const std::map<std::string, double>& relative_joint_space_goal)
|
void PickPlaceBase::setLiftPlace(const std::map<std::string, double>& joints)
|
||||||
{
|
{
|
||||||
auto& p = lift_stage_->properties();
|
auto& p = lift_stage_->properties();
|
||||||
p.set("relative_joint_space_goal", relative_joint_space_goal);
|
p.set("joints", joints);
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user