tried making compute collision free ik function

UM-ARM-Lab · Jul 13, 2021 · 5a7be53 · 5a7be53
1 parent 5c577d4
commit 5a7be53
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 4 deletions.
diff --git a/arm_robots/scripts/utility/move_to_impedance_switch.py b/arm_robots/scripts/utility/move_to_impedance_switch.py
@@ -22,7 +22,7 @@
         victor = Victor(robot_namespace='victor')
         victor.connect()
 
-        actually_switch = rospy.get_param("~actually_switch", False)
+        actually_switch = rospy.get_param("~actually_switch", True)
         if actually_switch:
             rospy.loginfo("switching to impedance mode")
 

diff --git a/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp b/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp
@@ -21,6 +21,7 @@
 #include <arc_utilities/eigen_typedefs.hpp>
 #include <arc_utilities/moveit_pose_type.hpp>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -99,7 +100,7 @@ class JacobianFollower {
   planning_scene_monitor::PlanningSceneMonitorPtr scene_monitor_;
 
   // Debugging
-  moveit_visual_tools::MoveItVisualTools visual_tools_;
+  mutable moveit_visual_tools::MoveItVisualTools visual_tools_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
@@ -144,6 +145,10 @@ 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(geometry_msgs::Pose target_pose,
+                                                                const std::string &group_name,
+                                                                const moveit_msgs::PlanningScene &scene_smg) const;
+
   geometry_msgs::Pose computeGroupFK(const moveit_msgs::RobotState &robot_state_msg,
                                      const std::string &group_name) const;
 
@@ -179,7 +184,7 @@ class JacobianFollower {
                                         robot_state::RobotState const &state, PoseSequence const &robotTservo);
 
   collision_detection::CollisionResult checkCollision(planning_scene::PlanningScenePtr planning_scene,
-                                                      robot_state::RobotState const &state);
+                                                      robot_state::RobotState const &state) const;
 
   bool check_collision(moveit_msgs::PlanningScene const &scene_msg, moveit_msgs::RobotState const &start_state);
 

diff --git a/jacobian_follower/src/jacobian_follower/bindings.cpp b/jacobian_follower/src/jacobian_follower/bindings.cpp
@@ -36,6 +36,8 @@ 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("pose"),
+           py::arg("group_name"), py::arg("scene_name"))
       .def("group_fk",
            py::overload_cast<const std::vector<double> &, const std::vector<std::string> &, const std::string &>(
                &JacobianFollower::computeGroupFK, py::const_),

diff --git a/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp b/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp
@@ -241,6 +241,26 @@ PlanResult JacobianFollower::plan(JacobianTrajectoryCommand traj_command) {
   return {robot_trajectory, reached_target};
 }
 
+std::optional<moveit_msgs::RobotState> JacobianFollower::computeCollisionFreeIK(
+    geometry_msgs::Pose target_pose, const std::string &group_name, const moveit_msgs::PlanningScene &scene_msg) const {
+  auto planning_scene = std::make_shared<planning_scene::PlanningScene>(model_);
+  planning_scene->usePlanningSceneMsg(scene_msg);
+  auto const jmg = model_->getJointModelGroup(group_name);
+  auto const &joint_names = jmg->getJointModelNames();
+  auto const &potential_solutions = compute_IK_solutions(target_pose, group_name);
+  for (auto const &potential_solution : potential_solutions) {
+    robot_state::RobotState &potential_solution_state = planning_scene->getCurrentStateNonConst();
+    potential_solution_state.setVariablePositions(joint_names, potential_solution);
+    auto const &res = checkCollision(planning_scene, potential_solution_state);
+    if (not res.collision) {
+      moveit_msgs::RobotState solution_msg;
+      moveit::core::robotStateToRobotStateMsg(potential_solution_state, solution_msg);
+      return solution_msg;
+    }
+  }
+  return {};
+}
+
 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);
@@ -494,7 +514,7 @@ robot_trajectory::RobotTrajectory JacobianFollower::jacobianPath3d(
 }
 
 collision_detection::CollisionResult JacobianFollower::checkCollision(planning_scene::PlanningScenePtr planning_scene,
-                                                                      robot_state::RobotState const &state) {
+                                                                      robot_state::RobotState const &state) const {
   collision_detection::CollisionRequest collisionRequest;
   collisionRequest.contacts = true;
   collisionRequest.max_contacts = 1;