mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
227d475282
commit
ad5c878f19
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user