From f46eec79ccfaf7402c97d27e0b96a8f59ad83b33 Mon Sep 17 00:00:00 2001 From: Peter Date: Sat, 10 Jul 2021 16:24:22 -0400 Subject: [PATCH] refactored IK to try out point-ik also fixed bug (?) in bio_ik linking https://github.com/TAMS-Group/bio_ik/issues/29 --- jacobian_follower/CMakeLists.txt | 13 ++--- .../jacobian_follower/jacobian_follower.hpp | 12 +++-- jacobian_follower/package.xml | 13 ++--- .../src/jacobian_follower/bindings.cpp | 4 +- .../jacobian_follower/jacobian_follower.cpp | 54 ++++++++++++++++++- 5 files changed, 77 insertions(+), 19 deletions(-) diff --git a/jacobian_follower/CMakeLists.txt b/jacobian_follower/CMakeLists.txt index 11fa7ae..fb9ddb1 100644 --- 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 index d516543..e961f29 100644 --- 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> compute_IK_solutions(geometry_msgs::Pose target_pose, const std::string &group_name) const; - std::optional computeCollisionFreeIK(const std::vector &target_pose, - const std::string &group_name, - const std::vector &tip_names, - const moveit_msgs::PlanningScene &scene_smg) const; + std::optional computeCollisionFreePointIK( + const std::vector &target_point, const std::string &group_name, + const std::vector &tip_names, const moveit_msgs::PlanningScene &scene_smg) const; + + std::optional computeCollisionFreePoseIK(const std::vector &target_pose, + const std::string &group_name, + const std::vector &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 index 745c00a..cc4ecf3 100644 --- a/jacobian_follower/package.xml +++ b/jacobian_follower/package.xml @@ -9,13 +9,14 @@ catkin arc_utilities - moveit_ros_planning arm_robots_msgs - roscpp - pyrosmsg - tf2_ros - tf2_msgs + bio_ik + eigen_conversions + moveit_ros_planning moveit_visual_tools pybind11_catkin - eigen_conversions + pyrosmsg + roscpp + tf2_msgs + tf2_ros diff --git a/jacobian_follower/src/jacobian_follower/bindings.cpp b/jacobian_follower/src/jacobian_follower/bindings.cpp index 768a863..cd265f2 100644 --- 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 &, const std::string &>( diff --git a/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp b/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp index e854e81..d01a3fa 100644 --- a/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp +++ b/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -13,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -249,7 +251,56 @@ bool isStateValid(planning_scene::PlanningScenePtr planning_scene, moveit::core: return planning_scene->isStateValid(*robot_state); } -std::optional JacobianFollower::computeCollisionFreeIK( +std::optional JacobianFollower::computeCollisionFreePointIK( + const std::vector &target_point, const std::string &group_name, + const std::vector &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(); + 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(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(), // 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 JacobianFollower::computeCollisionFreePoseIK( const std::vector &target_pose, const std::string &group_name, const std::vector &tip_names, const moveit_msgs::PlanningScene &scene_msg) const { auto joint_model_group = model_->getJointModelGroup(group_name); @@ -262,7 +313,6 @@ std::optional 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(model_);