add bio-ik usage

currently breaks loading the robot description when used from python!?
see TAMS-Group/bio_ik#29

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

jacobian_follower/include/jacobian_follower/jacobian_follower.hpp

@@ -125,25 +125,29 @@ class JacobianFollower {
   [[nodiscard]] bool isRequestValid(JacobianWaypointsCommand waypoints_command) const;
 
   PlanResultMsg plan(std::string const &group_name, std::vector<std::string> const &tool_names,
-                                std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
-                                std::vector<PointSequence> const &grippers, double max_velocity_scaling_factor,
-                                double max_acceleration_scaling_factor);
+                     std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
+                     std::vector<PointSequence> const &grippers, double max_velocity_scaling_factor,
+                     double max_acceleration_scaling_factor);
 
   PlanResultMsg plan(std::string const &group_name, std::vector<std::string> const &tool_names,
-                                std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
-                                moveit_msgs::RobotState const &start_state_msg,
-                                std::vector<PointSequence> const &grippers, double max_velocity_scaling_factor,
-                                double max_acceleration_scaling_factor);
+                     std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
+                     moveit_msgs::RobotState const &start_state_msg, std::vector<PointSequence> const &grippers,
+                     double max_velocity_scaling_factor, double max_acceleration_scaling_factor);
 
   PlanResultMsg plan(std::string const &group_name, std::vector<std::string> const &tool_names,
-                                          std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
-                                          moveit_msgs::RobotState const &start_state_msg,
-                                          moveit_msgs::PlanningScene const &scene_msg,
-                                          std::vector<PointSequence> const &grippers,
-                                          double max_velocity_scaling_factor, double max_acceleration_scaling_factor);
+                     std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
+                     moveit_msgs::RobotState const &start_state_msg, moveit_msgs::PlanningScene const &scene_msg,
+                     std::vector<PointSequence> const &grippers, double max_velocity_scaling_factor,
+                     double max_acceleration_scaling_factor);
 
   // TODO: Return std::vector<std_msgs/JointState.msg> to avoid ambiguity in the joint ordering?
-  std::vector<std::vector<double>> compute_IK_solutions(geometry_msgs::Pose target_pose, const std::string &group_name) const;
+  std::vector<std::vector<double>> compute_IK_solutions(geometry_msgs::Pose target_pose,
+                                                        const std::string &group_name) const;
+
+  std::tuple<bool, moveit_msgs::RobotState> compute_position_IK(
+      const std::string &group_name, std::vector<std::string> const &tip_names,
+      moveit_msgs::PlanningScene const &scene_msg,
+      std::vector<Eigen::Vector3d> const &target_positions) const;
 
   // TODO: Accept std_msgs/JointState.msg to avoid ambiguity in the joint ordering?
   geometry_msgs::Pose computeFK(const std::vector<double> &joint_angles, const std::string &group_name) const;
@@ -171,7 +175,8 @@ class JacobianFollower {
                                         std::vector<std::string> const &tool_names,
                                         robot_state::RobotState const &state, PoseSequence const &robotTservo);
 
-  collision_detection::CollisionResult checkCollision(planning_scene::PlanningScenePtr planning_scene, robot_state::RobotState const &state);
+  collision_detection::CollisionResult checkCollision(planning_scene::PlanningScenePtr planning_scene,
+                                                      robot_state::RobotState const &state);
 
   bool check_collision(moveit_msgs::PlanningScene const &scene_msg, moveit_msgs::RobotState const &start_state);
 

jacobian_follower/include/jacobian_follower/jacobian_utils.hpp

