Initial MoveIt plugin devel.

ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h

+
+#include <ros/ros.h>
+#include <moveit/kinematics_base/kinematics_base.h>
+#include <moveit_msgs/KinematicSolverInfo.h>
+
+namespace ur_kinematics
+{
+class URKinematicsPlugin : public kinematics::KinematicsBase
+{
+public:
+
+  /**
+* @brief Plugin-able interface to the ur kinematics
+*/
+  URKinematicsPlugin();
+
+  //~URKinematicsPlugin();
+
+  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;
+
+  /**
+* @brief Initialization function for the kinematics
+* @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::string& tip_frame,
+                          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:
+
+  moveit_msgs::KinematicSolverInfo fk_solver_info_, ik_solver_info_;
+};
+}

ur_kinematics/src/ur_moveit_plugin.cpp.bak

+
+#include <ur_kinematics/ur_moveit_plugin.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <ur_kinematics/ur_kin.h>
+
+//register URKinematicsPlugin as a KinematicsBase implementation
+PLUGINLIB_EXPORT_CLASS(ur_kinematics::URKinematicsPlugin,kinematics::KinematicsBase);
+
+namespace ur_kinematics
+{
+  URKinematicsPlugin::URKinematicsPlugin() {}
+  //URKinematicsPlugin::~URKinematicsPlugin() {}
+
+  bool URKinematicsPlugin::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) const
+  {}
+
+  bool URKinematicsPlugin::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) const
+  {}
+
+  bool URKinematicsPlugin::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) const
+  {}
+
+  bool URKinematicsPlugin::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) const
+  {}
+
+  bool URKinematicsPlugin::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) const
+  {}
+
+  bool URKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
+                                         const std::vector<double> &joint_angles,
+                                         std::vector<geometry_msgs::Pose> &poses) const
+  {
+    double q[6];
+    for(int i=0; i<6; i++)
+      q[i] = joint_angles[i];
+    double T[6][16];
+    forward_all(q, T[0], T[1], T[2], T[3], T[4], T[5]);
+    for(int i=0; i<6; i++) {
+      aa
+    }
+  }
+
+  /**
+* @brief Initialization function for the kinematics
+* @return True if initialization was successful, false otherwise
+*/
+  bool URKinematicsPlugin::initialize(const std::string& robot_description,
+                                      const std::string& group_name,
+                                      const std::string& base_frame,
+                                      const std::string& tip_frame,
+                                      double search_discretization)
+  { 
+    setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
+
+    fk_solver_info_.joint_names.push_back("shoulder_pan_joint");
+    fk_solver_info_.joint_names.push_back("shoulder_lift_joint");
+    fk_solver_info_.joint_names.push_back("elbow_joint");
+    fk_solver_info_.joint_names.push_back("wrist_1_joint");
+    fk_solver_info_.joint_names.push_back("wrist_2_joint");
+    fk_solver_info_.joint_names.push_back("wrist_3_joint");
+
+    fk_solver_info_.link_names.push_back("base_link");
+    fk_solver_info_.link_names.push_back("shoulder_link");
+    fk_solver_info_.link_names.push_back("upper_arm_link");
+    fk_solver_info_.link_names.push_back("forearm_link");
+    fk_solver_info_.link_names.push_back("wrist_1_link");
+    fk_solver_info_.link_names.push_back("wrist_2_link");
+    fk_solver_info_.link_names.push_back("wrist_3_link");
+    fk_solver_info_.link_names.push_back("ee_link");
+
+    return true;
+  }
+
+  /**
+* @brief Return all the joint names in the order they are used internally
+*/
+  const std::vector<std::string>& URKinematicsPlugin::getJointNames() const
+  {
+    return fk_solver_info_.joint_names;
+  }
+
+  /**
+* @brief Return all the link names in the order they are represented internally
+*/
+  const std::vector<std::string>& URKinematicsPlugin::getLinkNames() const
+  {
+    return fk_solver_info_.link_names;
+  }
+}

ur_kinematics/ur_moveit_plugins.xml

+<library path="lib/libur10_moveit_plugin">
+  <class name="ur_kinematics/UR10KinematicsPlugin" type="ur_kinematics::URKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+    <description>
+      Analytic kinematics for the Universal Robots UR10.
+      Developed by Kelsey Hawkins from Georgia Tech.
+      See http://hdl.handle.net/1853/50782 for details.
+    </description>
+  </class>
+</library>
+
+<library path="lib/libur5_moveit_plugin">
+  <class name="ur_kinematics/UR5KinematicsPlugin" type="ur_kinematics::URKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+    <description>
+      Analytic kinematics for the Universal Robots UR5.
+      Developed by Kelsey Hawkins from Georgia Tech.
+      See http://hdl.handle.net/1853/50782 for details.
+    </description>
+  </class>
+</library>