From 5a7be53993a9f99efe683701c8af0a45fa5c38cb Mon Sep 17 00:00:00 2001 From: Peter Date: Sun, 27 Jun 2021 14:18:51 -0400 Subject: [PATCH] tried making compute collision free ik function --- .../utility/move_to_impedance_switch.py | 2 +- .../jacobian_follower/jacobian_follower.hpp | 9 ++++++-- .../src/jacobian_follower/bindings.cpp | 2 ++ .../jacobian_follower/jacobian_follower.cpp | 22 ++++++++++++++++++- 4 files changed, 31 insertions(+), 4 deletions(-) diff --git a/arm_robots/scripts/utility/move_to_impedance_switch.py b/arm_robots/scripts/utility/move_to_impedance_switch.py index 04272df..e0796c2 100755 --- 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 index 530160e..1faba7a 100644 --- a/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp +++ b/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -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> compute_IK_solutions(geometry_msgs::Pose target_pose, const std::string &group_name) const; + std::optional 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 index faa4f53..572b76c 100644 --- 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 &, 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 index a51ee68..4ac1274 100644 --- 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 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(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> 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;