From f8911d89a94d823a86975b4f3742aedb406bd1c6 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 20 Feb 2014 13:19:17 +0900 Subject: [PATCH 01/10] KinematicsBase support for multiple tip frames and IK requests with multiple poses --- .../moveit/kinematics_base/kinematics_base.h | 119 +++++++++++++++++- kinematics_base/src/kinematics_base.cpp | 21 +++- 2 files changed, 134 insertions(+), 6 deletions(-) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 57c4e28d..cd4e43c2 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -40,6 +40,7 @@ #include #include #include +#include #include /** @brief API for forward and inverse kinematics */ @@ -54,12 +55,14 @@ struct KinematicsQueryOptions { KinematicsQueryOptions() : lock_redundant_joints(false), - return_approximate_solution(false) + return_approximate_solution(false), + solve_non_chains(false) { } bool lock_redundant_joints; bool return_approximate_solution; + bool solve_non_chains; }; @@ -130,7 +133,7 @@ class KinematicsBase const std::vector &consistency_limits, std::vector &solution, moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0; + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -178,6 +181,52 @@ class KinematicsBase moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0; + /** + * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles + * required to reach it them. This is useful for e.g. biped robots that need to perform whole-body IK. + * Not necessary for most robots that have kinematic chains. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_poses the desired pose of each tip link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param timeout The amount of time (in seconds) available to the solver + * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the current seed state + * @param solution the solution vector + * @param desired_pose_callback A callback function for the desired link pose - could be used, e.g. to check for collisions for the end-effector + * @param solution_callback A callback solution for the IK solution + * @param error_code an error code that encodes the reason for failure or success + * @param lock_redundant_joints if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed + * @return True if a valid solution was found, false otherwise + */ + virtual bool searchPositionIK(const std::vector &ik_poses, + const std::vector &ik_seed_state, + double timeout, + const std::vector &consistency_limits, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const + { + // For IK solvers that do not support multiple poses, fall back to single pose call + if (ik_poses.size() == 1) + { + return searchPositionIK(ik_poses[0], + ik_seed_state, + timeout, + consistency_limits, + solution, + solution_callback, + error_code, + options); + } + else + { + // Otherwise throw error + logError("moveit.kinematics_base: This kinematic solver does not support searchPositionIK with multiple poses"); + } + return false; + } + /** * @brief Given a set of joint angles and a set of links, compute their pose * @param link_names A set of links for which FK needs to be computed @@ -204,6 +253,21 @@ class KinematicsBase const std::string& tip_frame, double search_discretization); + /** + * @brief Set the parameters for the solver + * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; + * @param group_name The group for which this solver is being configured + * @param base_frame The base frame in which all input poses are expected. + * This may (or may not) be the root frame of the chain that the solver operates on + * @param tip_frame The tip of the chain + * @param search_discretization The discretization of the search when the solver steps through the redundancy + */ + virtual void setValues(const std::string& robot_description, + const std::string& group_name, + const std::string& base_frame, + const std::vector& tip_frames, + double search_discretization); + /** * @brief Initialization function for the kinematics * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; @@ -220,6 +284,36 @@ class KinematicsBase const std::string& tip_frame, double search_discretization) = 0; + /** + * @brief Initialization function for the kinematics + * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; + * @param group_name The group for which this solver is being configured + * @param base_frame The base frame in which all input poses are expected. + * This may (or may not) be the root frame of the chain that the solver operates on + * @param tip_frame The tip of the chain + * @param search_discretization The discretization of the search when the solver steps through the redundancy + * @return True if initialization was successful, false otherwise + */ + virtual bool initialize(const std::string& robot_description, + const std::string& group_name, + const std::string& base_frame, + const std::vector& tip_frames, + double search_discretization) + { + // For IK solvers that do not support multiple tip frames, fall back to single pose call + if (tip_frames.size() == 1) + { + return initialize(robot_description, + group_name, + base_frame, + tip_frames[0], + search_discretization); + } + + logError("moveit.kinematics_base: This kinematic solver does not support initialization with more than one tip frames"); + return false; + } + /** * @brief Return the name of the group that the solver is operating on * @return The string name of the group that the solver is operating on @@ -240,11 +334,24 @@ class KinematicsBase /** * @brief Return the name of the tip frame of the chain on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. + * Deprecated in favor of getTipFrames(), but will remain for foreseeable future for backwards compatibility * @return The string name of the tip frame of the chain on which the solver is operating */ virtual const std::string& getTipFrame() const { - return tip_frame_; + if (tip_frames_.size() > 1) + logError("moveit.kinematics_base: This kinematic solver has more than one tip frame, do not call getTipFrame()"); + + return tip_frame_; // for backwards-compatibility. should actually use tip_frames_[0] + } + + /** + * @brief Return the name of the tip frame of the chain on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. + * @return The string name of the tip frame of the chain on which the solver is operating + */ + virtual const std::vector& getTipFrames() const + { + return tip_frames_; } /** @@ -319,7 +426,8 @@ class KinematicsBase KinematicsBase() : search_discretization_(DEFAULT_SEARCH_DISCRETIZATION), - default_timeout_(DEFAULT_TIMEOUT) + default_timeout_(DEFAULT_TIMEOUT), + tip_frame_("DEPRECATED") // help users understand why this variable might not be set (if provide multiple tip frames, this variable will be unset) {} protected: @@ -327,7 +435,8 @@ class KinematicsBase std::string robot_description_; std::string group_name_; std::string base_frame_; - std::string tip_frame_; + std::vector tip_frames_; + std::string tip_frame_; // DEPRECATED - this variable only still exists for backwards compatibility with previously generated custom ik solvers like IKFast double search_discretization_; double default_timeout_; std::vector redundant_joint_indices_; diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index d4c99b57..ffc03314 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -48,10 +48,29 @@ void kinematics::KinematicsBase::setValues(const std::string& robot_description, robot_description_ = robot_description; group_name_ = group_name; base_frame_ = removeSlash(base_frame); - tip_frame_ = removeSlash(tip_frame); + tip_frames_.push_back(removeSlash(tip_frame)); + tip_frame_ = tip_frame; // for backwards compatibility search_discretization_ = search_discretization; } +void kinematics::KinematicsBase::setValues(const std::string& robot_description, + const std::string& group_name, + const std::string& base_frame, + const std::vector& tip_frames, + double search_discretization) +{ + robot_description_ = robot_description; + group_name_ = group_name; + base_frame_ = removeSlash(base_frame); + search_discretization_ = search_discretization; + + // Copy tip frames to local vector after stripping slashes + for (std::size_t i = 0; i < tip_frames.size(); ++i) + tip_frames_.push_back(removeSlash(tip_frames[i])); + + // Note that we do not set tip_frame_ variable here, because it should never be used in combination with multiple tip frames +} + bool kinematics::KinematicsBase::setRedundantJoints(const std::vector &redundant_joint_indices) { for(std::size_t i = 0; i < redundant_joint_indices.size(); ++i) From 870fd23ca3cae9e636ac577aea1ac13dc0fdbf95 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 20 Feb 2014 13:29:08 +0900 Subject: [PATCH 02/10] Cleaned up commit --- .../moveit/kinematics_base/kinematics_base.h | 33 +++++++++---------- kinematics_base/src/kinematics_base.cpp | 2 +- 2 files changed, 16 insertions(+), 19 deletions(-) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index cd4e43c2..3696a56c 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -55,14 +55,12 @@ struct KinematicsQueryOptions { KinematicsQueryOptions() : lock_redundant_joints(false), - return_approximate_solution(false), - solve_non_chains(false) + return_approximate_solution(false) { } bool lock_redundant_joints; bool return_approximate_solution; - bool solve_non_chains; }; @@ -133,7 +131,7 @@ class KinematicsBase const std::vector &consistency_limits, std::vector &solution, moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0; + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -219,11 +217,9 @@ class KinematicsBase error_code, options); } - else - { - // Otherwise throw error - logError("moveit.kinematics_base: This kinematic solver does not support searchPositionIK with multiple poses"); - } + + // Otherwise throw error because this function should have been implemented + logError("moveit.kinematics_base: This kinematic solver does not support searchPositionIK with multiple poses"); return false; } @@ -239,7 +235,7 @@ class KinematicsBase std::vector &poses) const = 0; /** - * @brief Set the parameters for the solver + * @brief Set the parameters for the solver, for use with kinematic chain IK solvers * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. @@ -254,12 +250,12 @@ class KinematicsBase double search_discretization); /** - * @brief Set the parameters for the solver + * @brief Set the parameters for the solver, for use with non-chain IK solvers * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. * This may (or may not) be the root frame of the chain that the solver operates on - * @param tip_frame The tip of the chain + * @param tip_frames A vector of tips of the kinematic tree * @param search_discretization The discretization of the search when the solver steps through the redundancy */ virtual void setValues(const std::string& robot_description, @@ -269,7 +265,7 @@ class KinematicsBase double search_discretization); /** - * @brief Initialization function for the kinematics + * @brief Initialization function for the kinematics, for use with kinematic chain IK solvers * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. @@ -285,12 +281,12 @@ class KinematicsBase double search_discretization) = 0; /** - * @brief Initialization function for the kinematics + * @brief Initialization function for the kinematics, for use with non-chain IK solvers * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. * This may (or may not) be the root frame of the chain that the solver operates on - * @param tip_frame The tip of the chain + * @param tip_frames A vector of tips of the kinematic tree * @param search_discretization The discretization of the search when the solver steps through the redundancy * @return True if initialization was successful, false otherwise */ @@ -346,8 +342,9 @@ class KinematicsBase } /** - * @brief Return the name of the tip frame of the chain on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. - * @return The string name of the tip frame of the chain on which the solver is operating + * @brief Return the names of the tip frames of the kinematic tree on which the solver is operating. + * This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. + * @return The vector of names of the tip frames of the kinematic tree on which the solver is operating */ virtual const std::vector& getTipFrames() const { @@ -427,7 +424,7 @@ class KinematicsBase KinematicsBase() : search_discretization_(DEFAULT_SEARCH_DISCRETIZATION), default_timeout_(DEFAULT_TIMEOUT), - tip_frame_("DEPRECATED") // help users understand why this variable might not be set (if provide multiple tip frames, this variable will be unset) + tip_frame_("DEPRECATED") // help users understand why this variable might not be set (if multiple tip frames provided, this variable will be unset) {} protected: diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index ffc03314..b2117427 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -48,8 +48,8 @@ void kinematics::KinematicsBase::setValues(const std::string& robot_description, robot_description_ = robot_description; group_name_ = group_name; base_frame_ = removeSlash(base_frame); + tip_frame_ = removeSlash(tip_frame); // for backwards compatibility tip_frames_.push_back(removeSlash(tip_frame)); - tip_frame_ = tip_frame; // for backwards compatibility search_discretization_ = search_discretization; } From f5183b4e97ccae92d9f768d75992e940bb4f671a Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 21 Feb 2014 10:38:20 +0900 Subject: [PATCH 03/10] Feedback and cleaned up comment lengths --- .../moveit/kinematics_base/kinematics_base.h | 38 ++++++++++++------- kinematics_base/src/kinematics_base.cpp | 5 ++- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 3696a56c..69f1b489 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -180,8 +180,8 @@ class KinematicsBase const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0; /** - * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles - * required to reach it them. This is useful for e.g. biped robots that need to perform whole-body IK. + * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles + * required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. * Not necessary for most robots that have kinematic chains. * This particular method is intended for "searching" for a solutions by stepping through the redundancy * (or other numerical routines). @@ -208,7 +208,7 @@ class KinematicsBase // For IK solvers that do not support multiple poses, fall back to single pose call if (ik_poses.size() == 1) { - return searchPositionIK(ik_poses[0], + return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, @@ -236,7 +236,8 @@ class KinematicsBase /** * @brief Set the parameters for the solver, for use with kinematic chain IK solvers - * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; + * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for; + * For example, the name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. * This may (or may not) be the root frame of the chain that the solver operates on @@ -251,7 +252,8 @@ class KinematicsBase /** * @brief Set the parameters for the solver, for use with non-chain IK solvers - * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; + * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for; + * For example, the name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. * This may (or may not) be the root frame of the chain that the solver operates on @@ -266,7 +268,8 @@ class KinematicsBase /** * @brief Initialization function for the kinematics, for use with kinematic chain IK solvers - * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; + * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for; + * For example, the name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. * This may (or may not) be the root frame of the chain that the solver operates on @@ -282,7 +285,8 @@ class KinematicsBase /** * @brief Initialization function for the kinematics, for use with non-chain IK solvers - * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description; + * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for; + * For example, rhe name of the ROS parameter that contains the robot description; * @param group_name The group for which this solver is being configured * @param base_frame The base frame in which all input poses are expected. * This may (or may not) be the root frame of the chain that the solver operates on @@ -320,7 +324,8 @@ class KinematicsBase } /** - * @brief Return the name of the frame in which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. + * @brief Return the name of the frame in which the solver is operating. This is usually a link name. + * No namespacing (e.g., no "/" prefix) should be used. * @return The string name of the frame in which the solver is operating */ virtual const std::string& getBaseFrame() const @@ -329,7 +334,8 @@ class KinematicsBase } /** - * @brief Return the name of the tip frame of the chain on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. + * @brief Return the name of the tip frame of the chain on which the solver is operating. This is usually a link name. + * No namespacing (e.g., no "/" prefix) should be used. * Deprecated in favor of getTipFrames(), but will remain for foreseeable future for backwards compatibility * @return The string name of the tip frame of the chain on which the solver is operating */ @@ -342,7 +348,7 @@ class KinematicsBase } /** - * @brief Return the names of the tip frames of the kinematic tree on which the solver is operating. + * @brief Return the names of the tip frames of the kinematic tree on which the solver is operating. * This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. * @return The vector of names of the tip frames of the kinematic tree on which the solver is operating */ @@ -352,7 +358,8 @@ class KinematicsBase } /** - * @brief Set a set of redundant joints for the kinematics solver to use. This can fail, depending on the IK solver and choice of redundant joints! + * @brief Set a set of redundant joints for the kinematics solver to use. + * This can fail, depending on the IK solver and choice of redundant joints! * @param redundant_joint_indices The set of redundant joint indices (corresponding to * the list of joints you get from getJointNames()). * @return False if any of the input joint indices are invalid (exceed number of @@ -410,7 +417,8 @@ class KinematicsBase default_timeout_ = timeout; } - /** @brief For functions that require a timeout specified but one is not specified using arguments, this default timeout is used */ + /** @brief For functions that require a timeout specified but one is not specified using arguments, + this default timeout is used */ double getDefaultTimeout() const { return default_timeout_; @@ -424,7 +432,8 @@ class KinematicsBase KinematicsBase() : search_discretization_(DEFAULT_SEARCH_DISCRETIZATION), default_timeout_(DEFAULT_TIMEOUT), - tip_frame_("DEPRECATED") // help users understand why this variable might not be set (if multiple tip frames provided, this variable will be unset) + tip_frame_("DEPRECATED") // help users understand why this variable might not be set + // (if multiple tip frames provided, this variable will be unset) {} protected: @@ -433,7 +442,8 @@ class KinematicsBase std::string group_name_; std::string base_frame_; std::vector tip_frames_; - std::string tip_frame_; // DEPRECATED - this variable only still exists for backwards compatibility with previously generated custom ik solvers like IKFast + std::string tip_frame_; // DEPRECATED - this variable only still exists for backwards compatibility with + // previously generated custom ik solvers like IKFast double search_discretization_; double default_timeout_; std::vector redundant_joint_indices_; diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index b2117427..889fd144 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -65,10 +65,13 @@ void kinematics::KinematicsBase::setValues(const std::string& robot_description, search_discretization_ = search_discretization; // Copy tip frames to local vector after stripping slashes + tip_frames_.clear(); for (std::size_t i = 0; i < tip_frames.size(); ++i) tip_frames_.push_back(removeSlash(tip_frames[i])); - // Note that we do not set tip_frame_ variable here, because it should never be used in combination with multiple tip frames + // Copy tip frames to our legacy variable if only one tip frame is passed in the input vector. Remove eventually. + if (tip_frames.size() == 1) + tip_frame_ = tip_frames[0]; } bool kinematics::KinematicsBase::setRedundantJoints(const std::vector &redundant_joint_indices) From 1ebd58721f55753423014c19c96e965552bf1dd5 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 21 Feb 2014 11:46:17 +0900 Subject: [PATCH 04/10] Fixed missing removeSlash to setValues() --- kinematics_base/src/kinematics_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index 889fd144..c49fa4b4 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -71,7 +71,7 @@ void kinematics::KinematicsBase::setValues(const std::string& robot_description, // Copy tip frames to our legacy variable if only one tip frame is passed in the input vector. Remove eventually. if (tip_frames.size() == 1) - tip_frame_ = tip_frames[0]; + tip_frame_ = removeSlash(tip_frames[0]); } bool kinematics::KinematicsBase::setRedundantJoints(const std::vector &redundant_joint_indices) From 1cb4e1674e4a7987cdddf8688804a76d6d62afa0 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 27 Feb 2014 15:06:21 +0900 Subject: [PATCH 05/10] Authorship --- .../include/moveit/kinematics_base/kinematics_base.h | 2 +- kinematics_base/src/kinematics_base.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 57c4e28d..d16e6bb4 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sachin Chitta */ +/* Author: Sachin Chitta, Dave Coleman */ #ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index d4c99b57..584f0269 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sachin Chitta */ +/* Author: Sachin Chitta, Dave Coleman */ #include From a35382a5b9fb56a9a80a54f3d414bb112c34df6c Mon Sep 17 00:00:00 2001 From: sachinc Date: Mon, 24 Feb 2014 22:58:27 -0800 Subject: [PATCH 06/10] fix for reverting PR #148 --- robot_state/src/conversions.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/robot_state/src/conversions.cpp b/robot_state/src/conversions.cpp index e6b9ce38..b3c9282d 100644 --- a/robot_state/src/conversions.cpp +++ b/robot_state/src/conversions.cpp @@ -360,16 +360,6 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms *tf, const moveit_ missing.erase(vnames[i]); } } - - // Create error message - if (!missing.empty()) - { - std::stringstream missing_frames; - std::copy(missing.begin(), missing.end(), std::ostream_iterator(missing_frames, ", ")); - logError("robot_state: State message is missing required frames: [%s]", missing_frames.str().c_str()); - return false; - } - return true; } else From 6aa705a04669bb4a5e4cca250a19362ca95b4ff2 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 27 Feb 2014 15:06:21 +0900 Subject: [PATCH 07/10] Kinematic base minor change --- .../include/moveit/kinematics_base/kinematics_base.h | 2 +- kinematics_base/src/kinematics_base.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 69f1b489..70340b26 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sachin Chitta */ +/* Author: Sachin Chitta, Dave Coleman */ #ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index c49fa4b4..7036534f 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sachin Chitta */ +/* Author: Sachin Chitta, Dave Coleman */ #include From dcb42ab39188d98d963208eea09f9db846855b57 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 6 Mar 2014 18:35:49 -0700 Subject: [PATCH 08/10] Created supportsGroup() test for IK solvers --- .../moveit/kinematics_base/kinematics_base.h | 18 ++++++++++++++++++ kinematics_base/src/kinematics_base.cpp | 12 ++++++++++++ 2 files changed, 30 insertions(+) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 70340b26..2d25b2b7 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -43,6 +43,14 @@ #include #include +namespace moveit +{ +namespace core +{ +class JointModelGroup; +} +} + /** @brief API for forward and inverse kinematics */ namespace kinematics { @@ -394,6 +402,16 @@ class KinematicsBase */ virtual const std::vector& getLinkNames() const = 0; + + /** + * \brief Only kinematic solvers know if they support a particular kinematic configuration, put the burden on them + * to return an explanation of why they are unable to service a joint model group + * Default implementation for legacy solvers provided + * \param JointModelGroup - the planning group being proposed to be solved by this IK solver + * \return a string specifying the reason the joint model group is not supported, or an empty string if supported + */ + virtual const std::string supportsGroup(const moveit::core::JointModelGroup *jmg) const; + /** * @brief Set the search discretization */ diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index 7036534f..18d709f9 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -35,6 +35,7 @@ /* Author: Sachin Chitta, Dave Coleman */ #include +#include const double kinematics::KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION = 0.1; const double kinematics::KinematicsBase::DEFAULT_TIMEOUT = 1.0; @@ -105,3 +106,14 @@ std::string kinematics::KinematicsBase::removeSlash(const std::string &str) cons { return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str; } + +const std::string kinematics::KinematicsBase::supportsGroup(const moveit::core::JointModelGroup *jmg) const +{ + // Default implementation for legacy solvers: + if (!jmg->isChain()) + { + return "This plugin only supports joint groups which are chains"; + } + + return ""; +} From d369b53743b40299ece84a463a696989bb89c1b2 Mon Sep 17 00:00:00 2001 From: Dave Hershberger Date: Tue, 18 Mar 2014 15:19:04 -0700 Subject: [PATCH 09/10] Changed KinematicsBase::supportsGroup() to use a more standard call signature. --- .../moveit/kinematics_base/kinematics_base.h | 22 ++++++++++++++----- kinematics_base/src/kinematics_base.cpp | 11 +++++++--- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 2d25b2b7..4953f818 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -404,13 +404,23 @@ class KinematicsBase /** - * \brief Only kinematic solvers know if they support a particular kinematic configuration, put the burden on them - * to return an explanation of why they are unable to service a joint model group - * Default implementation for legacy solvers provided - * \param JointModelGroup - the planning group being proposed to be solved by this IK solver - * \return a string specifying the reason the joint model group is not supported, or an empty string if supported + * \brief Check if this solver supports a given JointModelGroup. + * + * Override this function to check if your kinematics solver + * implementation supports the given group. + * + * The default implementation just returns jmg->isChain(), since + * solvers written before this function was added all supported only + * chain groups. + * + * \param jmg the planning group being proposed to be solved by this IK solver + * \param error_text_out If this pointer is non-null and the group is + * not supported, this is filled with a description of why it's not + * supported. + * \return True if the group is supported, false if not. */ - virtual const std::string supportsGroup(const moveit::core::JointModelGroup *jmg) const; + virtual const bool supportsGroup(const moveit::core::JointModelGroup *jmg, + std::string* error_text_out = NULL) const; /** * @brief Set the search discretization diff --git a/kinematics_base/src/kinematics_base.cpp b/kinematics_base/src/kinematics_base.cpp index 18d709f9..acbd9608 100644 --- a/kinematics_base/src/kinematics_base.cpp +++ b/kinematics_base/src/kinematics_base.cpp @@ -107,13 +107,18 @@ std::string kinematics::KinematicsBase::removeSlash(const std::string &str) cons return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str; } -const std::string kinematics::KinematicsBase::supportsGroup(const moveit::core::JointModelGroup *jmg) const +const bool kinematics::KinematicsBase::supportsGroup(const moveit::core::JointModelGroup *jmg, + std::string* error_text_out) const { // Default implementation for legacy solvers: if (!jmg->isChain()) { - return "This plugin only supports joint groups which are chains"; + if(error_text_out) + { + *error_text_out = "This plugin only supports joint groups which are chains"; + } + return false; } - return ""; + return true; } From d0c93af3c82b1b4a8c9efa952fdd0718541a9f65 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 18 Mar 2014 17:47:52 -0600 Subject: [PATCH 10/10] Added support for legacy IK calls without solution_callback --- .../moveit/kinematics_base/kinematics_base.h | 33 ++++++++++++------- 1 file changed, 22 insertions(+), 11 deletions(-) diff --git a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 2d25b2b7..d6dcd590 100644 --- a/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -149,7 +149,6 @@ class KinematicsBase * @param ik_seed_state an initial guess solution for the inverse kinematics * @param timeout The amount of time (in seconds) available to the solver * @param solution the solution vector - * @param desired_pose_callback A callback function for the desired link pose - could be used, e.g. to check for collisions for the end-effector * @param solution_callback A callback solution for the IK solution * @param error_code an error code that encodes the reason for failure or success * @param lock_redundant_joints if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed @@ -172,7 +171,6 @@ class KinematicsBase * @param timeout The amount of time (in seconds) available to the solver * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the current seed state * @param solution the solution vector - * @param desired_pose_callback A callback function for the desired link pose - could be used, e.g. to check for collisions for the end-effector * @param solution_callback A callback solution for the IK solution * @param error_code an error code that encodes the reason for failure or success * @param lock_redundant_joints if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed @@ -198,7 +196,6 @@ class KinematicsBase * @param timeout The amount of time (in seconds) available to the solver * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the current seed state * @param solution the solution vector - * @param desired_pose_callback A callback function for the desired link pose - could be used, e.g. to check for collisions for the end-effector * @param solution_callback A callback solution for the IK solution * @param error_code an error code that encodes the reason for failure or success * @param lock_redundant_joints if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed @@ -216,14 +213,28 @@ class KinematicsBase // For IK solvers that do not support multiple poses, fall back to single pose call if (ik_poses.size() == 1) { - return searchPositionIK(ik_poses[0], - ik_seed_state, - timeout, - consistency_limits, - solution, - solution_callback, - error_code, - options); + // Check if a solution_callback function was provided and call the corresponding function + if (solution_callback) + { + return searchPositionIK(ik_poses[0], + ik_seed_state, + timeout, + consistency_limits, + solution, + solution_callback, + error_code, + options); + } + else + { + return searchPositionIK(ik_poses[0], + ik_seed_state, + timeout, + consistency_limits, + solution, + error_code, + options); + } } // Otherwise throw error because this function should have been implemented