Migration of sr_hand_kinematics from arm_navigation to moveit

sr_hand_kinematics/hand_kinematics/CMakeLists.txt

@@ -16,20 +16,28 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
-#  Uncomment this if you want the plugin interface to be compiled (requires arm_navigation stack)
-#rosbuild_add_library(hand_kinematics
-#	 src/hand_kinematics_coupling_plugin.cpp
-#	    src/hand_kinematics_utils.cpp
-#  )
+# Comment out all the library build if you don't want the plugin interface to be compiled
+rosbuild_add_library(hand_kinematics
+	 src/hand_kinematics_coupling_plugin.cpp
+    src/hand_kinematics_utils.cpp
+  )
 
 rosbuild_add_library(hand_kinematics_coupling_lib
 	         src/hand_kinematics_coupling.cpp )
+target_link_libraries(hand_kinematics_coupling_lib moveit_kinematics_base)
 
 rosbuild_add_executable(hand_kinematics_coupling src/hand_kinematics_coupling.cpp)
 
-rosbuild_add_executable(circle_ik test/circle_ik.cpp)
-rosbuild_add_executable(square_ik_th test/square_ik_th.cpp)
-rosbuild_add_executable(upmc_fkik_test test/upmc_fkik_test.cpp)
+rosbuild_add_executable(test/circle_ik test/circle_ik.cpp)
+rosbuild_add_executable(test/square_ik_th test/square_ik_th.cpp)
+rosbuild_add_executable(test/upmc_fkik_test test/upmc_fkik_test.cpp)
+target_link_libraries(test/circle_ik hand_kinematics_coupling_lib)
+target_link_libraries(test/square_ik_th hand_kinematics_coupling_lib)
+
+# Comment out the plugin test if you did not build the plugin library
+rosbuild_add_gtest(test/test_kinematics_as_plugin test/test_kinematics_as_plugin.cpp)
+target_link_libraries(test/test_kinematics_as_plugin moveit_kinematics_base)
+target_link_libraries(test/test_kinematics_as_plugin hand_kinematics_coupling_lib)
+
+rosbuild_add_gtest(test/test_kinematics_as_service test/test_kinematics_as_service.cpp)
 
-target_link_libraries(circle_ik hand_kinematics_coupling_lib)
-target_link_libraries(square_ik_th hand_kinematics_coupling_lib)

sr_hand_kinematics/hand_kinematics/README.txt

@@ -1,20 +1,20 @@
 Copyright UPMC 2012
+Updated by Guillaume WALCK CITEC University Bielefeld 2014
 
 DESCRIPTION
 -----------
 Hand_kinematics provides inverse kinematics for the Shadow Hand fingers and thumb, coping with coupled joints in the fingers and redundant joints in the thumb.
 The IK services requires a 3D pose to be requested and not a 6D pose since 6D requests are most of the time unfeasible (only 3 to 5 DOF)
 However, the request pose message must be 6D, only 3D translation part will be considered.
 
-This code is based on some modified functions taken out of kdl and augmented with coupling possibilities. These functions are in package kdl_coupling, under the same KDL:: namespace and can work together with standard KDL functionnalities. Indeed all the functions created have different names.
+This code is based on some modified functions taken out of kdl and augmented with coupling possibilities. These functions are in package kdl_coupling, 
+under the same KDL:: namespace and can work together with standard KDL functionnalities. Indeed all the functions created have different names.
 
 
 PRE-REQUEST
 -----------
-This package provides several interfaces, one of them is a plugin for constraint aware kinematics that depends on arm_navigation stack. It is deactivated by default
-The other interfaces have less complex dependencies.
-
-If you need the plugin interface, you need to uncomment pluginlib and kinematic_base package in the manifest and uncomment the library in CMakelists.txt
+This package provides several interfaces, one of them is a plugin for constraint aware kinematics that depends on moveit_core package whereas the service interface only requires moveit_msgs. 
+The plugin is now activated by default, comment out the dependencies in manifest and CMakelists.txt if you don't want it.
 
 INSTALL
 -------
@@ -23,13 +23,18 @@ rosmake hand_kinematics --rosdep-install
 
 USAGE
 -----
