fix: Fix the compilation problem of kinematics plugin

aubo_kinematics/CHANGELOG.rst

@@ -0,0 +1,31 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package aubo_kinematics
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.0 (2016-11-15)
+------------------
+
+0.2.2 (2016-10-27)
+------------------
+
+0.2.1 (2016-10-11)
+------------------
+
+0.2.0 (2016-10-11)
+------------------
+
+0.1.2 (2016-09-30)
+------------------
+
+0.1.1 (2016-09-20)
+------------------
+* update CHANGELOG.rst
+* Contributors: robot
+
+0.1.0 (2016-09-19)
+------------------
+* add CHANGELOG.rst
+* update version 0.0.1
+* update README
+* Aubo robotics ROS version 1.0
+* Contributors: robot

aubo_kinematics/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(aubo_kinematics)
+
+add_compile_options(-std=c++11)
+find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core moveit_kinematics moveit_ros_planning_interface moveit_ros_planning pluginlib tf_conversions)
+if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
+    find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core moveit_kinematics moveit_ros_planning_interface
+      moveit_ros_planning pluginlib tf_conversions)
+    Message(STATUS "<<<<<<<<<<<<<<<<<<<<<<<<<Note: Find Package for kinetic in kinematics build")
+endif()
+
+
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES aubo_i3_kinematic aubo_i5_kinematic aubo_i10_kinematic aubo_i3_moveit_kinematic_plugin aubo_i5_moveit_kinematic_plugin aubo_i10_moveit_kinematic_plugin
+  CATKIN_DEPENDS roscpp geometry_msgs moveit_core moveit_ros_planning
+                 pluginlib tf_conversions
+  DEPENDS boost
+)
+
+
+###########
+## Build ##
+###########
+
+include_directories(SYSTEM ${Boost_INCLUDE_DIR})
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+
+#add_library(aubo_i5_kin src/aubo_kin.cpp)
+#set_target_properties(aubo_i5_kin PROPERTIES COMPILE_DEFINITIONS "AUBO_I5_PARAMS")
+
+
+add_library(aubo_i3_kinematic src/kinematics.cpp)
+set_target_properties(aubo_i3_kinematic PROPERTIES COMPILE_DEFINITIONS "AUBO_I3_PARAMS")
+
+add_library(aubo_i5_kinematic src/kinematics.cpp)
+set_target_properties(aubo_i5_kinematic PROPERTIES COMPILE_DEFINITIONS "AUBO_I5_PARAMS")
+
+add_library(aubo_i10_kinematic src/kinematics.cpp)
+set_target_properties(aubo_i10_kinematic PROPERTIES COMPILE_DEFINITIONS "AUBO_I10_PARAMS")
+
+
+add_library(aubo_i3_moveit_kinematic_plugin src/aubo_moveit_plugin.cpp)
+target_link_libraries(aubo_i3_moveit_kinematic_plugin
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  aubo_i3_kinematic)
+
+add_library(aubo_i5_moveit_kinematic_plugin src/aubo_moveit_plugin.cpp)
+target_link_libraries(aubo_i5_moveit_kinematic_plugin
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  aubo_i5_kinematic)
+
+add_library(aubo_i10_moveit_kinematic_plugin src/aubo_moveit_plugin.cpp)
+target_link_libraries(aubo_i10_moveit_kinematic_plugin
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  aubo_i10_kinematic)
+
+#add_library(aubo_i5_moveit_plugin src/aubo_moveit_plugin.cpp)
+#set_target_properties(aubo_i5_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "AUBO_I5_PARAMS")
+#target_link_libraries(aubo_i5_moveit_plugin
+#  ${catkin_LIBRARIES}
+#  ${Boost_LIBRARIES}
+#  aubo_i5_kin
+#  aubo_i5_kinematic)
+
+
+#if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
+#    ## Declare a C++ executable
+#    ## With catkin_make MoveGroupInterface_To_Kinetic package is built within a single CMake context
+#    ## The recommended prefix ensures that target names across packages don't collide
+#    add_executable(test_kinematic src/test_kinematic.cpp)
+#
+#    ## Specify libraries to link a library or executable target against
+#    target_link_libraries(test_kinematic ${catkin_LIBRARIES} ${Boost_LIBRARIES} aubo_i5_kin aubo_i5_kinematic)
+#
+#    Message(STATUS "<<<<<<<<<<<<<<<<<<<<<<<<<Note:build aubo_demo node for kinetic")
+#endif()
+
+
+#############
+## Install ##
+#############
+
+install(TARGETS aubo_i3_kinematic aubo_i5_kinematic aubo_i10_kinematic aubo_i3_moveit_kinematic_plugin aubo_i5_moveit_kinematic_plugin aubo_i10_moveit_kinematic_plugin
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# install header files
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+# install moveit plugin description file
+install(FILES aubo_moveit_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+

aubo_kinematics/aubo_moveit_plugins.xml

@@ -0,0 +1,23 @@
+<library path="lib/libaubo_i3_moveit_kinematic_plugin">
+  <class name="aubo_kinematics/AuboI3KinematicsPlugin" type="aubo_kinematics::AuboKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+    <description>
+      Analytic kinematics for the Aubo Robots I3.
+    </description>
+  </class>
+</library>
+
+<library path="lib/libaubo_i5_moveit_kinematic_plugin">
+  <class name="aubo_kinematics/AuboI5KinematicsPlugin" type="aubo_kinematics::AuboKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+    <description>
+      Analytic kinematics for the Aubo Robots I5.
+    </description>
+  </class>
+</library>
+
+<library path="lib/libaubo_i10_moveit_kinematic_plugin">
+  <class name="aubo_kinematics/AuboI10KinematicsPlugin" type="aubo_kinematics::AuboKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+    <description>
+      Analytic kinematics for the Aubo Robots I10.
+    </description>
+  </class>
+</library>

aubo_kinematics/include/aubo_kinematics/aubo_moveit_plugin.h

@@ -0,0 +1,209 @@
+#ifndef AUBO_KINEMATICS_PLUGIN_
+#define AUBO_KINEMATICS_PLUGIN_
+
+// ROS
+#include <ros/ros.h>
+#include <random_numbers/random_numbers.h>
+
+// System
+#include <boost/shared_ptr.hpp>
+
+// ROS msgs
+#include <geometry_msgs/PoseStamped.h>
+#include <moveit_msgs/GetPositionFK.h>
+#include <moveit_msgs/GetPositionIK.h>
+#include <moveit_msgs/KinematicSolverInfo.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
+//#include <moveit_msgs/GetKinematicSolverInfo.h>
+
+// KDL
+#include <kdl/jntarray.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <moveit/kdl_kinematics_plugin/joint_mimic.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 aubo_kinematics
+{
+/**
+* @brief Specific implementation of kinematics using KDL. This version can be used with any robot.
+*/
+  class AuboKinematicsPlugin : public kinematics::KinematicsBase
+  {
+    public:
+
+    /**
+* @brief Default constructor
+*/
+    AuboKinematicsPlugin();
+
+    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;
+
+    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;
+
+    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;
+
+    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);
+
+    /**
+* @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;
+
+  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
+*/
+    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 std::vector<double> &consistency_limits,
+                          const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+    virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
+
+  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;
+
+    int getJointIndex(const std::string &name) const;
+
+    int getKDLSegmentIndex(const std::string &name) const;
+
+    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;
+
+    bool isRedundantJoint(unsigned int index) const;
+
+    bool active_; /** Internal variable that indicates whether solvers are configured and ready */
+
+    moveit_msgs::KinematicSolverInfo ik_chain_info_; /** Stores information for the inverse kinematics solver */
+
+    moveit_msgs::KinematicSolverInfo fk_chain_info_; /** Store information for the forward kinematics solver */
+
+    KDL::Chain kdl_chain_;
+
+    unsigned int dimension_; /** Dimension of the group */
+
+    KDL::JntArray joint_min_, joint_max_; /** Joint limits */
+
+    mutable random_numbers::RandomNumberGenerator random_number_generator_;
+
+    robot_model::RobotModelPtr robot_model_;
+
+    robot_state::RobotStatePtr state_, state_2_;
+
+    int num_possible_redundant_joints_;
+    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_;
+    double max_solver_iterations_;
+    double epsilon_;
+    std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints_;
+
+    std::vector<double> ik_weights_;
+    std::vector<std::string> aubo_joint_names_;
+    std::vector<std::string> aubo_link_names_;
+    int aubo_joint_inds_start_;
+    std::string arm_prefix_;
+
+    // kinematic chains representing the chain from the group base link to the
+    // Aubo base link, and the UR tip link to the group tip link
+    KDL::Chain kdl_base_chain_;
+    KDL::Chain kdl_tip_chain_;
+
+  };
+}
+
+#endif