@@ -21,3 +21,7 @@ std::pair<Eigen::VectorXd, Eigen::VectorXd> calcVecError(PoseSequence const &err
 
 Matrix6Xd getJacobianServoFrame(moveit::core::JointModelGroup const *jmg, robot_state::RobotState const &state,
                                 robot_model::LinkModel const *link, Pose const &robotTservo);
+
+bool is_ik_valid(std::shared_ptr<planning_scene::PlanningScene> planning_scene, bool verbose, bool only_check_self_collision,
+                 moveit_visual_tools::MoveItVisualTools visual_tools, moveit::core::RobotState *robot_state,
+                 const moveit::core::JointModelGroup *group, const double *ik_solution);

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>

jacobian_follower/src/jacobian_follower/bindings.cpp

@@ -36,6 +36,7 @@ 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_position_IK", &JacobianFollower::compute_position_IK)
       .def("fk", &JacobianFollower::computeFK, py::arg("joint_angles"), py::arg("joint_group_name"))
       .def("check_collision", &JacobianFollower::check_collision, py::arg("scene"), py::arg("start_state"))
       .def("get_tool_positions", &JacobianFollower::get_tool_positions, py::arg("tool_names"), py::arg("state"))

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>
@@ -12,6 +13,7 @@
 #include <arc_utilities/moveit_pose_type.hpp>
 #include <arc_utilities/ostream_operators.hpp>
 #include <arc_utilities/ros_helpers.hpp>
+#include <boost/iterator/zip_iterator.hpp>
 #include <jacobian_follower/jacobian_follower.hpp>
 #include <jacobian_follower/jacobian_utils.hpp>
 #include <sstream>
@@ -141,12 +143,11 @@ void JacobianFollower::debugLogState(const std::string prefix, robot_state::Robo
   ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".joint_state", prefix << ss.str());
 }
 
