ComputeIK: Allow additional constraints for filtering solutions (#464)

Add "constraint" property.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
This commit is contained in:
VideoSystemsTech 2024-05-28 16:54:10 +02:00 committed by GitHub
parent 227d475282
commit ad5c878f19
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 24 additions and 6 deletions

View File

@ -191,10 +191,13 @@ void export_stages(pybind11::module& m) {
int: Set the maximum number of inverse
kinematic solutions thats should be generated.
)")
.property<uint32_t>("max_ik_solutions", "uint: max number of solutions to return")
.property<bool>("ignore_collisions", R"(
bool: Specify if collisions with other members of
the planning scene are allowed.
)")
.property<double>("min_solution_distance", "reject solution that are closer than this to previously found solutions")
.property<moveit_msgs::Constraints>("constraints", "additional constraints to obey")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
PoseStamped_: Specify the frame with respect
to which the inverse kinematics

View File

@ -63,6 +63,7 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
p.declare<bool>("ignore_collisions", false);
p.declare<double>("min_solution_distance", 0.1,
"minimum distance between seperate IK solutions for the same target");
p.declare<moveit_msgs::Constraints>("constraints", moveit_msgs::Constraints(), "additional constraints to obey");
// ik_frame and target_pose are read from the interface
p.declare<geometry_msgs::PoseStamped>("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<double> joint_positions;
bool feasible;
collision_detection::Contact contact;
bool collision_free;
bool satisfies_constraints;
};
using IKSolutions = std::vector<IKSolution>;
@ -359,8 +361,11 @@ void ComputeIK::compute() {
double min_solution_distance = props.get<double>("min_solution_distance");
kinematic_constraints::KinematicConstraintSet constraint_set(robot_model);
constraint_set.add(props.get<moveit_msgs::Constraints>("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<uint32_t>("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();