From 3ab15824542221e30861a6d4ff23f93d454a3a61 Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Wed, 25 Aug 2021 13:50:18 +0000 Subject: [PATCH 1/3] Replace getFixedTransforms with getShapePoses --- src/grasp_filter.cpp | 2 +- src/grasp_planner.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/grasp_filter.cpp b/src/grasp_filter.cpp index cbe5e3c..7d8e6da 100644 --- a/src/grasp_filter.cpp +++ b/src/grasp_filter.cpp @@ -592,7 +592,7 @@ bool GraspFilter::findIKSolution(std::vector& ik_solution, const IkThrea ik_thread_struct->planning_scene_->getCurrentState().getAttachedBodies(attached_bodies); for (const robot_state::AttachedBody* ab : attached_bodies) - state.attachBody(ab->getName(), ab->getShapes(), ab->getFixedTransforms(), ab->getTouchLinks(), + state.attachBody(ab->getName(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture()); bool ik_success = state.setFromIK(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_pose_.pose, diff --git a/src/grasp_planner.cpp b/src/grasp_planner.cpp index bcb24e8..2081b13 100644 --- a/src/grasp_planner.cpp +++ b/src/grasp_planner.cpp @@ -346,7 +346,7 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida std::vector attached_bodies; start_state_with_body->getAttachedBodies(attached_bodies); for (const auto& ab : attached_bodies) - start_state_copy->attachBody(ab->getName(), ab->getShapes(), ab->getFixedTransforms(), ab->getTouchLinks(), + start_state_copy->attachBody(ab->getName(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), ab->getSubframeTransforms()); constraint_fn = boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, only_check_self_collision, visual_tools_, _1, _2, _3); From b6d3e3759710ab0630537a235a58dbe037b8e792 Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Thu, 26 Aug 2021 15:53:01 +0200 Subject: [PATCH 2/3] Fix attachBody breaking changes from MoveIt upstream --- src/grasp_filter.cpp | 5 +++-- src/grasp_planner.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/grasp_filter.cpp b/src/grasp_filter.cpp index 7d8e6da..f06c697 100644 --- a/src/grasp_filter.cpp +++ b/src/grasp_filter.cpp @@ -592,8 +592,9 @@ bool GraspFilter::findIKSolution(std::vector& ik_solution, const IkThrea ik_thread_struct->planning_scene_->getCurrentState().getAttachedBodies(attached_bodies); for (const robot_state::AttachedBody* ab : attached_bodies) - state.attachBody(ab->getName(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), - ab->getAttachedLinkName(), ab->getDetachPosture()); + state.attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), + ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), + ab->getSubframes()); bool ik_success = state.setFromIK(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_pose_.pose, ik_thread_struct->timeout_, constraint_fn); diff --git a/src/grasp_planner.cpp b/src/grasp_planner.cpp index 2081b13..2827bfa 100644 --- a/src/grasp_planner.cpp +++ b/src/grasp_planner.cpp @@ -346,8 +346,9 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida std::vector attached_bodies; start_state_with_body->getAttachedBodies(attached_bodies); for (const auto& ab : attached_bodies) - start_state_copy->attachBody(ab->getName(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), - ab->getAttachedLinkName(), ab->getDetachPosture(), ab->getSubframeTransforms()); + start_state_copy->attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), + ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), + ab->getSubframes()); constraint_fn = boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, only_check_self_collision, visual_tools_, _1, _2, _3); } From e397192733984b88078a71266f03fc215728cb21 Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Thu, 26 Aug 2021 16:25:03 +0200 Subject: [PATCH 3/3] Apply .clangformat --- src/grasp_filter.cpp | 5 ++--- src/grasp_planner.cpp | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/grasp_filter.cpp b/src/grasp_filter.cpp index f06c697..e81a914 100644 --- a/src/grasp_filter.cpp +++ b/src/grasp_filter.cpp @@ -592,9 +592,8 @@ bool GraspFilter::findIKSolution(std::vector& ik_solution, const IkThrea ik_thread_struct->planning_scene_->getCurrentState().getAttachedBodies(attached_bodies); for (const robot_state::AttachedBody* ab : attached_bodies) - state.attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), - ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), - ab->getSubframes()); + state.attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), + ab->getAttachedLinkName(), ab->getDetachPosture(), ab->getSubframes()); bool ik_success = state.setFromIK(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_pose_.pose, ik_thread_struct->timeout_, constraint_fn); diff --git a/src/grasp_planner.cpp b/src/grasp_planner.cpp index 2827bfa..af63203 100644 --- a/src/grasp_planner.cpp +++ b/src/grasp_planner.cpp @@ -346,8 +346,8 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida std::vector attached_bodies; start_state_with_body->getAttachedBodies(attached_bodies); for (const auto& ab : attached_bodies) - start_state_copy->attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), - ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), + start_state_copy->attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), + ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), ab->getSubframes()); constraint_fn = boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, only_check_self_collision, visual_tools_, _1, _2, _3);