refactored IK to try out point-ik

also fixed bug (?) in bio_ik linking TAMS-Group/bio_ik#29
UM-ARM-Lab · Jul 13, 2021 · feb5754 · feb5754
1 parent e3cd6ed
commit feb5754
Show file tree

Hide file tree

Showing 5 changed files with 77 additions and 19 deletions.
diff --git a/jacobian_follower/CMakeLists.txt b/jacobian_follower/CMakeLists.txt
@@ -3,15 +3,16 @@ project(jacobian_follower)
 
 set(CATKIN_PACKAGES
         arc_utilities
-        moveit_ros_planning
         arm_robots_msgs
-        roscpp
-        tf2_ros
-        tf2_msgs
-        pyrosmsg
+        bio_ik
+        eigen_conversions
+        moveit_ros_planning
         moveit_visual_tools
         pybind11_catkin
-        eigen_conversions
+        pyrosmsg
+        roscpp
+        tf2_msgs
+        tf2_ros
         )
 
 find_package(catkin REQUIRED COMPONENTS

diff --git a/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp b/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp
@@ -145,10 +145,14 @@ class JacobianFollower {
   std::vector<std::vector<double>> compute_IK_solutions(geometry_msgs::Pose target_pose,
                                                         const std::string &group_name) const;
 
-  std::optional<moveit_msgs::RobotState> computeCollisionFreeIK(const std::vector<geometry_msgs::Pose> &target_pose,
-                                                                const std::string &group_name,
-                                                                const std::vector<std::string> &tip_names,
-                                                                const moveit_msgs::PlanningScene &scene_smg) const;
+  std::optional<moveit_msgs::RobotState> computeCollisionFreePointIK(
+      const std::vector<geometry_msgs::Point> &target_point, const std::string &group_name,
+      const std::vector<std::string> &tip_names, const moveit_msgs::PlanningScene &scene_smg) const;
+
+  std::optional<moveit_msgs::RobotState> computeCollisionFreePoseIK(const std::vector<geometry_msgs::Pose> &target_pose,
+                                                                    const std::string &group_name,
+                                                                    const std::vector<std::string> &tip_names,
+                                                                    const moveit_msgs::PlanningScene &scene_smg) const;
 
   geometry_msgs::Pose computeGroupFK(const moveit_msgs::RobotState &robot_state_msg,
                                      const std::string &group_name) const;

diff --git a/jacobian_follower/package.xml b/jacobian_follower/package.xml
@@ -9,13 +9,14 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>arc_utilities</depend>
-  <depend>moveit_ros_planning</depend>
   <depend>arm_robots_msgs</depend>
-  <depend>roscpp</depend>
-  <depend>pyrosmsg</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_msgs</depend>
+  <depend>bio_ik</depend>
+  <depend>eigen_conversions</depend>
+  <depend>moveit_ros_planning</depend>
   <depend>moveit_visual_tools</depend>
   <depend>pybind11_catkin</depend>
-  <depend>eigen_conversions</depend>
+  <depend>pyrosmsg</depend>
+  <depend>roscpp</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf2_ros</depend>
 </package>
diff --git a/jacobian_follower/src/jacobian_follower/bindings.cpp b/jacobian_follower/src/jacobian_follower/bindings.cpp
@@ -36,7 +36,9 @@ PYBIND11_MODULE(pyjacobian_follower, m) {
           py::arg("max_velocity_scaling_factor"), py::arg("max_acceleration_scaling_factor"))
       .def("compute_IK_solutions", &JacobianFollower::compute_IK_solutions, py::arg("pose"),
            py::arg("joint_group_name"))
-      .def("compute_collision_free_ik", &JacobianFollower::computeCollisionFreeIK, py::arg("poses"),
+      .def("compute_collision_free_pose_ik", &JacobianFollower::computeCollisionFreePoseIK, py::arg("poses"),
+           py::arg("group_name"), py::arg("tip_names"), py::arg("scene_name"))
+      .def("compute_collision_free_point_ik", &JacobianFollower::computeCollisionFreePointIK, py::arg("points"),
            py::arg("group_name"), py::arg("tip_names"), py::arg("scene_name"))
       .def("group_fk",
            py::overload_cast<const std::vector<double> &, const std::vector<std::string> &, const std::string &>(

diff --git a/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp b/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp
@@ -1,4 +1,5 @@
 #include <arc_utilities/enumerate.h>
+#include <bio_ik/bio_ik.h>
 #include <moveit/robot_state/conversions.h>
 #include <moveit_msgs/DisplayTrajectory.h>
 #include <moveit_visual_tools/moveit_visual_tools.h>
@@ -13,6 +14,7 @@
 #include <arc_utilities/moveit_pose_type.hpp>
 #include <arc_utilities/ostream_operators.hpp>
 #include <arc_utilities/ros_helpers.hpp>
+#include <boost/range/combine.hpp>
 #include <exception>
 #include <jacobian_follower/jacobian_follower.hpp>
 #include <jacobian_follower/jacobian_utils.hpp>
@@ -249,7 +251,56 @@ bool isStateValid(planning_scene::PlanningScenePtr planning_scene, moveit::core:
   return planning_scene->isStateValid(*robot_state);
 }
 
-std::optional<moveit_msgs::RobotState> JacobianFollower::computeCollisionFreeIK(
+std::optional<moveit_msgs::RobotState> JacobianFollower::computeCollisionFreePointIK(
+    const std::vector<geometry_msgs::Point> &target_point, const std::string &group_name,
+    const std::vector<std::string> &tip_names, const moveit_msgs::PlanningScene &scene_msg) const {
+  auto joint_model_group = model_->getJointModelGroup(group_name);
+  ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".ik", "Tips: " << tip_names);
+
+  bio_ik::BioIKKinematicsQueryOptions opts;
+  opts.replace = true;
+  opts.return_approximate_solution = false;
+
+  for (auto tup : boost::combine(target_point, tip_names)) {
+    auto goal = std::make_unique<bio_ik::PositionGoal>();
+    std::string name;
+    geometry_msgs::Point p;
+    boost::tie(p, name) = tup;
+    goal->setPosition(tf2::Vector3(p.x, p.y, p.z));
+    goal->setLinkName(name);
+    opts.goals.emplace_back(goal.get());
+  }
+
+  robot_state::RobotState robot_state_ik(model_);
+  robot_state_ik.setToDefaultValues();
+  robot_state_ik.update();
+
+  // Collision checking
+  auto planning_scene = std::make_shared<planning_scene::PlanningScene>(model_);
+  planning_scene->usePlanningSceneMsg(scene_msg);
+  moveit::core::GroupStateValidityCallbackFn constraint_fn_boost;
+  constraint_fn_boost = boost::bind(&isStateValid, planning_scene, _1, _2, _3);
+
+  bool ok = robot_state_ik.setFromIK(joint_model_group,            // joints to be used for IK
+                                     EigenSTL::vector_Isometry3d(),  // this isn't used, goals are described in opts
+                                     std::vector<std::string>(),   // names of the end-effector links
+                                     0.1,                          // timeout
+                                     constraint_fn_boost,
+                                     opts  // mostly empty
+  );
+  ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".ik", "ok? " << ok);
+
+  if (ok) {
+    moveit_msgs::RobotState solution_msg;
+    moveit::core::robotStateToRobotStateMsg(robot_state_ik, solution_msg);
+    ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".ik", "sln: " << solution_msg.joint_state.position);
+    return solution_msg;
+  }
+
+  return {};
+}
+
+std::optional<moveit_msgs::RobotState> JacobianFollower::computeCollisionFreePoseIK(
     const std::vector<geometry_msgs::Pose> &target_pose, const std::string &group_name,
     const std::vector<std::string> &tip_names, const moveit_msgs::PlanningScene &scene_msg) const {
   auto joint_model_group = model_->getJointModelGroup(group_name);
@@ -262,7 +313,6 @@ std::optional<moveit_msgs::RobotState> JacobianFollower::computeCollisionFreeIK(
   robot_state_ik.update();
 
   auto const tip_transforms = EigenHelpersConversions::VectorGeometryPoseToVectorIsometry3d(target_pose);
-  ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".ik", "target " << tip_transforms[0].matrix());
 
   // Collision checking
   auto planning_scene = std::make_shared<planning_scene::PlanningScene>(model_);