From ad5c878f1914965d1b70e8c6dd93f8fe97a74ad9 Mon Sep 17 00:00:00 2001 From: VideoSystemsTech <61198558+VideoSystemsTech@users.noreply.github.com> Date: Tue, 28 May 2024 16:54:10 +0200 Subject: [PATCH] ComputeIK: Allow additional constraints for filtering solutions (#464) Add "constraint" property. Co-authored-by: Robert Haschke --- core/python/bindings/src/stages.cpp | 3 +++ core/src/stages/compute_ik.cpp | 27 +++++++++++++++++++++------ 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index db263ffa5..fc8c5949e 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -191,10 +191,13 @@ void export_stages(pybind11::module& m) { int: Set the maximum number of inverse kinematic solutions thats should be generated. )") + .property("max_ik_solutions", "uint: max number of solutions to return") .property("ignore_collisions", R"( bool: Specify if collisions with other members of the planning scene are allowed. )") + .property("min_solution_distance", "reject solution that are closer than this to previously found solutions") + .property("constraints", "additional constraints to obey") .property("ik_frame", R"( PoseStamped_: Specify the frame with respect to which the inverse kinematics diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 4822998ad..29cf79afd 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -63,6 +63,7 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB p.declare("ignore_collisions", false); p.declare("min_solution_distance", 0.1, "minimum distance between seperate IK solutions for the same target"); + p.declare("constraints", moveit_msgs::Constraints(), "additional constraints to obey"); // ik_frame and target_pose are read from the interface p.declare("ik_frame", "frame to be moved towards goal pose"); @@ -88,8 +89,9 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& struct IKSolution { std::vector joint_positions; - bool feasible; collision_detection::Contact contact; + bool collision_free; + bool satisfies_constraints; }; using IKSolutions = std::vector; @@ -359,8 +361,11 @@ void ComputeIK::compute() { double min_solution_distance = props.get("min_solution_distance"); + kinematic_constraints::KinematicConstraintSet constraint_set(robot_model); + constraint_set.add(props.get("constraints"), scene->getTransforms()); + IKSolutions ik_solutions; - auto is_valid = [scene, ignore_collisions, min_solution_distance, + auto is_valid = [scene, ignore_collisions, min_solution_distance, &constraint_set = std::as_const(constraint_set), &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { @@ -368,20 +373,28 @@ void ComputeIK::compute() { return false; // too close to already found solution } state->setJointGroupPositions(jmg, joint_positions); + state->update(); + ik_solutions.emplace_back(); auto& solution{ ik_solutions.back() }; state->copyJointGroupPositions(jmg, solution.joint_positions); + + // validate constraints + solution.satisfies_constraints = constraint_set.decide(*state).satisfied; + + // check for collisions collision_detection::CollisionRequest req; collision_detection::CollisionResult res; req.contacts = true; req.max_contacts = 1; req.group_name = jmg->getName(); scene->checkCollision(req, res, *state); - solution.feasible = ignore_collisions || !res.collision; + solution.collision_free = ignore_collisions || !res.collision; if (!res.contacts.empty()) { solution.contact = res.contacts.begin()->second.front(); } - return solution.feasible; + + return solution.satisfies_constraints && solution.collision_free; }; uint32_t max_ik_solutions = props.get("max_ik_solutions"); @@ -411,14 +424,16 @@ void ComputeIK::compute() { solution.setComment(s.comment()); std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); - if (ik_solutions[i].feasible) + if (ik_solutions[i].collision_free && ik_solutions[i].satisfies_constraints) // compute cost as distance to compare_pose solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data())); - else { // found an IK solution, but this was not valid + else if (!ik_solutions[i].collision_free) { // solution was in collision std::stringstream ss; ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '" << ik_solutions[i].contact.body_name_2 << "'"; solution.markAsFailure(ss.str()); + } else if (!ik_solutions[i].satisfies_constraints) { // solution was violating constraints + solution.markAsFailure("Constraints violated"); } // set scene's robot state moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst();