mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ComputeIK: Limit collision checking to JMG (#428)
That's what MoveIt is doing as well.
This commit is contained in:
parent
a3cb8c6584
commit
61bb2fdc58
@ -99,7 +99,7 @@ namespace {
|
|||||||
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
|
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
|
||||||
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
|
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
|
||||||
robot_state::RobotState& robot_state, Eigen::Isometry3d pose,
|
robot_state::RobotState& robot_state, Eigen::Isometry3d pose,
|
||||||
const robot_model::LinkModel* link,
|
const robot_model::LinkModel* link, const robot_model::JointModelGroup* jmg = nullptr,
|
||||||
collision_detection::CollisionResult* collision_result = nullptr) {
|
collision_detection::CollisionResult* collision_result = nullptr) {
|
||||||
// consider all rigidly connected parent links as well
|
// consider all rigidly connected parent links as well
|
||||||
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
|
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
|
||||||
@ -130,6 +130,8 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
|
|||||||
collision_detection::CollisionRequest req;
|
collision_detection::CollisionRequest req;
|
||||||
collision_detection::CollisionResult result;
|
collision_detection::CollisionResult result;
|
||||||
req.contacts = (collision_result != nullptr);
|
req.contacts = (collision_result != nullptr);
|
||||||
|
if (jmg)
|
||||||
|
req.group_name = jmg->getName();
|
||||||
collision_detection::CollisionResult& res = collision_result ? *collision_result : result;
|
collision_detection::CollisionResult& res = collision_result ? *collision_result : result;
|
||||||
scene->checkCollision(req, res, robot_state, acm);
|
scene->checkCollision(req, res, robot_state, acm);
|
||||||
return res.collision;
|
return res.collision;
|
||||||
@ -311,7 +313,7 @@ void ComputeIK::compute() {
|
|||||||
collision_detection::CollisionResult collisions;
|
collision_detection::CollisionResult collisions;
|
||||||
robot_state::RobotState sandbox_state{ scene->getCurrentState() };
|
robot_state::RobotState sandbox_state{ scene->getCurrentState() };
|
||||||
bool colliding =
|
bool colliding =
|
||||||
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
|
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions);
|
||||||
|
|
||||||
// frames at target pose and ik frame
|
// frames at target pose and ik frame
|
||||||
std::deque<visualization_msgs::Marker> frame_markers;
|
std::deque<visualization_msgs::Marker> frame_markers;
|
||||||
@ -371,6 +373,7 @@ void ComputeIK::compute() {
|
|||||||
collision_detection::CollisionResult res;
|
collision_detection::CollisionResult res;
|
||||||
req.contacts = true;
|
req.contacts = true;
|
||||||
req.max_contacts = 1;
|
req.max_contacts = 1;
|
||||||
|
req.group_name = jmg->getName();
|
||||||
scene->checkCollision(req, res, *state);
|
scene->checkCollision(req, res, *state);
|
||||||
solution.feasible = ignore_collisions || !res.collision;
|
solution.feasible = ignore_collisions || !res.collision;
|
||||||
if (!res.contacts.empty()) {
|
if (!res.contacts.empty()) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user