-PlanResultMsg JacobianFollower::plan(std::string const &group_name,
-                                                std::vector<std::string> const &tool_names,
-                                                std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
-                                                std::vector<PointSequence> const &grippers,
-                                                double const max_velocity_scaling_factor,
-                                                double const max_acceleration_scaling_factor) {
+PlanResultMsg JacobianFollower::plan(std::string const &group_name, std::vector<std::string> const &tool_names,
+                                     std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
+                                     std::vector<PointSequence> const &grippers,
+                                     double const max_velocity_scaling_factor,
+                                     double const max_acceleration_scaling_factor) {
   scene_monitor_->lockSceneRead();
   auto planning_scene = planning_scene::PlanningScene::clone(scene_monitor_->getPlanningScene());
   scene_monitor_->unlockSceneRead();
@@ -164,13 +165,12 @@ PlanResultMsg JacobianFollower::plan(std::string const &group_name,
   return std::make_pair(plan_msg, plan_result.second);
 }
 
-PlanResultMsg JacobianFollower::plan(std::string const &group_name,
-                                                std::vector<std::string> const &tool_names,
-                                                std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
-                                                moveit_msgs::RobotState const &start_state_msg,
-                                                std::vector<PointSequence> const &grippers,
-                                                double const max_velocity_scaling_factor,
-                                                double const max_acceleration_scaling_factor) {
+PlanResultMsg JacobianFollower::plan(std::string const &group_name, std::vector<std::string> const &tool_names,
+                                     std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
+                                     moveit_msgs::RobotState const &start_state_msg,
+                                     std::vector<PointSequence> const &grippers,
+                                     double const max_velocity_scaling_factor,
+                                     double const max_acceleration_scaling_factor) {
   scene_monitor_->lockSceneRead();
   auto planning_scene = planning_scene::PlanningScene::clone(scene_monitor_->getPlanningScene());
   scene_monitor_->unlockSceneRead();
@@ -188,11 +188,13 @@ PlanResultMsg JacobianFollower::plan(std::string const &group_name,
   return std::make_pair(plan_msg, plan_result.second);
 }
 
-PlanResultMsg JacobianFollower::plan(
-    std::string const &group_name, std::vector<std::string> const &tool_names,
-    std::vector<Eigen::Vector4d> const &preferred_tool_orientations, moveit_msgs::RobotState const &start_state_msg,
-    moveit_msgs::PlanningScene const &scene_msg, std::vector<PointSequence> const &grippers,
-    double const max_velocity_scaling_factor, double const max_acceleration_scaling_factor) {
+PlanResultMsg JacobianFollower::plan(std::string const &group_name, std::vector<std::string> const &tool_names,
+                                     std::vector<Eigen::Vector4d> const &preferred_tool_orientations,
+                                     moveit_msgs::RobotState const &start_state_msg,
+                                     moveit_msgs::PlanningScene const &scene_msg,
+                                     std::vector<PointSequence> const &grippers,
+                                     double const max_velocity_scaling_factor,
+                                     double const max_acceleration_scaling_factor) {
   robot_state::RobotState robot_start_state(model_);
   robotStateMsgToRobotState(start_state_msg, robot_start_state);
   auto planning_scene = std::make_shared<planning_scene::PlanningScene>(model_);
@@ -240,6 +242,71 @@ PlanResult JacobianFollower::plan(JacobianTrajectoryCommand traj_command) {
   return {robot_trajectory, reached_target};
 }
 
+std::tuple<bool, moveit_msgs::RobotState> JacobianFollower::compute_position_IK(
+    const std::string &group_name, std::vector<std::string> const &tip_names,
+    moveit_msgs::PlanningScene const &scene_msg, std::vector<Eigen::Vector3d> const &target_positions) const {
+  auto joint_model_group = model_->getJointModelGroup(group_name);
+  auto solver = joint_model_group->getSolverInstance();
+
+  if (!solver) {
+    ROS_ERROR_STREAM_NAMED(LOGGER_NAME + ".ik", "Failed to get solver for group " << group_name);
+    moveit_msgs::RobotState empty_msg;
+    return {false, empty_msg};
+  }
+
+  robot_state::RobotState robot_state_ik(model_);
+
+  // traditional "basic" bio-ik usage. The end-effector goal poses
+  // and end-effector link names are passed into the setFromIK()
+  // call. The KinematicsQueryOptions are empty.
+  //
+  EigenSTL::vector_Isometry3d targets_isometry;
+
+  // collision check
+  auto planning_scene = std::make_shared<planning_scene::PlanningScene>(model_);
+  planning_scene->usePlanningSceneMsg(scene_msg);
+
+  moveit::core::GroupStateValidityCallbackFn constraint_fn;
+  bool collision_checking_verbose_ = true;
+  bool only_check_self_collision_ = false;
+
+  constraint_fn =
+      boost::bind(&is_ik_valid, planning_scene, collision_checking_verbose_,  // NOLINT(modernize-avoid-bind)
+                  only_check_self_collision_, visual_tools_, _1, _2, _3);
+
+  auto point_to_isometry = [](Eigen::Vector3d p) {
+    Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity();
+    isometry.translation().x() = p.x();
+    isometry.translation().y() = p.y();
+    isometry.translation().z() = p.z();
+    return isometry;
+  };
+  std::transform(target_positions.cbegin(), target_positions.cend(), std::back_inserter(targets_isometry),
+                 point_to_isometry);
+
+  // make position-only goals
+  bio_ik::BioIKKinematicsQueryOptions opts;
+  opts.replace = true;
+  for (auto i{0u}; i < tip_names.size(); ++i) {
+    auto const &tip_name = tip_names[i];
+    auto const &target_position = target_positions[i];
+    auto *torso_goal = new bio_ik::PositionGoal();
+    torso_goal->setLinkName(tip_name);
+    torso_goal->setWeight(1);
+    // TODO: make a conversion function for this in arc_utilities
+    torso_goal->setPosition(tf2::Vector3(target_position.x(), target_position.y(), target_position.z()));
+    opts.goals.emplace_back(torso_goal);
+  }
+
+  auto const ok = robot_state_ik.setFromIK(joint_model_group, targets_isometry, tip_names, 1, constraint_fn, opts);
+
+  moveit_msgs::RobotState robot_state_ik_msg;
+  if (ok) {
+    robot_state::robotStateToRobotStateMsg(robot_state_ik, robot_state_ik_msg);
+  }
+  return {ok, robot_state_ik_msg};
+}
+
 std::vector<std::vector<double>> JacobianFollower::compute_IK_solutions(geometry_msgs::Pose target_pose,
                                                                         const std::string &group_name) const {
   auto const jmg = model_->getJointModelGroup(group_name);
@@ -363,7 +430,7 @@ PlanResult JacobianFollower::moveInWorldFrame(JacobianWaypointCommand waypoint_c
   auto const traj = jacobianPath3d(waypoint_command.context.planning_scene, waypoint_command.context.world_to_robot,
                                    jmg, waypoint_command.context.tool_names,
                                    waypoint_command.preferred_tool_orientations, tools_waypoint_interpolated);
-  auto const n_waypoints = tools_waypoint_interpolated[0].size(); // just pick gripper 0, they are all the same
+  auto const n_waypoints = tools_waypoint_interpolated[0].size();  // just pick gripper 0, they are all the same
   // NOTE: if the result of jacobianPath3d has the same number of waypoints as the input (tools_waypoint_interpolated)
   // that means it reached the final position
   auto const reached_target = traj.getWayPointCount() == (n_waypoints - 1);

jacobian_follower/src/jacobian_follower/jacobian_utils.cpp

@@ -1,3 +1,6 @@
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
 #include <arc_utilities/eigen_helpers.hpp>
 #include <arc_utilities/eigen_helpers_conversions.hpp>
 #include <arc_utilities/eigen_ros_conversions.hpp>
@@ -103,3 +106,51 @@ Matrix6Xd getJacobianServoFrame(moveit::core::JointModelGroup const *const jmg,
   }
   return jacobian;
 }
+
+bool is_ik_valid(std::shared_ptr<planning_scene::PlanningScene> planning_scene, bool verbose, bool only_check_self_collision,
+                    moveit_visual_tools::MoveItVisualTools visual_tools, moveit::core::RobotState *robot_state,
+                    const moveit::core::JointModelGroup *group, const double *ik_solution) {
+  // Apply IK solution to robot state
+  robot_state->setJointGroupPositions(group, ik_solution);
+  robot_state->update();
+
+  // Ensure there are objects in the planning scene
+  if (false) {
+    const std::size_t num_collision_objects = planning_scene->getWorld()->size();
+    if (num_collision_objects == 0) {
+      ROS_ERROR_STREAM_NAMED("imarker_robot_state",
+                             "No collision objects exist in world, you need at least a table "
+                             "modeled for the controller to work");
+      ROS_ERROR_STREAM_NAMED("imarker_robot_state",
+                             "To fix this, relaunch the teleop/head tracking/whatever MoveIt! "
+                             "node to publish the collision objects");
+      return false;
+    }
+  }
+
+  if (!planning_scene) {
+    ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No planning scene provided");
+    return false;
+  }
+  if (only_check_self_collision) {
+    // No easy API exists for only checking self-collision, so we do it here.
+    // TODO(davetcoleman): move this into planning_scene.cpp
+    collision_detection::CollisionRequest req;
+    req.verbose = verbose;
+    req.group_name = group->getName();
+    collision_detection::CollisionResult res;
+    planning_scene->checkSelfCollision(req, res, *robot_state);
+    if (!res.collision) return true;  // not in collision
+  } else if (!planning_scene->isStateColliding(*robot_state, group->getName()))
+    return true;  // not in collision
+
+  // Display more info about the collision
+  if (verbose) {
+    visual_tools.publishRobotState(*robot_state, rviz_visual_tools::RED);
+    planning_scene->isStateColliding(*robot_state, group->getName(), true);
+    visual_tools.publishContactPoints(*robot_state, planning_scene.get());
+    ROS_WARN_STREAM_THROTTLE_NAMED(2.0, "imarker_robot_state", "Collision in IK CC callback");
+  }
+
+  return false;
+}