IK: combine PRs ros-industrial#239, ros-industrial#305, and ros-indus…

…trial#316 ros-industrial#358

ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h

@@ -1,83 +1,83 @@
 /*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2014, Georgia Tech
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Georgia Tech nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2014, Georgia Tech
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Georgia Tech nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
 
 /* Author: Kelsey Hawkins */
 
 /* Based on orignal source from Willow Garage. License copied below */
 
 /*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2012, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
 
 /* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */
 
 #ifndef UR_KINEMATICS_PLUGIN_
 #define UR_KINEMATICS_PLUGIN_
 
 // ROS
-#include <ros/ros.h>
 #include <random_numbers/random_numbers.h>
+#include <ros/ros.h>
 
 // System
 #include <boost/shared_ptr.hpp>
@@ -90,29 +90,26 @@
 #include <moveit_msgs/MoveItErrorCodes.h>
 
 // KDL
-#include <kdl/jntarray.hpp>
-#include <kdl/chainiksolvervel_pinv.hpp>
-#include <kdl/chainiksolverpos_nr_jl.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/jntarray.hpp>
 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
 
 // MoveIt!
 #include <moveit/kinematics_base/kinematics_base.h>
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_state/robot_state.h>
 
-namespace ur_kinematics
-{
+namespace ur_kinematics {
 /**
-* @brief Specific implementation of kinematics using KDL. This version can be used with any robot.
-*/
-  class URKinematicsPlugin : public kinematics::KinematicsBase
-  {
-    public:
-
+ * @brief Specific implementation of kinematics using KDL. This version can be used with any robot.
+ */
+class URKinematicsPlugin : public kinematics::KinematicsBase {
+  public:
     /**
-* @brief Default constructor
-*/
+     * @brief Default constructor
+     */
     URKinematicsPlugin();
 
     virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
@@ -121,6 +118,12 @@ namespace ur_kinematics
                                moveit_msgs::MoveItErrorCodes &error_code,
                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
 
+    virtual bool getPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
+                               const std::vector<double> &ik_seed_state,
+                               std::vector<std::vector<double>> &solutions,
+                               kinematics::KinematicsResult &result,
+                               const kinematics::KinematicsQueryOptions &options) const;
+
     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                   const std::vector<double> &ik_seed_state,
                                   double timeout,
@@ -153,43 +156,38 @@ namespace ur_kinematics
                                   moveit_msgs::MoveItErrorCodes &error_code,
                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
 
-    virtual bool getPositionFK(const std::vector<std::string> &link_names,
-                               const std::vector<double> &joint_angles,
-                               std::vector<geometry_msgs::Pose> &poses) const;
+    virtual bool getPositionFK(const std::vector<std::string> &link_names, const std::vector<double> &joint_angles, std::vector<geometry_msgs::Pose> &poses) const;
 
-    virtual bool initialize(const std::string &robot_description,
-                            const std::string &group_name,
-                            const std::string &base_name,
-                            const std::string &tip_name,
-                            double search_discretization);
+    virtual bool
+    initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization);
 
     /**
-* @brief Return all the joint names in the order they are used internally
-*/
-    const std::vector<std::string>& getJointNames() const;
+     * @brief Return all the joint names in the order they are used internally
+     */
+    const std::vector<std::string> &getJointNames() const;
 
     /**
-* @brief Return all the link names in the order they are represented internally
-*/
-    const std::vector<std::string>& getLinkNames() const;
+     * @brief Return all the link names in the order they are represented internally
+     */
+    const std::vector<std::string> &getLinkNames() const;
 
   protected:
-
-  /**
-* @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
-* This particular method is intended for "searching" for a solutions by stepping through the redundancy
-* (or other numerical routines).
-* @param ik_pose the desired pose of the 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 solution the solution vector
-* @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 check_consistency Set to true if consistency check needs to be performed
-* @param redundancy The index of the redundant joint
-* @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
-* @return True if a valid solution was found, false otherwise
-*/
+    /**
+     * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
+     * This particular method is intended for "searching" for a solutions by stepping through the redundancy
+     * (or other numerical routines).
+     * @param ik_pose the desired pose of the 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 solution the solution vector
+     * @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 check_consistency Set to true if consistency check needs to be performed
+     * @param redundancy The index of the redundant joint
+     * @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range
+     * [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
+     * @return True if a valid solution was found, false otherwise
+     */
     bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
                           const std::vector<double> &ik_seed_state,
                           double timeout,
@@ -201,21 +199,20 @@ namespace ur_kinematics
 
     virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
 
-  private:
+    bool getAllPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_seed_state, std::vector<std::vector<double>> &solutions) const;
 
+  private:
     bool timedOut(const ros::WallTime &start_time, double duration) const;
 
-
     /** @brief Check whether the solution lies within the consistency limit of the seed state
-* @param seed_state Seed state
-* @param redundancy Index of the redundant joint within the chain
-* @param consistency_limit The returned state for redundant joint should be in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
-* @param solution solution configuration
-* @return true if check succeeds
-*/
-    bool checkConsistency(const KDL::JntArray& seed_state,
-                          const std::vector<double> &consistency_limit,
-                          const KDL::JntArray& solution) const;
+     * @param seed_state Seed state
+     * @param redundancy Index of the redundant joint within the chain
+     * @param consistency_limit The returned state for redundant joint should be in the range
+     * [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
+     * @param solution solution configuration
+     * @return true if check succeeds
+     */
+    bool checkConsistency(const KDL::JntArray &seed_state, const std::vector<double> &consistency_limit, const KDL::JntArray &solution) const;
 
     int getJointIndex(const std::string &name) const;
 
@@ -224,15 +221,13 @@ namespace ur_kinematics
     void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const;
 
     /** @brief Get a random configuration within joint limits close to the seed state
-* @param seed_state Seed state
-* @param redundancy Index of the redundant joint within the chain
-* @param consistency_limit The returned state will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
-* @param jnt_array Returned random configuration
-*/
-    void getRandomConfiguration(const KDL::JntArray& seed_state,
-                                const std::vector<double> &consistency_limits,
-                                KDL::JntArray &jnt_array,
-                                bool lock_redundancy) const;
+     * @param seed_state Seed state
+     * @param redundancy Index of the redundant joint within the chain
+     * @param consistency_limit The returned state will contain a value for the redundant joint in the range
+     * [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
+     * @param jnt_array Returned random configuration
+     */
+    void getRandomConfiguration(const KDL::JntArray &seed_state, const std::vector<double> &consistency_limits, KDL::JntArray &jnt_array, bool lock_redundancy) const;
 
     bool isRedundantJoint(unsigned int index) const;
 
@@ -258,8 +253,8 @@ namespace ur_kinematics
     std::vector<unsigned int> redundant_joints_map_index_;
 
     // Storage required for when the set of redundant joints is reset
-    bool position_ik_; //whether this solver is only being used for position ik
-    robot_model::JointModelGroup* joint_model_group_;
+    bool position_ik_; // whether this solver is only being used for position ik
+    robot_model::JointModelGroup *joint_model_group_;
     double max_solver_iterations_;
     double epsilon_;
     std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints_;
@@ -274,8 +269,7 @@ namespace ur_kinematics
     // UR base link, and the UR tip link to the group tip link
     KDL::Chain kdl_base_chain_;
     KDL::Chain kdl_tip_chain_;
+};
+} // namespace ur_kinematics
 
-  };
-}
-
-#endif
+#endif