From 4cc0451a4f580e25508b57236879a9ea789107ae Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 24 Sep 2015 11:52:05 +0200 Subject: [PATCH 1/3] improved computation of interactive marker size - use parent_link if group == parent_group - scale smaller than 5cm is clipped to 5cm instead of using default - clarified size computation, using diameter of AABB --- .../robot_interaction/robot_interaction.h | 2 ++ robot_interaction/src/robot_interaction.cpp | 34 ++++++++++++++----- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index 101292f2ec..78fe1d3318 100644 --- a/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -184,6 +184,8 @@ class RobotInteraction // interactive marker from other ROS nodes void registerMoveInteractiveMarkerTopic( const std::string marker_name, const std::string& name); + // return the diameter of the sphere that certainly can enclose the AABB of the link + double computeLinkMarkerSize(const std::string &group, const std::string &link); // return the diameter of the sphere that certainly can enclose the AABB of // the links in this group double computeGroupMarkerSize(const std::string &group); diff --git a/robot_interaction/src/robot_interaction.cpp b/robot_interaction/src/robot_interaction.cpp index 71b6061ac5..bf600aa17c 100644 --- a/robot_interaction/src/robot_interaction.cpp +++ b/robot_interaction/src/robot_interaction.cpp @@ -116,9 +116,28 @@ void RobotInteraction::addActiveComponent(const InteractiveMarkerConstructorFn & active_generic_.push_back(g); } +static const double DEFAULT_SCALE = 0.25; +double RobotInteraction::computeLinkMarkerSize(const std::string &group, const std::string &link) +{ + const robot_model::JointModelGroup *jmg = robot_model_->getJointModelGroup(group); + if (!jmg) return DEFAULT_SCALE; + + robot_state::RobotState default_state(robot_model_); + default_state.setToDefaultValues(); + + const robot_model::LinkModel *lm = default_state.getLinkModel(link); + if (!lm) return DEFAULT_SCALE; + + const Eigen::Vector3d &ext = lm->getShapeExtentsAtOrigin(); + // slightly bigger than the link diameter + double s = 1.01 * ext.norm(); + + // clip scale to 5cm + return std::max(0.05, s); +} + double RobotInteraction::computeGroupMarkerSize(const std::string &group) { - static const double DEFAULT_SCALE = 0.25; if (group.empty()) return DEFAULT_SCALE; const robot_model::JointModelGroup *jmg = robot_model_->getJointModelGroup(group); @@ -151,14 +170,11 @@ double RobotInteraction::computeGroupMarkerSize(const std::string &group) hi = hi.cwiseMax(corner2); } - // slightly bigger than the size of the largest end effector dimension - double s = std::max(std::max(hi.x() - lo.x(), hi.y() - lo.y()), hi.z() - lo.z()); - s *= 1.73205081; // sqrt(3) + // slightly bigger than the end-effector diameter + double s = 1.01 * (hi - lo).norm(); - // if the scale is less than 5cm, set it to default - if (s < 0.05) - s = DEFAULT_SCALE; - return s; + // clip scale to 5cm + return std::max(0.05, s); } void RobotInteraction::decideActiveJoints(const std::string &group) @@ -320,7 +336,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string &group, Intera // if we have a separate group for the eef, we compute the scale based on // it; otherwise, we use a default scale active_eef_[i].size = active_eef_[i].eef_group == active_eef_[i].parent_group ? - computeGroupMarkerSize("") : + computeLinkMarkerSize(active_eef_[i].parent_group, active_eef_[i].parent_link) : computeGroupMarkerSize(active_eef_[i].eef_group); ROS_DEBUG_NAMED("robot_interaction", "Found active end-effector '%s', of scale %lf", From d346130c7591e4c7e6512bfb0f35d927c5763e69 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 27 Sep 2015 21:21:16 +0200 Subject: [PATCH 2/3] reworked computeLinkMarkerSize() compute size such that the marker sphere will cover - a spherical link geometry -> AABB.maxCoeff - a cubical link geometry -> AABB.norm -> use average of both values Virtual links (without any shape) will have a size of AABB of zero dims. In this case use the dimensions of the closest parent link instead. --- .../robot_interaction/robot_interaction.h | 2 +- robot_interaction/src/robot_interaction.cpp | 57 ++++++++++++------- 2 files changed, 36 insertions(+), 23 deletions(-) diff --git a/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index 78fe1d3318..bd3dc7f0b9 100644 --- a/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -185,7 +185,7 @@ class RobotInteraction void registerMoveInteractiveMarkerTopic( const std::string marker_name, const std::string& name); // return the diameter of the sphere that certainly can enclose the AABB of the link - double computeLinkMarkerSize(const std::string &group, const std::string &link); + double computeLinkMarkerSize(const std::string &link); // return the diameter of the sphere that certainly can enclose the AABB of // the links in this group double computeGroupMarkerSize(const std::string &group); diff --git a/robot_interaction/src/robot_interaction.cpp b/robot_interaction/src/robot_interaction.cpp index bf600aa17c..1a538421e3 100644 --- a/robot_interaction/src/robot_interaction.cpp +++ b/robot_interaction/src/robot_interaction.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan, Adam Leeper */ -#include +#include "moveit/robot_interaction/robot_interaction.h" #include #include #include @@ -117,23 +117,31 @@ void RobotInteraction::addActiveComponent(const InteractiveMarkerConstructorFn & } static const double DEFAULT_SCALE = 0.25; -double RobotInteraction::computeLinkMarkerSize(const std::string &group, const std::string &link) +double RobotInteraction::computeLinkMarkerSize(const std::string &link) { - const robot_model::JointModelGroup *jmg = robot_model_->getJointModelGroup(group); - if (!jmg) return DEFAULT_SCALE; - - robot_state::RobotState default_state(robot_model_); - default_state.setToDefaultValues(); - - const robot_model::LinkModel *lm = default_state.getLinkModel(link); - if (!lm) return DEFAULT_SCALE; + const robot_model::LinkModel *lm = robot_model_->getLinkModel(link); + double size = 0; - const Eigen::Vector3d &ext = lm->getShapeExtentsAtOrigin(); - // slightly bigger than the link diameter - double s = 1.01 * ext.norm(); + while (lm) { + const Eigen::Vector3d &ext = lm->getShapeExtentsAtOrigin(); + // compute size such that the marker sphere will cover + // - a spherical link geometry -> maxCoeff + // - a cubical link geometry -> norm + // average of AABB diameter (norm) and max AABB extension (maxCoeff) + size = (ext.norm() + ext.maxCoeff()) / 2.0; + if (size > 0) break; // break, if a non-empty shape was found + + // process kinematic chain upwards (but only following fixed joints) + // to find a link with some non-empty shape (to ignore virtual links) + if (lm->getParentJointModel()->getType() == robot_model::JointModel::FIXED) + lm = lm->getParentLinkModel(); + else + lm = 0; + } + if (!lm) return DEFAULT_SCALE; // no link with non-zero shape extends found - // clip scale to 5cm - return std::max(0.05, s); + // the marker sphere will be half the size, so double the size here + return 2. * size; } double RobotInteraction::computeGroupMarkerSize(const std::string &group) @@ -169,12 +177,17 @@ double RobotInteraction::computeGroupMarkerSize(const std::string &group) lo = lo.cwiseMin(corner1); hi = hi.cwiseMax(corner2); } + const Eigen::Vector3d &ext = hi - lo; + + // average of AABB diameter (norm) and max AABB extension (maxCoeff) + double size = (ext.norm() + ext.maxCoeff()) / 2.0; - // slightly bigger than the end-effector diameter - double s = 1.01 * (hi - lo).norm(); + // if size is zero, all links have empty shapes and are placed at same position + // in this case, fall back to link marker size + if (size == 0) return computeLinkMarkerSize(links[0]); - // clip scale to 5cm - return std::max(0.05, s); + // the marker sphere will be half the size, so double the size here + return 2. * size; } void RobotInteraction::decideActiveJoints(const std::string &group) @@ -333,10 +346,10 @@ void RobotInteraction::decideActiveEndEffectors(const std::string &group, Intera for (std::size_t i = 0 ; i < active_eef_.size() ; ++i) { - // if we have a separate group for the eef, we compute the scale based on - // it; otherwise, we use a default scale + // if we have a separate group for the eef, we compute the scale based on it; + // otherwise, we use the size of the parent_link active_eef_[i].size = active_eef_[i].eef_group == active_eef_[i].parent_group ? - computeLinkMarkerSize(active_eef_[i].parent_group, active_eef_[i].parent_link) : + computeLinkMarkerSize(active_eef_[i].parent_link) : computeGroupMarkerSize(active_eef_[i].eef_group); ROS_DEBUG_NAMED("robot_interaction", "Found active end-effector '%s', of scale %lf", From fd83c6ed88bbc8e17b20f4537e4420208d83f9cc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 21 Nov 2015 22:35:45 +0100 Subject: [PATCH 3/3] further adapted marker size computation - drop largest extension dimension (-> use cross-section size of elongated link) - for an end-effector group, consider the sizes of individual links instead of the overall size of all links (which becomes huge very fast) - enlarge marker size by factor of 1.5 when there is only a single eef marker --- robot_interaction/src/robot_interaction.cpp | 41 ++++++++------------- 1 file changed, 16 insertions(+), 25 deletions(-) diff --git a/robot_interaction/src/robot_interaction.cpp b/robot_interaction/src/robot_interaction.cpp index 1a538421e3..73f9378d0b 100644 --- a/robot_interaction/src/robot_interaction.cpp +++ b/robot_interaction/src/robot_interaction.cpp @@ -123,12 +123,11 @@ double RobotInteraction::computeLinkMarkerSize(const std::string &link) double size = 0; while (lm) { - const Eigen::Vector3d &ext = lm->getShapeExtentsAtOrigin(); - // compute size such that the marker sphere will cover - // - a spherical link geometry -> maxCoeff - // - a cubical link geometry -> norm - // average of AABB diameter (norm) and max AABB extension (maxCoeff) - size = (ext.norm() + ext.maxCoeff()) / 2.0; + Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin(); + // drop largest extension and take norm of two remaining + Eigen::MatrixXd::Index maxIndex; + ext.maxCoeff(&maxIndex); ext[maxIndex] = 0; + size = 1.01 * ext.norm(); if (size > 0) break; // break, if a non-empty shape was found // process kinematic chain upwards (but only following fixed joints) @@ -157,30 +156,19 @@ double RobotInteraction::computeGroupMarkerSize(const std::string &group) return DEFAULT_SCALE; // compute the aabb of the links that make up the group - const double inf = std::numeric_limits::infinity(); - Eigen::Vector3d lo( inf, inf, inf); - Eigen::Vector3d hi(-inf, -inf, -inf); - robot_state::RobotState default_state(robot_model_); - default_state.setToDefaultValues(); - + double size = 0; for (std::size_t i = 0 ; i < links.size() ; ++i) { - const robot_model::LinkModel *lm = default_state.getLinkModel(links[i]); + const robot_model::LinkModel *lm = robot_model_->getLinkModel(links[i]); if (!lm) continue; - const Eigen::Vector3d &ext = lm->getShapeExtentsAtOrigin(); - - Eigen::Vector3d corner1 = ext/2.0; - corner1 = default_state.getGlobalLinkTransform(lm) * corner1; - Eigen::Vector3d corner2 = ext/-2.0; - corner2 = default_state.getGlobalLinkTransform(lm) * corner2; - lo = lo.cwiseMin(corner1); - hi = hi.cwiseMax(corner2); - } - const Eigen::Vector3d &ext = hi - lo; + Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin(); - // average of AABB diameter (norm) and max AABB extension (maxCoeff) - double size = (ext.norm() + ext.maxCoeff()) / 2.0; + // drop largest extension and take norm of two remaining + Eigen::MatrixXd::Index maxIndex; + ext.maxCoeff(&maxIndex); ext[maxIndex] = 0; + size = std::max(size, 1.01 * ext.norm()); + } // if size is zero, all links have empty shapes and are placed at same position // in this case, fall back to link marker size @@ -356,6 +344,9 @@ void RobotInteraction::decideActiveEndEffectors(const std::string &group, Intera active_eef_[i].eef_group.c_str(), active_eef_[i].size); } + // if there is only a single end effector marker, we can safely use a larger marker + if (active_eef_.size() == 1) + active_eef_[0].size *= 1.5; } void RobotInteraction::clear()