-Launch files are provided to start the 5 kinematics services (one for each finger), either in standalone version (requires sr_hand to find URDF file) or in standard version supposing you have already LOADED the robot_description on the parameter server. This robot description MUST contain tip frames (only in newer versions of URDF files)
+Launch files are provided to start the 5 kinematics services (one for each finger), either in standalone version (requires sr_hand to find URDF file)
+or in standard version supposing you have already LOADED the robot_description on the parameter server. This robot description MUST contain tip frames (only in newer versions of URDF files)
 
 
 TEST
 ----
 To test the FK/IK several possibilities are offered : 
-1) Use provided tests 
+1) Automatic test
+  * start a roscore
+	* rostest hand_kinematics hand_kinematics_services_test.test
+	* rostest hand_kinematics hand_kinematics_plugin_test.test
+2) Manual provided tests 
        * start a simulation of the hand :
 	roslaunch sr_hand gazebo_arm_and_hand_motor.launch
 	roslaunch sr_hand sr_arm_motor.launch
@@ -50,7 +55,7 @@ To test the FK/IK several possibilities are offered :
 
 	look at the provided snapshots in test folder to verify what you should see.
 
-2) Use provided command line requests (test/command_line_tests.txt) and paste them into the shell (works for first finger)
-3) Use standard IK tutorials from ROS and adapt the service topics you request from
+3) Use provided command line requests (test/command_line_tests.txt) and paste them into the shell (works for first finger)
+4) Use standard IK tutorials from ROS and adapt the service topics you request from
 
 

sr_hand_kinematics/hand_kinematics/bin/test/.gitignore

@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore

sr_hand_kinematics/hand_kinematics/include/hand_kinematics/hand_kinematics_plugin.h

@@ -55,17 +55,14 @@
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl_coupling/chainiksolverpos_nr_jl_coupling.hpp>
 #include <kdl_coupling/chainiksolvervel_wdls_coupling.hpp>
-#include <kinematics_msgs/GetPositionFK.h>
-#include <kinematics_msgs/GetPositionIK.h>
-#include <kinematics_msgs/GetKinematicSolverInfo.h>
-#include <kinematics_msgs/KinematicSolverInfo.h>
+#include <moveit_msgs/KinematicSolverInfo.h>
 #include <urdf/model.h>
 #include <string>
 #include <vector>
 #include <tf_conversions/tf_kdl.h>
-#include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
 #include <boost/shared_ptr.hpp>
