From 062408f5596e2e41a84bbc92c43ede72606459dd Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 9 Nov 2022 11:14:10 -0600 Subject: [PATCH 1/2] Small optimization in constructGoalConstraints() --- moveit_core/kinematic_constraints/src/utils.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 5bb222994c..4a26db6d72 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -181,13 +181,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n pcm.header = pose.header; pcm.constraint_region.primitive_poses.resize(1); - pcm.constraint_region.primitive_poses[0].position = pose.pose.position; - // orientation of constraint region does not affect anything, since it is a sphere - pcm.constraint_region.primitive_poses[0].orientation.x = 0.0; - pcm.constraint_region.primitive_poses[0].orientation.y = 0.0; - pcm.constraint_region.primitive_poses[0].orientation.z = 0.0; - pcm.constraint_region.primitive_poses[0].orientation.w = 1.0; + pcm.constraint_region.primitive_poses[0] = pose.pose; pcm.weight = 1.0; goal.orientation_constraints.resize(1); From 2913dfc58f5915430962f0e2d6621e8180d92772 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 9 Nov 2022 13:11:24 -0600 Subject: [PATCH 2/2] Quat defaults to unity Co-authored-by: Henning Kayser --- moveit_core/kinematic_constraints/src/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 4a26db6d72..67bc542c28 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -182,7 +182,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n pcm.header = pose.header; pcm.constraint_region.primitive_poses.resize(1); // orientation of constraint region does not affect anything, since it is a sphere - pcm.constraint_region.primitive_poses[0] = pose.pose; + pcm.constraint_region.primitive_poses[0].position = pose.pose.position; pcm.weight = 1.0; goal.orientation_constraints.resize(1);