From d86ba0e402329af5900fa036076449ce315c4a5f Mon Sep 17 00:00:00 2001 From: Peter Date: Fri, 9 Jul 2021 12:16:07 -0400 Subject: [PATCH] API for collision free IK with multiple end-effectors --- arm_robots/src/arm_robots/robot_utils.py | 3 +- .../jacobian_follower/jacobian_follower.hpp | 3 +- .../src/jacobian_follower/bindings.cpp | 4 +- .../jacobian_follower/jacobian_follower.cpp | 53 ++++++++++++++----- 4 files changed, 46 insertions(+), 17 deletions(-) diff --git a/arm_robots/src/arm_robots/robot_utils.py b/arm_robots/src/arm_robots/robot_utils.py index a88398c..d15ca6a 100644 --- a/arm_robots/src/arm_robots/robot_utils.py +++ b/arm_robots/src/arm_robots/robot_utils.py @@ -182,8 +182,9 @@ def make_robot_state_from_joint_state(scene_msg: PlanningScene, joint_state: Joi joint_state=joint_state) -def merge_joint_state_and_scene_msg(scene_msg, joint_state): +def merge_joint_state_and_scene_msg(scene_msg: PlanningScene, joint_state: JointState): robot_state = make_robot_state_from_joint_state(scene_msg=scene_msg, joint_state=joint_state) + robot_state.attached_collision_objects = scene_msg.robot_state.attached_collision_objects scene_msg_with_state = scene_msg # NOTE: I used to have a deepcopy here, but it was slow so I removed it scene_msg_with_state.robot_state.joint_state = joint_state return scene_msg_with_state, robot_state diff --git a/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp b/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp index 1faba7a..d516543 100644 --- a/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp +++ b/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp @@ -145,8 +145,9 @@ 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, + 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; geometry_msgs::Pose computeGroupFK(const moveit_msgs::RobotState &robot_state_msg, diff --git a/jacobian_follower/src/jacobian_follower/bindings.cpp b/jacobian_follower/src/jacobian_follower/bindings.cpp index 572b76c..768a863 100644 --- a/jacobian_follower/src/jacobian_follower/bindings.cpp +++ b/jacobian_follower/src/jacobian_follower/bindings.cpp @@ -36,8 +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("compute_collision_free_ik", &JacobianFollower::computeCollisionFreeIK, py::arg("poses"), + py::arg("group_name"), py::arg("tip_names"), 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 4ac1274..e854e81 100644 --- a/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp +++ b/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -241,22 +242,48 @@ PlanResult JacobianFollower::plan(JacobianTrajectoryCommand traj_command) { return {robot_trajectory, reached_target}; } +bool isStateValid(planning_scene::PlanningScenePtr planning_scene, moveit::core::RobotState *robot_state, + moveit::core::JointModelGroup const *jmg, double const *joint_positions) { + robot_state->setJointGroupPositions(jmg, joint_positions); + robot_state->update(); // I *think* this updates the internally stored transforms, and probably isn't necessary + return planning_scene->isStateValid(*robot_state); +} + std::optional JacobianFollower::computeCollisionFreeIK( - geometry_msgs::Pose target_pose, const std::string &group_name, const moveit_msgs::PlanningScene &scene_msg) const { + 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); + ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".ik", "Tips: " << tip_names); + + kinematics::KinematicsQueryOptions opts; + + robot_state::RobotState robot_state_ik(model_); + robot_state_ik.setToDefaultValues(); + 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_); 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; - } + 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 + tip_transforms, // multiple end-effector goal poses + tip_names, // 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 {}; }