-#include <kinematics_base/kinematics_base.h>
+#include <moveit/kinematics_base/kinematics_base.h>
 
 namespace hand_kinematics
 {
@@ -86,114 +83,123 @@ class HandKinematicsPlugin : public kinematics::KinematicsBase
 
     /**
      * @brief Given a desired pose of the end-effector, compute the joint angles to reach it
-     * @param ik_link_name - the name of the link for which IK is being computed
-     * @param ik_pose the desired pose of the link
-     * @param ik_seed_state an initial guess solution for the inverse kinematics
      * @return True if a valid solution was found, false otherwise
      */
-    bool getPositionIK(const geometry_msgs::Pose &ik_pose,
-                       const std::vector<double> &ik_seed_state,
-                       std::vector<double> &solution,
-		       int &error_code);      
+    virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
+                             const std::vector<double> &ik_seed_state,
+                             std::vector<double> &solution,
+                             moveit_msgs::MoveItErrorCodes &error_code,
+                             const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+    virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                                const std::vector<double> &ik_seed_state,
+                                double timeout,
+                                std::vector<double> &solution,
+                                moveit_msgs::MoveItErrorCodes &error_code,
+                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+
 
+    virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                                const std::vector<double> &ik_seed_state,
+                                double timeout,
+                                const std::vector<double> &consistency_limits,
+                                std::vector<double> &solution,
+                                moveit_msgs::MoveItErrorCodes &error_code,
+                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+
     /**
      * @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
-     * @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,
-                          const double &timeout,
-                          std::vector<double> &solution,
-			  int &error_code);      
+    virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                                const std::vector<double> &ik_seed_state,
+                                double timeout,
+                                std::vector<double> &solution,
+                                const IKCallbackFn &solution_callback,
+                                moveit_msgs::MoveItErrorCodes &error_code,
+                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
     /**
      * @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
-     * @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,
-                          const double &timeout,
-                          std::vector<double> &solution,
-                          const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
-                          const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
-			  int &error_code);      
+    virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                                const std::vector<double> &ik_seed_state,
+                                double timeout,
+                                const std::vector<double> &consistency_limits,
+                                std::vector<double> &solution,
+                                const IKCallbackFn &solution_callback,
+                                moveit_msgs::MoveItErrorCodes &error_code,
+                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+
 
     /**
      * @brief Given a set of joint angles and a set of links, compute their pose
-     * @param request  - the request contains the joint angles, set of links for which poses are to be computed and a timeout
-     * @param response - the response contains stamped pose information for all the requested links
+     * @param link_names - set of links for which poses are to be computed
+     * @param joint_angles - the contains the joint angles 
+     * @param poses - the response contains pose information for all the requested links
      * @return True if a valid solution was found, false otherwise
      */
-    bool getPositionFK(const std::vector<std::string> &link_names,
-                       const std::vector<double> &joint_angles, 
-                       std::vector<geometry_msgs::Pose> &poses);
+    virtual bool getPositionFK(const std::vector<std::string> &link_names,
+                            const std::vector<double> &joint_angles,
+                            std::vector<geometry_msgs::Pose> &poses) const;
+
+
 
     /**
      * @brief  Initialization function for the kinematics
      * @return True if initialization was successful, false otherwise
      */
-    bool initialize(std::string name);
-
-    /**
-     * @brief  Return the frame in which the kinematics is operating
-     * @return the string name of the frame in which the kinematics is operating
-     */
-     std::string getBaseFrame();
-
-    /**
-     * @brief  Return the links for which kinematics can be computed
-     */
-    std::string getToolFrame();
-
+    virtual bool initialize(const std::string& robot_description,
+                          const std::string& group_name,
+                          const std::string& base_frame,
+                          const std::string& tip_frame,
+                          double search_discretization);
+
+
     /**
-     * @brief  Return all the joint names in the order they are used internally
+     * @brief Return all the joint names in the order they are used internally
      */
-    std::vector<std::string> getJointNames();
-    
+      const std::vector<std::string>& getJointNames() const;
+
     /**
-     * @brief  Return all the link names in the order they are represented internally
+     * @brief Return all the link names in the order they are represented internally
      */
-    std::vector<std::string> getLinkNames();
-
-    protected:
+      const std::vector<std::string>& getLinkNames() const;
+      
+  protected:
 
     bool active_;
-    int free_angle_;
     urdf::Model robot_model_;
     double search_discretization_;
     ros::NodeHandle node_handle_, root_handle_;
 
 
     KDL::ChainFkSolverPos_recursive* fk_solver;
-	  KDL::ChainIkSolverPos_NR_JL_coupling *ik_solver_pos;
+    KDL::ChainIkSolverPos_NR_JL_coupling *ik_solver_pos;
     KDL::ChainIkSolverVel_wdls_coupling *ik_solver_vel;
-    kinematics_msgs::KinematicSolverInfo solver_info_;
+    moveit_msgs::KinematicSolverInfo solver_info_;
 
-    //ros::ServiceServer ik_service_,fk_service_,ik_solver_info_service_,fk_solver_info_service_;
-    //tf::TransformListener tf_;
-    std::string root_name_,finger_base_name, tip_name;
+    std::string finger_base_name;
     int dimension_;
     boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver_;
 
-    //KDL::Chain kdl_chain_;
     KDL::Chain_coupling kdl_chain_;
 
     KDL::JntArray joint_min_, joint_max_;
-    //TODO CHECK IF NEEDED  tf::TransformListener tf_listener;
-    //TODO CHECK IF NEEDED  bool readJoints(urdf::Model &robot_model);
-
+
+    moveit_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
 
-    kinematics_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
-
-
-
+    /**
+    * @brief This method generates a random joint array vector between the joint limits so that local minima in IK can be avoided.
+    * @param Joint vector to be initialized with random values.
+    */  
+    void generateRandomJntSeed(KDL::JntArray &jnt_pos_in) const;
 
   };
 }