From d858edcd9cee0cc29a06d5c2d53ed7a84e10fab4 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 18 Mar 2024 17:13:57 +0100 Subject: [PATCH 01/19] Correctly handle cartesian waypoints in SimplePlannerFixedSizeAssignPlanProfile --- ...planner_fixed_size_assign_plan_profile.cpp | 114 +++++++++--------- 1 file changed, 59 insertions(+), 55 deletions(-) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp index 6fcd5097e7..0eb022b0a1 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp @@ -1,13 +1,13 @@ /** - * @file simple_planner_default_plan_profile.cpp + * @file simple_planner_fixed_size_assign_plan_profile.cpp * @brief * - * @author Matthew Powelson - * @date July 23, 2020 + * @author Roelof Oomen + * @date May 29, 2024 * @version TODO * @bug No known bugs * - * @copyright Copyright (c) 2020, Southwest Research Institute + * @copyright Copyright (c) 2024, ROS Industrial Consortium * * @par License * Software License Agreement (Apache License) @@ -24,6 +24,7 @@ * limitations under the License. */ +#include #include #include #include @@ -31,9 +32,8 @@ #include #include - -#include #include +#include #include #include @@ -54,77 +54,81 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo base(base_instruction, *env, global_manip_info); - Eigen::MatrixXd states; - if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) + Eigen::VectorXd j2; + if (!base.has_cartesian_waypoint) { - const Eigen::VectorXd& jp = info2.extractJointPosition(); - if (info2.instruction.isLinear()) - states = jp.replicate(1, linear_steps + 1); - else if (info2.instruction.isFreespace()) - states = jp.replicate(1, freespace_steps + 1); - else - throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); - } - else if (!info1.has_cartesian_waypoint && info2.has_cartesian_waypoint) - { - const Eigen::VectorXd& jp = info1.extractJointPosition(); - if (info2.instruction.isLinear()) - states = jp.replicate(1, linear_steps + 1); - else if (info2.instruction.isFreespace()) - states = jp.replicate(1, freespace_steps + 1); - else - throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); - } - else if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) - { - const Eigen::VectorXd& jp = info2.extractJointPosition(); - if (info2.instruction.isLinear()) - states = jp.replicate(1, linear_steps + 1); - else if (info2.instruction.isFreespace()) - states = jp.replicate(1, freespace_steps + 1); - else - throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + j2 = base.extractJointPosition(); } else { - Eigen::VectorXd seed = env->getCurrentJointValues(info2.manip->getJointNames()); - tesseract_common::enforceLimits(seed, info2.manip->getLimits().joint_limits); - - if (info2.instruction.isLinear()) - states = seed.replicate(1, linear_steps + 1); - else if (info2.instruction.isFreespace()) - states = seed.replicate(1, freespace_steps + 1); + // Determine base_instruction joint position and replicate + const auto& base_cwp = base.instruction.getWaypoint().as(); + if (base_cwp.hasSeed()) + { + // Use joint position of cartesian base_instruction + j2 = base_cwp.getSeed().position; + } else - throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + { + if (prev.has_cartesian_waypoint) + { + const auto& prev_cwp = prev.instruction.getWaypoint().as(); + if (prev_cwp.hasSeed()) + { + // Use joint position of cartesian prev_instruction as seed + j2 = getClosestJointSolution(base, prev_cwp.getSeed().position); + } + else + { + // Use current env_state as seed + j2 = getClosestJointSolution(base, env->getCurrentJointValues(base.manip->getJointNames())); + } + } + else + { + // Use prev_instruction as seed + j2 = getClosestJointSolution(base, prev.extractJointPosition()); + } + } + tesseract_common::enforceLimits(j2, base.manip->getLimits().joint_limits); } + Eigen::MatrixXd states; + // Replicate base_instruction joint position + if (base.instruction.isLinear()) + states = j2.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = j2.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + // Linearly interpolate in cartesian space if linear move if (base_instruction.isLinear()) { Eigen::Isometry3d p1_world; - if (info1.has_cartesian_waypoint) - p1_world = info1.extractCartesianPose(); + if (prev.has_cartesian_waypoint) + p1_world = prev.extractCartesianPose(); else - p1_world = info1.calcCartesianPose(info1.extractJointPosition()); + p1_world = prev.calcCartesianPose(prev.extractJointPosition()); Eigen::Isometry3d p2_world; - if (info2.has_cartesian_waypoint) - p2_world = info2.extractCartesianPose(); + if (base.has_cartesian_waypoint) + p2_world = base.extractCartesianPose(); else - p2_world = info2.calcCartesianPose(info2.extractJointPosition()); + p2_world = base.calcCartesianPose(base.extractJointPosition()); tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, linear_steps); for (auto& pose : poses) - pose = info2.working_frame_transform.inverse() * pose; + pose = base.working_frame_transform.inverse() * pose; assert(static_cast(poses.size()) == states.cols()); - return getInterpolatedInstructions(poses, info2.manip->getJointNames(), states, info2.instruction); + return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction); } - return getInterpolatedInstructions(info2.manip->getJointNames(), states, info2.instruction); + return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); } template @@ -139,4 +143,4 @@ void SimplePlannerFixedSizeAssignPlanProfile::serialize(Archive& ar, const unsig #include TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) \ No newline at end of file From c22443474e2f53d1a8d50cbf54db5d239b52ff28 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 19 Mar 2024 09:15:22 +0100 Subject: [PATCH 02/19] Simplify --- .../profile/simple_planner_fixed_size_assign_plan_profile.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h index a652400a49..cca44b4a9b 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h @@ -1,5 +1,5 @@ /** - * @file simple_planner_default_plan_profile.h + * @file simple_planner_fixed_size_assign_plan_profile.h * @brief * * @author Matthew Powelson From 02266a9a31d4a3ac520606c3ac7cb706dad78c34 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 20 Mar 2024 07:07:46 +0100 Subject: [PATCH 03/19] Add SimplePlannerLVSAssignPlanProfile --- .../simple/CMakeLists.txt | 5 +- .../simple_planner_lvs_assign_plan_profile.h | 81 +++++++++ ...simple_planner_lvs_assign_plan_profile.cpp | 171 ++++++++++++++++++ 3 files changed, 256 insertions(+), 1 deletion(-) create mode 100644 tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h create mode 100644 tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 0b63020236..23969e1efe 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -6,7 +6,10 @@ add_library( src/profile/simple_planner_lvs_plan_profile.cpp src/profile/simple_planner_lvs_no_ik_plan_profile.cpp src/profile/simple_planner_fixed_size_assign_plan_profile.cpp - src/profile/simple_planner_fixed_size_plan_profile.cpp) + src/profile/simple_planner_fixed_size_plan_profile.cpp + src/profile/simple_planner_lvs_assign_plan_profile.cpp + src/profile/simple_planner_lvs_no_ik_plan_profile.cpp + src/profile/simple_planner_lvs_plan_profile.cpp) target_link_libraries(${PROJECT_NAME}_simple PUBLIC ${PROJECT_NAME}_core Boost::boost) target_compile_options(${PROJECT_NAME}_simple PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) target_compile_options(${PROJECT_NAME}_simple PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h new file mode 100644 index 0000000000..53ca756ece --- /dev/null +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h @@ -0,0 +1,81 @@ +/** + * @file simple_planner_lvs_assign_plan_profile.h + * @brief + * + * @author Roelof Oomen + * @date March 19, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, ROS Industrial Consortium + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_PLAN_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_PLAN_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class SimplePlannerLVSAssignPlanProfile : public SimplePlannerPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + /** + * @brief SimplePlannerFixedSizeAssignPlanProfile + * @param freespace_steps The number of steps to use for freespace instruction + * @param linear_steps The number of steps to use for linear instruction + */ + SimplePlannerLVSAssignPlanProfile(double state_longest_valid_segment_length = 5 * M_PI / 180, + double translation_longest_valid_segment_length = 0.1, + double rotation_longest_valid_segment_length = 5 * M_PI / 180, + int min_steps = 1, + int max_steps = std::numeric_limits::max()); + + std::vector generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& prev_seed, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& next_instruction, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const override; + + /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ + double state_longest_valid_segment_length; + + /** @brief The maximum translation distance between successive steps */ + double translation_longest_valid_segment_length; + + /** @brief The maximum rotational distance between successive steps */ + double rotation_longest_valid_segment_length; + + /** @brief The minimum number of steps for the plan */ + int min_steps; + + /** @brief The maximum number of steps for the plan */ + int max_steps; +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp new file mode 100644 index 0000000000..d623a26ca2 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp @@ -0,0 +1,171 @@ +/** + * @file simple_planner_lvs_assign_plan_profile.cpp + * @brief + * + * @author Roelof Oomen + * @date March 19, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, ROS Industrial Consortium + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +SimplePlannerLVSAssignPlanProfile::SimplePlannerLVSAssignPlanProfile(double state_longest_valid_segment_length, + double translation_longest_valid_segment_length, + double rotation_longest_valid_segment_length, + int min_steps, + int max_steps) + : state_longest_valid_segment_length(state_longest_valid_segment_length) + , translation_longest_valid_segment_length(translation_longest_valid_segment_length) + , rotation_longest_valid_segment_length(rotation_longest_valid_segment_length) + , min_steps(min_steps) + , max_steps(max_steps) +{ +} + +std::vector +SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& /*prev_seed*/, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& /*next_instruction*/, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const +{ + KinematicGroupInstructionInfo prev(prev_instruction, request, global_manip_info); + KinematicGroupInstructionInfo base(base_instruction, request, global_manip_info); + + Eigen::VectorXd j1; + Eigen::Isometry3d p1_world; + if (!prev.has_cartesian_waypoint) + { + j1 = prev.extractJointPosition(); + p1_world = prev.calcCartesianPose(j1); + } + else + { + p1_world = prev.extractCartesianPose(); + const auto& prev_cwp = prev.instruction.getWaypoint().as(); + if (prev_cwp.hasSeed()) + { + // Use joint position of cartesian base_instruction + j1 = prev_cwp.getSeed().position; + } + else + { + if (base.has_cartesian_waypoint) + { + const auto& base_cwp = base.instruction.getWaypoint().as(); + if (base_cwp.hasSeed()) + { + // Use joint position of cartesian base_instruction as seed + j1 = getClosestJointSolution(prev, base_cwp.getSeed().position); + } + else + { + // Use current env_state as seed + j1 = getClosestJointSolution(prev, request.env_state.getJointValues(prev.manip->getJointNames())); + } + } + else + { + // Use base_instruction as seed + j1 = getClosestJointSolution(prev, base.extractJointPosition()); + } + } + tesseract_common::enforcePositionLimits(j1, prev.manip->getLimits().joint_limits); + } + + Eigen::VectorXd j2; + Eigen::Isometry3d p2_world; + if (!base.has_cartesian_waypoint) + { + j2 = base.extractJointPosition(); + p2_world = base.calcCartesianPose(j2); + } + else + { + p2_world = base.extractCartesianPose(); + const auto& base_cwp = base.instruction.getWaypoint().as(); + if (base_cwp.hasSeed()) + { + // Use joint position of cartesian base_instruction + j2 = base_cwp.getSeed().position; + } + else + { + if (prev.has_cartesian_waypoint) + { + const auto& prev_cwp = prev.instruction.getWaypoint().as(); + if (prev_cwp.hasSeed()) + { + // Use joint position of cartesian prev_instruction as seed + j2 = getClosestJointSolution(base, prev_cwp.getSeed().position); + } + else + { + // Use current env_state as seed + j2 = getClosestJointSolution(base, request.env_state.getJointValues(base.manip->getJointNames())); + } + } + else + { + // Use prev_instruction as seed + j2 = getClosestJointSolution(base, prev.extractJointPosition()); + } + } + tesseract_common::enforcePositionLimits(j2, base.manip->getLimits().joint_limits); + } + + double trans_dist = (p2_world.translation() - p1_world.translation()).norm(); + double rot_dist = Eigen::Quaterniond(p1_world.linear()).angularDistance(Eigen::Quaterniond(p2_world.linear())); + double joint_dist = (j2 - j1).norm(); + + int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; + int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; + int joint_steps = int(joint_dist / state_longest_valid_segment_length) + 1; + + int steps = std::max(trans_steps, rot_steps); + steps = std::max(steps, joint_steps); + steps = std::max(steps, min_steps); + steps = std::min(steps, max_steps); + + Eigen::MatrixXd states; + // Replicate base_instruction joint position + states = j2.replicate(1, steps + 1); + + // Linearly interpolate in cartesian space if linear move + if (base_instruction.isLinear()) + { + tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, steps); + for (auto& pose : poses) + pose = base.working_frame_transform.inverse() * pose; + + assert(poses.size() == states.cols()); + return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction); + } + + return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); +} + +} // namespace tesseract_planning From b69efdae2227c60a8d5a51fa8b8648aa1928cb91 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 1 May 2024 11:52:35 +0200 Subject: [PATCH 04/19] Fix fwd decl --- .../src/profile/simple_planner_lvs_assign_plan_profile.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp index d623a26ca2..1bb5f5e905 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp @@ -25,9 +25,12 @@ */ #include +#include +#include #include #include #include +#include namespace tesseract_planning { From 21e005ef510e1d6afd12d937064f685497c36f67 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 29 May 2024 16:17:36 +0200 Subject: [PATCH 05/19] Add SimplePlannerLVSAssignNoIKPlanProfile --- ...le_planner_lvs_assign_no_ik_plan_profile.h | 81 ++++++++++ ..._planner_lvs_assign_no_ik_plan_profile.cpp | 143 ++++++++++++++++++ ...simple_planner_lvs_assign_plan_profile.cpp | 6 +- 3 files changed, 227 insertions(+), 3 deletions(-) create mode 100644 tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h create mode 100644 tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h new file mode 100644 index 0000000000..aaf97a4c30 --- /dev/null +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h @@ -0,0 +1,81 @@ +/** + * @file simple_planner_lvs_assign_no_ik_plan_profile.h + * @brief + * + * @author Roelof Oomen + * @date May 29, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, ROS Industrial Consortium + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_PLAN_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_PLAN_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class SimplePlannerLVSAssignNoIKPlanProfile : public SimplePlannerPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + /** + * @brief SimplePlannerFixedSizeAssignPlanProfile + * @param freespace_steps The number of steps to use for freespace instruction + * @param linear_steps The number of steps to use for linear instruction + */ + SimplePlannerLVSAssignNoIKPlanProfile(double state_longest_valid_segment_length = 5 * M_PI / 180, + double translation_longest_valid_segment_length = 0.1, + double rotation_longest_valid_segment_length = 5 * M_PI / 180, + int min_steps = 1, + int max_steps = std::numeric_limits::max()); + + std::vector generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& prev_seed, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& next_instruction, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const override; + + /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ + double state_longest_valid_segment_length; + + /** @brief The maximum translation distance between successive steps */ + double translation_longest_valid_segment_length; + + /** @brief The maximum rotational distance between successive steps */ + double rotation_longest_valid_segment_length; + + /** @brief The minimum number of steps for the plan */ + int min_steps; + + /** @brief The maximum number of steps for the plan */ + int max_steps; +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp new file mode 100644 index 0000000000..f3269c8884 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp @@ -0,0 +1,143 @@ +/** + * @file simple_planner_lvs_assign_no_ik_plan_profile.cpp + * @brief + * + * @author Roelof Oomen + * @date May 29, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, ROS Industrial Consortium + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +SimplePlannerLVSAssignNoIKPlanProfile::SimplePlannerLVSAssignNoIKPlanProfile( + double state_longest_valid_segment_length, + double translation_longest_valid_segment_length, + double rotation_longest_valid_segment_length, + int min_steps, + int max_steps) + : state_longest_valid_segment_length(state_longest_valid_segment_length) + , translation_longest_valid_segment_length(translation_longest_valid_segment_length) + , rotation_longest_valid_segment_length(rotation_longest_valid_segment_length) + , min_steps(min_steps) + , max_steps(max_steps) +{ +} + +std::vector +SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& /*prev_seed*/, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& /*next_instruction*/, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const +{ + JointGroupInstructionInfo prev(prev_instruction, request, global_manip_info); + JointGroupInstructionInfo base(base_instruction, request, global_manip_info); + + Eigen::VectorXd j1; + Eigen::Isometry3d p1_world; + bool has_j1 = false; + if (prev.has_cartesian_waypoint) + { + p1_world = prev.extractCartesianPose(); + if (prev.instruction.getWaypoint().as().hasSeed()) + { + j1 = prev.instruction.getWaypoint().as().getSeed().position; + has_j1 = true; + } + } + else + { + j1 = prev.extractJointPosition(); + p1_world = prev.calcCartesianPose(j1); + has_j1 = true; + } + + Eigen::VectorXd j2; + Eigen::Isometry3d p2_world; + bool has_j2 = false; + if (base.has_cartesian_waypoint) + { + p2_world = base.extractCartesianPose(); + if (base.instruction.getWaypoint().as().hasSeed()) + { + j2 = base.instruction.getWaypoint().as().getSeed().position; + has_j2 = true; + } + } + else + { + j2 = base.extractJointPosition(); + p2_world = base.calcCartesianPose(j2); + has_j2 = true; + } + + double trans_dist = (p2_world.translation() - p1_world.translation()).norm(); + double rot_dist = Eigen::Quaterniond(p1_world.linear()).angularDistance(Eigen::Quaterniond(p2_world.linear())); + int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; + int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; + int steps = std::max(trans_steps, rot_steps); + + if (has_j1 && has_j2) + { + double joint_dist = (j2 - j1).norm(); + int joint_steps = int(joint_dist / state_longest_valid_segment_length) + 1; + steps = std::max(steps, joint_steps); + } + + Eigen::MatrixXd states; + if (has_j2) + { + states = j2.replicate(1, steps + 1); + } + else if (has_j1) + { + states = j1.replicate(1, steps + 1); + } + else + { + Eigen::VectorXd seed = request.env_state.getJointValues(base.manip->getJointNames()); + tesseract_common::enforceLimits(seed, base.manip->getLimits().joint_limits); + states = seed.replicate(1, steps + 1); + } + + // Linearly interpolate in cartesian space if linear move + if (base_instruction.isLinear()) + { + tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, steps); + for (auto& pose : poses) + pose = base.working_frame_transform.inverse() * pose; + + assert(poses.size() == states.cols()); + return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction); + } + + return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); +} + +} // namespace tesseract_planning diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp index 1bb5f5e905..e7617eb99d 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp @@ -3,7 +3,7 @@ * @brief * * @author Roelof Oomen - * @date March 19, 2024 + * @date May 29, 2024 * @version TODO * @bug No known bugs * @@ -96,7 +96,7 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst j1 = getClosestJointSolution(prev, base.extractJointPosition()); } } - tesseract_common::enforcePositionLimits(j1, prev.manip->getLimits().joint_limits); + tesseract_common::enforceLimits(j1, prev.manip->getLimits().joint_limits); } Eigen::VectorXd j2; @@ -137,7 +137,7 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst j2 = getClosestJointSolution(base, prev.extractJointPosition()); } } - tesseract_common::enforcePositionLimits(j2, base.manip->getLimits().joint_limits); + tesseract_common::enforceLimits(j2, base.manip->getLimits().joint_limits); } double trans_dist = (p2_world.translation() - p1_world.translation()).norm(); From 96402434e60fda9e85d17915a1071baac870a222 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 29 May 2024 16:19:20 +0200 Subject: [PATCH 06/19] Rename SimplePlannerFixedSizeAssignPlanProfile to SimplePlannerFixedSizeAssignNoIKPlanProfile and add SimplePlannerFixedSizeAssignPlanProfile (with IK) --- .../simple/CMakeLists.txt | 1 + ...ner_fixed_size_assign_no_ik_plan_profile.h | 63 +++++++++ ...e_planner_fixed_size_assign_plan_profile.h | 6 +- ...r_fixed_size_assign_no_ik_plan_profile.cpp | 128 ++++++++++++++++++ 4 files changed, 195 insertions(+), 3 deletions(-) create mode 100644 tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h create mode 100644 tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 23969e1efe..126c7f3e93 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -7,6 +7,7 @@ add_library( src/profile/simple_planner_lvs_no_ik_plan_profile.cpp src/profile/simple_planner_fixed_size_assign_plan_profile.cpp src/profile/simple_planner_fixed_size_plan_profile.cpp + src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp src/profile/simple_planner_lvs_assign_plan_profile.cpp src/profile/simple_planner_lvs_no_ik_plan_profile.cpp src/profile/simple_planner_lvs_plan_profile.cpp) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h new file mode 100644 index 0000000000..48c02e87fa --- /dev/null +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h @@ -0,0 +1,63 @@ +/** + * @file simple_planner_fixed_size_assign_no_ik_plan_profile.h + * @brief + * + * @author Matthew Powelson + * @date July 23, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H + +#include + +namespace tesseract_planning +{ +class SimplePlannerFixedSizeAssignNoIKPlanProfile : public SimplePlannerPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + /** + * @brief SimplePlannerFixedSizeAssignPlanProfile + * @param freespace_steps The number of steps to use for freespace instruction + * @param linear_steps The number of steps to use for linear instruction + */ + SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps = 10, int linear_steps = 10); + + std::vector generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& prev_seed, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& next_instruction, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const override; + + /** @brief The number of steps to use for freespace instruction */ + int freespace_steps; + + /** @brief The number of steps to use for linear instruction */ + int linear_steps; +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h index cca44b4a9b..70e6c29d0c 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h @@ -2,12 +2,12 @@ * @file simple_planner_fixed_size_assign_plan_profile.h * @brief * - * @author Matthew Powelson - * @date July 23, 2020 + * @author Roelof Oomen + * @date May 29, 2024 * @version TODO * @bug No known bugs * - * @copyright Copyright (c) 2020, Southwest Research Institute + * @copyright Copyright (c) 2024, ROS Industrial Consortium * * @par License * Software License Agreement (Apache License) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp new file mode 100644 index 0000000000..eb05e45876 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp @@ -0,0 +1,128 @@ +/** + * @file simple_planner_fixed_size_assign_no_ik_plan_profile.cpp + * @brief + * + * @author Matthew Powelson + * @date July 23, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +#include +#include + +#include + +#include + +namespace tesseract_planning +{ +SimplePlannerFixedSizeAssignNoIKPlanProfile::SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps, + int linear_steps) + : freespace_steps(freespace_steps), linear_steps(linear_steps) +{ +} + +std::vector +SimplePlannerFixedSizeAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& /*prev_seed*/, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& /*next_instruction*/, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const +{ + JointGroupInstructionInfo prev(prev_instruction, request, global_manip_info); + JointGroupInstructionInfo base(base_instruction, request, global_manip_info); + + Eigen::MatrixXd states; + if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + { + const Eigen::VectorXd& jp = base.extractJointPosition(); + if (base.instruction.isLinear()) + states = jp.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = jp.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + else if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint) + { + const Eigen::VectorXd& jp = prev.extractJointPosition(); + if (base.instruction.isLinear()) + states = jp.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = jp.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + else if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + { + const Eigen::VectorXd& jp = base.extractJointPosition(); + if (base.instruction.isLinear()) + states = jp.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = jp.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + else + { + Eigen::VectorXd seed = request.env_state.getJointValues(base.manip->getJointNames()); + tesseract_common::enforceLimits(seed, base.manip->getLimits().joint_limits); + + if (base.instruction.isLinear()) + states = seed.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = seed.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + + // Linearly interpolate in cartesian space if linear move + if (base_instruction.isLinear()) + { + Eigen::Isometry3d p1_world; + if (prev.has_cartesian_waypoint) + p1_world = prev.extractCartesianPose(); + else + p1_world = prev.calcCartesianPose(prev.extractJointPosition()); + + Eigen::Isometry3d p2_world; + if (base.has_cartesian_waypoint) + p2_world = base.extractCartesianPose(); + else + p2_world = base.calcCartesianPose(base.extractJointPosition()); + + tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, linear_steps); + for (auto& pose : poses) + pose = base.working_frame_transform.inverse() * pose; + + assert(poses.size() == states.cols()); + return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction); + } + + return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); +} + +} // namespace tesseract_planning From d17c9c96dc5cd2213ac358d7ff0873b4e1a631e8 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 29 May 2024 16:19:36 +0200 Subject: [PATCH 07/19] Fix file names in comments --- .../simple/profile/simple_planner_fixed_size_plan_profile.h | 2 +- .../src/profile/simple_planner_fixed_size_plan_profile.cpp | 2 +- .../simple/src/profile/simple_planner_lvs_plan_profile.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h index 85cb50a59d..cbb48096a6 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h @@ -1,5 +1,5 @@ /** - * @file simple_planner_interpolation_plan_profile.h + * @file simple_planner_fixed_size_plan_profile.h * @brief * * @author Matthew Powelson diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp index 47285a69f7..3b45c2c06f 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp @@ -1,5 +1,5 @@ /** - * @file simple_planner_interpolation_plan_profile.cpp + * @file simple_planner_fixed_size_plan_profile.cpp * @brief * * @author Matthew Powelson diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp index f74318b935..82f664368d 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp @@ -1,5 +1,5 @@ /** - * @file simple_planner_default_lvs_plan_profile.cpp + * @file simple_planner_lvs_plan_profile.cpp * @brief * * @author Tyler Marr From 6a07a43eeeee28bd42a4628e10a187a3173534ca Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 31 May 2024 20:37:53 +0200 Subject: [PATCH 08/19] Fix unit test --- ...simple_planner_fixed_size_assign_position.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp index 5373142863..0b9eb5319f 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp @@ -34,7 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit : public ::testing::Test +class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit : public ::testing::Test { protected: Environment::Ptr env_; @@ -68,7 +68,7 @@ class TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit : public ::testi } }; -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, JointCartesian_AssignJointPosition) // NOLINT { JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -80,7 +80,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian InstructionPoly instr3; - SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); + SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); std::vector move_instructions = profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); @@ -107,7 +107,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, CartesianJoint_AssignJointPosition) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -119,7 +119,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint InstructionPoly instr3; - SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); + SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); std::vector move_instructions = profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); @@ -145,7 +145,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCartesian_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, CartesianCartesian_AssignJointPosition) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -157,7 +157,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte InstructionPoly instr3; - SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); + SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); std::vector move_instructions = profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); auto fwd_kin = env_->getJointGroup(manip_info_.manipulator); From cfc4ca030b950206d74ae48e1de42ee3a16f29c3 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 4 Dec 2024 10:30:11 +0100 Subject: [PATCH 09/19] Add missing min and max_steps --- .../profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp index f3269c8884..b9dc325ee1 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp @@ -99,16 +99,19 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_ double trans_dist = (p2_world.translation() - p1_world.translation()).norm(); double rot_dist = Eigen::Quaterniond(p1_world.linear()).angularDistance(Eigen::Quaterniond(p2_world.linear())); + int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; - int steps = std::max(trans_steps, rot_steps); + int steps = std::max(trans_steps, rot_steps); if (has_j1 && has_j2) { double joint_dist = (j2 - j1).norm(); int joint_steps = int(joint_dist / state_longest_valid_segment_length) + 1; steps = std::max(steps, joint_steps); } + steps = std::max(steps, min_steps); + steps = std::min(steps, max_steps); Eigen::MatrixXd states; if (has_j2) From dafa2f7089ed2ce52b7c141eacd3bffbc1a05127 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 4 Dec 2024 10:36:20 +0100 Subject: [PATCH 10/19] Rename unit test file for SimplePlannerFixedSizeAssignNoIKPlanProfile --- .../simple/test/CMakeLists.txt | 22 +++++++++---------- ...nner_fixed_size_assign_no_ik_position.cpp} | 0 2 files changed, 11 insertions(+), 11 deletions(-) rename tesseract_motion_planners/simple/test/{simple_planner_fixed_size_assign_position.cpp => simple_planner_fixed_size_assign_no_ik_position.cpp} (100%) diff --git a/tesseract_motion_planners/simple/test/CMakeLists.txt b/tesseract_motion_planners/simple/test/CMakeLists.txt index f489def804..da08e3a787 100644 --- a/tesseract_motion_planners/simple/test/CMakeLists.txt +++ b/tesseract_motion_planners/simple/test/CMakeLists.txt @@ -21,26 +21,26 @@ add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_interpolation add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit ${PROJECT_NAME}_simple) add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit) -add_executable(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit - simple_planner_fixed_size_assign_position.cpp) -target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit PRIVATE GTest::GTest GTest::Main +add_executable(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit + simple_planner_fixed_size_assign_no_ik_position.cpp) +target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit +target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit +target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit PRIVATE +target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) target_code_coverage( - ${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit + ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit PRIVATE ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit) -add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit ${PROJECT_NAME}_simple) -add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit) +add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit) +add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit ${PROJECT_NAME}_simple) +add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit) add_executable(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit simple_planner_lvs_interpolation.cpp) target_link_libraries(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit PRIVATE GTest::GTest GTest::Main diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp similarity index 100% rename from tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp rename to tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp From d3ef34816e92f825c9231edb1e7c53d67882e043 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 4 Dec 2024 11:13:11 +0100 Subject: [PATCH 11/19] Refactor tests to match profile names --- .../simple/test/CMakeLists.txt | 76 +++++++++---------- ...ner_fixed_size_assign_no_ik_plan_unit.cpp} | 10 +-- ...> simple_planner_fixed_size_plan_unit.cpp} | 12 +-- ...n.cpp => simple_planner_lvs_plan_unit.cpp} | 20 ++--- 4 files changed, 58 insertions(+), 60 deletions(-) rename tesseract_motion_planners/simple/test/{simple_planner_fixed_size_assign_no_ik_position.cpp => simple_planner_fixed_size_assign_no_ik_plan_unit.cpp} (93%) rename tesseract_motion_planners/simple/test/{simple_planner_fixed_size_interpolation.cpp => simple_planner_fixed_size_plan_unit.cpp} (94%) rename tesseract_motion_planners/simple/test/{simple_planner_lvs_interpolation.cpp => simple_planner_lvs_plan_unit.cpp} (96%) diff --git a/tesseract_motion_planners/simple/test/CMakeLists.txt b/tesseract_motion_planners/simple/test/CMakeLists.txt index da08e3a787..f94c1e5ea6 100644 --- a/tesseract_motion_planners/simple/test/CMakeLists.txt +++ b/tesseract_motion_planners/simple/test/CMakeLists.txt @@ -1,62 +1,60 @@ find_package(tesseract_command_language REQUIRED) # SimplePlanner Tests -add_executable(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit simple_planner_fixed_size_interpolation.cpp) -target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit PRIVATE GTest::GTest GTest::Main - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit +add_executable(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit + simple_planner_fixed_size_assign_no_ik_plan_unit.cpp) +target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}_simple) +target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit +target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit PRIVATE +target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit + ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) target_code_coverage( - ${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit + ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit PRIVATE ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit) -add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit ${PROJECT_NAME}_simple) -add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_interpolation_unit) +add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit) +add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit ${PROJECT_NAME}_simple) +add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit) -add_executable(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit - simple_planner_fixed_size_assign_no_ik_position.cpp) -target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit PRIVATE GTest::GTest GTest::Main - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit - PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit - PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit PRIVATE - VERSION ${TESSERACT_CXX_VERSION}) +add_executable(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit simple_planner_fixed_size_plan_unit.cpp) +target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}_simple) +target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} + ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) +target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) target_code_coverage( - ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit + ${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit) -add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit ${PROJECT_NAME}_simple) -add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_position_unit) +add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit) +add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit ${PROJECT_NAME}_simple) +add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_plan_unit) -add_executable(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit simple_planner_lvs_interpolation.cpp) -target_link_libraries(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit PRIVATE GTest::GTest GTest::Main - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit - PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit - PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) +add_executable(${PROJECT_NAME}_simple_planner_lvs_plan_unit simple_planner_lvs_plan_unit.cpp) +target_link_libraries(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}_simple) +target_compile_options(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} + ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) +target_clang_tidy(${PROJECT_NAME}_simple_planner_lvs_plan_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) target_code_coverage( - ${PROJECT_NAME}_simple_planner_lvs_interpolation_unit + ${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit) -add_dependencies(${PROJECT_NAME}_simple_planner_lvs_interpolation_unit ${PROJECT_NAME}_simple) -add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_lvs_interpolation_unit) +add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_lvs_plan_unit) +add_dependencies(${PROJECT_NAME}_simple_planner_lvs_plan_unit ${PROJECT_NAME}_simple) +add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_lvs_plan_unit) diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_plan_unit.cpp similarity index 93% rename from tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp rename to tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_plan_unit.cpp index 0b9eb5319f..08dd9da505 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_plan_unit.cpp @@ -1,5 +1,5 @@ /** - * @file simple_planner_fixed_size_assign_position.cpp + * @file simple_planner_fixed_size_assign_no_ik_plan_unit.cpp * @brief * * @author Levi Armstrong @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit : public ::testing::Test +class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit : public ::testing::Test { protected: Environment::Ptr env_; @@ -68,7 +68,7 @@ class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit : public ::t } }; -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, JointCartesian_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, JointCartesian_AssignJointPosition) // NOLINT { JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -107,7 +107,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, JointCarte EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, CartesianJoint_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, CartesianJoint_AssignJointPosition) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -145,7 +145,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, CartesianJ EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPositionUnit, CartesianCartesian_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, CartesianCartesian_AssignJointPosition) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_plan_unit.cpp similarity index 94% rename from tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp rename to tesseract_motion_planners/simple/test/simple_planner_fixed_size_plan_unit.cpp index 4e8fd89402..943980b8e2 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_plan_unit.cpp @@ -1,5 +1,5 @@ /** - * @file simple_planner_fixed_size_interpolation.cpp + * @file simple_planner_fixed_size_plan_unit.cpp * @brief * * @author Matthew Powelson @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerFixedSizeInterpolationUnit : public ::testing::Test +class TesseractPlanningSimplePlannerFixedSizePlanProfileUnit : public ::testing::Test { protected: Environment::Ptr env_; @@ -68,7 +68,7 @@ class TesseractPlanningSimplePlannerFixedSizeInterpolationUnit : public ::testin } }; -TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_JointInterpolation) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizePlanProfileUnit, JointJoint_JointInterpolation) // NOLINT { JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -107,7 +107,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_JointInterpolation) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizePlanProfileUnit, JointCart_JointInterpolation) // NOLINT { JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -150,7 +150,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint EXPECT_TRUE(wp2.getTransform().isApprox(final_pose, 1e-3)); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_JointInterpolation) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizePlanProfileUnit, CartJoint_JointInterpolation) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); @@ -190,7 +190,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointInterpolation) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizePlanProfileUnit, CartCart_JointInterpolation) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, -0.1, 1); diff --git a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_lvs_plan_unit.cpp similarity index 96% rename from tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp rename to tesseract_motion_planners/simple/test/simple_planner_lvs_plan_unit.cpp index f7cc4431d9..ac2e688990 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_lvs_plan_unit.cpp @@ -1,5 +1,5 @@ /** - * @file simple_planner_fixed_size_interpolation.cpp + * @file simple_planner_lvs_plan_unit.cpp * @brief * * @author Matthew Powelson @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerLVSInterpolationUnit : public ::testing::Test +class TesseractPlanningSimplePlannerLVSPlanProfileUnit : public ::testing::Test { protected: Environment::Ptr env_; @@ -68,7 +68,7 @@ class TesseractPlanningSimplePlannerLVSInterpolationUnit : public ::testing::Tes } }; -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointJoint_Freespace) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_JointJoint_Freespace) // NOLINT { JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -121,7 +121,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo EXPECT_EQ(cl.size(), steps); } -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointJoint_Linear) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_JointJoint_Linear) // NOLINT { auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -186,7 +186,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo EXPECT_EQ(crl.size(), rot_steps); } -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Freespace) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_JointCart_Freespace) // NOLINT { auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -241,7 +241,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo EXPECT_TRUE(static_cast(cl.size()) > min_steps); } -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Linear) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_JointCart_Linear) // NOLINT { auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -307,7 +307,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo EXPECT_EQ(crl.size(), rot_steps); } -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Freespace) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_CartJoint_Freespace) // NOLINT { auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -360,7 +360,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo EXPECT_TRUE(static_cast(cl.size()) > min_steps); } -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Linear) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_CartJoint_Linear) // NOLINT { auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -425,7 +425,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo EXPECT_EQ(crl.size(), rot_steps); } -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Freespace) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_CartCart_Freespace) // NOLINT { auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -480,7 +480,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo EXPECT_TRUE(static_cast(cl.size()) > min_steps); } -TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Linear) // NOLINT +TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_CartCart_Linear) // NOLINT { auto joint_group = env_->getJointGroup(manip_info_.manipulator); From 950878f1ef7bb89377a356ffdc43b9ea523daac7 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 4 Dec 2024 12:02:29 +0100 Subject: [PATCH 12/19] Share test boilerplate --- ...nner_fixed_size_assign_no_ik_plan_unit.cpp | 37 ++--------- .../simple_planner_fixed_size_plan_unit.cpp | 34 +---------- .../test/simple_planner_lvs_plan_unit.cpp | 34 +---------- .../simple/test/simple_planner_test_utils.hpp | 61 +++++++++++++++++++ 4 files changed, 69 insertions(+), 97 deletions(-) create mode 100644 tesseract_motion_planners/simple/test/simple_planner_test_utils.hpp diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_plan_unit.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_plan_unit.cpp index 08dd9da505..42f4d61fd1 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_plan_unit.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_plan_unit.cpp @@ -23,49 +23,19 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include "simple_planner_test_utils.hpp" #include -#include -#include -#include #include -#include #include #include #include #include -#include -using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit : public ::testing::Test +class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit : public TesseractPlanningSimplePlannerUnit { -protected: - Environment::Ptr env_; - tesseract_common::ManipulatorInfo manip_info_; - std::vector joint_names_; - - void SetUp() override - { - auto locator = std::make_shared(); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); - tesseract_common::fs::path srdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - env_ = env; - - manip_info_.tcp_frame = "tool0"; - manip_info_.working_frame = "base_link"; - manip_info_.manipulator = "manipulator"; - joint_names_ = env_->getJointGroup("manipulator")->getJointNames(); - } }; TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, JointCartesian_AssignJointPosition) // NOLINT @@ -145,7 +115,8 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, Cartesi EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, CartesianCartesian_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, + CartesianCartesian_AssignJointPosition) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_plan_unit.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_plan_unit.cpp index 943980b8e2..1c0b364b78 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_plan_unit.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_plan_unit.cpp @@ -23,49 +23,19 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include "simple_planner_test_utils.hpp" #include -#include -#include -#include #include -#include #include #include #include #include -#include -using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerFixedSizePlanProfileUnit : public ::testing::Test +class TesseractPlanningSimplePlannerFixedSizePlanProfileUnit : public TesseractPlanningSimplePlannerUnit { -protected: - Environment::Ptr env_; - tesseract_common::ManipulatorInfo manip_info_; - std::vector joint_names_; - - void SetUp() override - { - auto locator = std::make_shared(); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); - tesseract_common::fs::path srdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - env_ = env; - - manip_info_.tcp_frame = "tool0"; - manip_info_.manipulator = "manipulator"; - manip_info_.working_frame = "base_link"; - joint_names_ = env_->getJointGroup("manipulator")->getJointNames(); - } }; TEST_F(TesseractPlanningSimplePlannerFixedSizePlanProfileUnit, JointJoint_JointInterpolation) // NOLINT diff --git a/tesseract_motion_planners/simple/test/simple_planner_lvs_plan_unit.cpp b/tesseract_motion_planners/simple/test/simple_planner_lvs_plan_unit.cpp index ac2e688990..b8baf8e938 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_lvs_plan_unit.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_lvs_plan_unit.cpp @@ -23,49 +23,19 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include "simple_planner_test_utils.hpp" #include -#include -#include -#include #include -#include #include #include #include #include -#include -using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerLVSPlanProfileUnit : public ::testing::Test +class TesseractPlanningSimplePlannerLVSPlanProfileUnit : public TesseractPlanningSimplePlannerUnit { -protected: - Environment::Ptr env_; - tesseract_common::ManipulatorInfo manip_info_; - std::vector joint_names_; - - void SetUp() override - { - auto locator = std::make_shared(); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); - tesseract_common::fs::path srdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - env_ = env; - - manip_info_.manipulator = "manipulator"; - manip_info_.tcp_frame = "tool0"; - manip_info_.working_frame = "base_link"; - joint_names_ = env_->getJointGroup("manipulator")->getJointNames(); - } }; TEST_F(TesseractPlanningSimplePlannerLVSPlanProfileUnit, InterpolateStateWaypoint_JointJoint_Freespace) // NOLINT diff --git a/tesseract_motion_planners/simple/test/simple_planner_test_utils.hpp b/tesseract_motion_planners/simple/test/simple_planner_test_utils.hpp new file mode 100644 index 0000000000..64c41765b4 --- /dev/null +++ b/tesseract_motion_planners/simple/test/simple_planner_test_utils.hpp @@ -0,0 +1,61 @@ +/** + * @file simple_planner_test_utils.cpp + * @brief + * + * @author Matthew Powelson + * @date July 23, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +using namespace tesseract_environment; + +class TesseractPlanningSimplePlannerUnit : public ::testing::Test +{ +protected: + Environment::Ptr env_; + tesseract_common::ManipulatorInfo manip_info_; + std::vector joint_names_; + + void SetUp() override + { + auto locator = std::make_shared(); + Environment::Ptr env = std::make_shared(); + tesseract_common::fs::path urdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); + tesseract_common::fs::path srdf_path( + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); + EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); + env_ = env; + + manip_info_.manipulator = "manipulator"; + manip_info_.tcp_frame = "tool0"; + manip_info_.working_frame = "base_link"; + joint_names_ = env_->getJointGroup("manipulator")->getJointNames(); + } +}; From ab495f4c81bbe8e9f3d2a6aaf7264db60e799fec Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 4 Dec 2024 12:16:08 +0100 Subject: [PATCH 13/19] Simplify CMakeLists --- .../simple/test/CMakeLists.txt | 78 +++++-------------- 1 file changed, 21 insertions(+), 57 deletions(-) diff --git a/tesseract_motion_planners/simple/test/CMakeLists.txt b/tesseract_motion_planners/simple/test/CMakeLists.txt index f94c1e5ea6..dc7d7f4d3f 100644 --- a/tesseract_motion_planners/simple/test/CMakeLists.txt +++ b/tesseract_motion_planners/simple/test/CMakeLists.txt @@ -1,60 +1,24 @@ find_package(tesseract_command_language REQUIRED) -# SimplePlanner Tests -add_executable(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit - simple_planner_fixed_size_assign_no_ik_plan_unit.cpp) -target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit PRIVATE GTest::GTest GTest::Main - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit - PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit - PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit - ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit PRIVATE - VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit - PRIVATE - ALL - EXCLUDE ${COVERAGE_EXCLUDE} - ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit) -add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit ${PROJECT_NAME}_simple) -add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit) +macro(add_gtest test_name test_file) + add_executable(${test_name} ${test_file}) + target_link_libraries(${test_name} PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}_simple) + target_compile_options(${test_name} PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) + target_compile_definitions(${test_name} PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) + target_clang_tidy(${test_name} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) + target_cxx_version(${test_name} PRIVATE VERSION ${TESSERACT_CXX_VERSION}) + target_code_coverage( + ${test_name} + PRIVATE + ALL + EXCLUDE ${COVERAGE_EXCLUDE} + ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) + add_gtest_discover_tests(${test_name}) + add_dependencies(${test_name} ${PROJECT_NAME}_simple) + add_dependencies(run_tests ${test_name}) +endmacro() -add_executable(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit simple_planner_fixed_size_plan_unit.cpp) -target_link_libraries(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE GTest::GTest GTest::Main - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} - ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_simple_planner_fixed_size_plan_unit - PRIVATE - ALL - EXCLUDE ${COVERAGE_EXCLUDE} - ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit) -add_dependencies(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit ${PROJECT_NAME}_simple) -add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_fixed_size_plan_unit) - -add_executable(${PROJECT_NAME}_simple_planner_lvs_plan_unit simple_planner_lvs_plan_unit.cpp) -target_link_libraries(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE GTest::GTest GTest::Main - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} - ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_simple_planner_lvs_plan_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_simple_planner_lvs_plan_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_simple_planner_lvs_plan_unit - PRIVATE - ALL - EXCLUDE ${COVERAGE_EXCLUDE} - ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_simple_planner_lvs_plan_unit) -add_dependencies(${PROJECT_NAME}_simple_planner_lvs_plan_unit ${PROJECT_NAME}_simple) -add_dependencies(run_tests ${PROJECT_NAME}_simple_planner_lvs_plan_unit) +add_gtest(${PROJECT_NAME}_simple_planner_fixed_size_assign_no_ik_plan_unit + simple_planner_fixed_size_assign_no_ik_plan_unit.cpp) +add_gtest(${PROJECT_NAME}_simple_planner_fixed_size_plan_unit simple_planner_fixed_size_plan_unit.cpp) +add_gtest(${PROJECT_NAME}_simple_planner_lvs_plan_unit simple_planner_lvs_plan_unit.cpp) From e1560e58e11aa288025df675b7cecea6a5a33545 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 17 Dec 2024 13:08:46 +0100 Subject: [PATCH 14/19] Remove PlanningRequest from simple profile interface --- ...ner_fixed_size_assign_no_ik_plan_profile.h | 2 +- ...le_planner_lvs_assign_no_ik_plan_profile.h | 2 +- .../simple_planner_lvs_assign_plan_profile.h | 2 +- ...r_fixed_size_assign_no_ik_plan_profile.cpp | 22 +++++++++---------- ..._planner_lvs_assign_no_ik_plan_profile.cpp | 9 ++++---- ...simple_planner_lvs_assign_plan_profile.cpp | 11 +++++----- 6 files changed, 25 insertions(+), 23 deletions(-) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h index 48c02e87fa..09f78a1d4a 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h @@ -48,7 +48,7 @@ class SimplePlannerFixedSizeAssignNoIKPlanProfile : public SimplePlannerPlanProf const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The number of steps to use for freespace instruction */ diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h index aaf97a4c30..0561d01caa 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h @@ -57,7 +57,7 @@ class SimplePlannerLVSAssignNoIKPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h index 53ca756ece..f98bf15d7a 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h @@ -57,7 +57,7 @@ class SimplePlannerLVSAssignPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp index eb05e45876..04b865bbfa 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp @@ -31,7 +31,7 @@ #include #include - +#include #include #include @@ -44,16 +44,16 @@ SimplePlannerFixedSizeAssignNoIKPlanProfile::SimplePlannerFixedSizeAssignNoIKPla { } -std::vector -SimplePlannerFixedSizeAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instruction, - const MoveInstructionPoly& /*prev_seed*/, - const MoveInstructionPoly& base_instruction, - const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& global_manip_info) const +std::vector SimplePlannerFixedSizeAssignNoIKPlanProfile::generate( + const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& /*prev_seed*/, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& /*next_instruction*/, + const std::shared_ptr& env, + const tesseract_common::ManipulatorInfo& global_manip_info) const { - JointGroupInstructionInfo prev(prev_instruction, request, global_manip_info); - JointGroupInstructionInfo base(base_instruction, request, global_manip_info); + JointGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + JointGroupInstructionInfo base(base_instruction, *env, global_manip_info); Eigen::MatrixXd states; if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) @@ -88,7 +88,7 @@ SimplePlannerFixedSizeAssignNoIKPlanProfile::generate(const MoveInstructionPoly& } else { - Eigen::VectorXd seed = request.env_state.getJointValues(base.manip->getJointNames()); + Eigen::VectorXd seed = env->getCurrentJointValues(base.manip->getJointNames()); tesseract_common::enforceLimits(seed, base.manip->getLimits().joint_limits); if (base.instruction.isLinear()) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp index b9dc325ee1..7a415c8a24 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -53,11 +54,11 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_ const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - JointGroupInstructionInfo prev(prev_instruction, request, global_manip_info); - JointGroupInstructionInfo base(base_instruction, request, global_manip_info); + JointGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + JointGroupInstructionInfo base(base_instruction, *env, global_manip_info); Eigen::VectorXd j1; Eigen::Isometry3d p1_world; @@ -124,7 +125,7 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_ } else { - Eigen::VectorXd seed = request.env_state.getJointValues(base.manip->getJointNames()); + Eigen::VectorXd seed = env->getCurrentJointValues(base.manip->getJointNames()); tesseract_common::enforceLimits(seed, base.manip->getLimits().joint_limits); states = seed.replicate(1, steps + 1); } diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp index e7617eb99d..a66b97c675 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -52,11 +53,11 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo prev(prev_instruction, request, global_manip_info); - KinematicGroupInstructionInfo base(base_instruction, request, global_manip_info); + KinematicGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo base(base_instruction, *env, global_manip_info); Eigen::VectorXd j1; Eigen::Isometry3d p1_world; @@ -87,7 +88,7 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst else { // Use current env_state as seed - j1 = getClosestJointSolution(prev, request.env_state.getJointValues(prev.manip->getJointNames())); + j1 = getClosestJointSolution(prev, env->getCurrentJointValues(prev.manip->getJointNames())); } } else @@ -128,7 +129,7 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst else { // Use current env_state as seed - j2 = getClosestJointSolution(base, request.env_state.getJointValues(base.manip->getJointNames())); + j2 = getClosestJointSolution(base, env->getCurrentJointValues(base.manip->getJointNames())); } } else From db7a8a0780766177f3c7ade6c4b54dac58508666 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 18 Dec 2024 12:39:12 +0100 Subject: [PATCH 15/19] Add serialization --- ...planner_fixed_size_assign_no_ik_plan_profile.h | 7 +++++++ ...simple_planner_lvs_assign_no_ik_plan_profile.h | 7 +++++++ .../simple_planner_lvs_assign_plan_profile.h | 7 +++++++ ...anner_fixed_size_assign_no_ik_plan_profile.cpp | 12 ++++++++++++ ...mple_planner_lvs_assign_no_ik_plan_profile.cpp | 15 +++++++++++++++ .../simple_planner_lvs_assign_plan_profile.cpp | 15 +++++++++++++++ 6 files changed, 63 insertions(+) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h index 09f78a1d4a..609595b8d0 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h @@ -56,8 +56,15 @@ class SimplePlannerFixedSizeAssignNoIKPlanProfile : public SimplePlannerPlanProf /** @brief The number of steps to use for linear instruction */ int linear_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerFixedSizeAssignNoIKPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h index 0561d01caa..6449251fba 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h @@ -74,8 +74,15 @@ class SimplePlannerLVSAssignNoIKPlanProfile : public SimplePlannerPlanProfile /** @brief The maximum number of steps for the plan */ int max_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSAssignNoIKPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h index f98bf15d7a..86ce0e0e61 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h @@ -74,8 +74,15 @@ class SimplePlannerLVSAssignPlanProfile : public SimplePlannerPlanProfile /** @brief The maximum number of steps for the plan */ int max_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSAssignPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp index 04b865bbfa..68808f0773 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp @@ -125,4 +125,16 @@ std::vector SimplePlannerFixedSizeAssignNoIKPlanProfile::ge return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); } +template +void SimplePlannerFixedSizeAssignNoIKPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(freespace_steps); + ar& BOOST_SERIALIZATION_NVP(linear_steps); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizeAssignNoIKPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignNoIKPlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp index 7a415c8a24..bd5113d760 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp @@ -144,4 +144,19 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_ return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); } +template +void SimplePlannerLVSAssignNoIKPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(translation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(rotation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(min_steps); + ar& BOOST_SERIALIZATION_NVP(max_steps); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerLVSAssignNoIKPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerLVSAssignNoIKPlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp index a66b97c675..bc0c3d6940 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp @@ -172,4 +172,19 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); } +template +void SimplePlannerLVSAssignPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(translation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(rotation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(min_steps); + ar& BOOST_SERIALIZATION_NVP(max_steps); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerLVSAssignPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerLVSAssignPlanProfile) From 61f35c277b4ca39272d9cc63b4b1837fb4b25454 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 4 Dec 2024 10:36:20 +0100 Subject: [PATCH 16/19] Rename unit test file for SimplePlannerFixedSizeAssignNoIKPlanProfile --- ...anner_fixed_size_assign_no_ik_position.cpp | 164 ++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp new file mode 100644 index 0000000000..42f4d61fd1 --- /dev/null +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp @@ -0,0 +1,164 @@ +/** + * @file simple_planner_fixed_size_assign_no_ik_plan_unit.cpp + * @brief + * + * @author Levi Armstrong + * @date July 28, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "simple_planner_test_utils.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace tesseract_planning; + +class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit : public TesseractPlanningSimplePlannerUnit +{ +}; + +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, JointCartesian_AssignJointPosition) // NOLINT +{ + JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); + MoveInstruction instr1_seed{ instr1 }; + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); + + CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; + MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); + + InstructionPoly instr3; + + SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); + std::vector move_instructions = + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); + EXPECT_EQ(move_instructions.size(), 10); + for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) + { + const MoveInstructionPoly& mi = move_instructions.at(i); + EXPECT_TRUE(mi.getWaypoint().isJointWaypoint()); + EXPECT_TRUE(wp1.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); + EXPECT_FALSE(mi.getWaypoint().as().isConstrained()); + if (instr2.getPathProfile().empty()) + { + EXPECT_EQ(mi.getProfile(), instr2.getProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); + } + else + { + EXPECT_EQ(mi.getProfile(), instr2.getPathProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); + } + } + const MoveInstructionPoly& mi = move_instructions.back(); + EXPECT_TRUE(wp1.getPosition().isApprox(mi.getWaypoint().as().getSeed().position, 1e-5)); + EXPECT_EQ(mi.getProfile(), instr2.getProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); +} + +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, CartesianJoint_AssignJointPosition) // NOLINT +{ + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); + MoveInstruction instr1_seed{ instr1 }; + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); + + JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; + MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); + + InstructionPoly instr3; + + SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); + std::vector move_instructions = + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); + EXPECT_EQ(move_instructions.size(), 10); + for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) + { + const MoveInstructionPoly& mi = move_instructions.at(i); + EXPECT_TRUE(mi.getWaypoint().isJointWaypoint()); + EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); + if (instr2.getPathProfile().empty()) + { + EXPECT_EQ(mi.getProfile(), instr2.getProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); + } + else + { + EXPECT_EQ(mi.getProfile(), instr2.getPathProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); + } + } + const MoveInstructionPoly& mi = move_instructions.back(); + EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); + EXPECT_EQ(mi.getProfile(), instr2.getProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); +} + +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, + CartesianCartesian_AssignJointPosition) // NOLINT +{ + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); + MoveInstruction instr1_seed{ instr1 }; + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); + + CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; + MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); + + InstructionPoly instr3; + + SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); + std::vector move_instructions = + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); + auto fwd_kin = env_->getJointGroup(manip_info_.manipulator); + Eigen::VectorXd position = env_->getCurrentJointValues(fwd_kin->getJointNames()); + EXPECT_EQ(move_instructions.size(), 10); + for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) + { + const MoveInstructionPoly& mi = move_instructions.at(i); + EXPECT_TRUE(mi.getWaypoint().isJointWaypoint()); + EXPECT_TRUE(position.isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); + if (instr2.getPathProfile().empty()) + { + EXPECT_EQ(mi.getProfile(), instr2.getProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); + } + else + { + EXPECT_EQ(mi.getProfile(), instr2.getPathProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); + } + } + const MoveInstructionPoly& mi = move_instructions.back(); + EXPECT_TRUE(position.isApprox(mi.getWaypoint().as().getSeed().position, 1e-5)); + EXPECT_EQ(mi.getProfile(), instr2.getProfile()); + EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From 6cd80f15adc62671dc0def6e2efda3d72e149d1f Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 4 Dec 2024 11:13:11 +0100 Subject: [PATCH 17/19] Refactor tests to match profile names --- ...anner_fixed_size_assign_no_ik_position.cpp | 164 ------------------ 1 file changed, 164 deletions(-) delete mode 100644 tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp deleted file mode 100644 index 42f4d61fd1..0000000000 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_no_ik_position.cpp +++ /dev/null @@ -1,164 +0,0 @@ -/** - * @file simple_planner_fixed_size_assign_no_ik_plan_unit.cpp - * @brief - * - * @author Levi Armstrong - * @date July 28, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include "simple_planner_test_utils.hpp" - -#include -#include -#include -#include -#include -#include - -using namespace tesseract_planning; - -class TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit : public TesseractPlanningSimplePlannerUnit -{ -}; - -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, JointCartesian_AssignJointPosition) // NOLINT -{ - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); - MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); - - CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); - - InstructionPoly instr3; - - SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); - std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); - EXPECT_EQ(move_instructions.size(), 10); - for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) - { - const MoveInstructionPoly& mi = move_instructions.at(i); - EXPECT_TRUE(mi.getWaypoint().isJointWaypoint()); - EXPECT_TRUE(wp1.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); - EXPECT_FALSE(mi.getWaypoint().as().isConstrained()); - if (instr2.getPathProfile().empty()) - { - EXPECT_EQ(mi.getProfile(), instr2.getProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); - } - else - { - EXPECT_EQ(mi.getProfile(), instr2.getPathProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); - } - } - const MoveInstructionPoly& mi = move_instructions.back(); - EXPECT_TRUE(wp1.getPosition().isApprox(mi.getWaypoint().as().getSeed().position, 1e-5)); - EXPECT_EQ(mi.getProfile(), instr2.getProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); -} - -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, CartesianJoint_AssignJointPosition) // NOLINT -{ - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); - MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); - - JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); - - InstructionPoly instr3; - - SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); - std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); - EXPECT_EQ(move_instructions.size(), 10); - for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) - { - const MoveInstructionPoly& mi = move_instructions.at(i); - EXPECT_TRUE(mi.getWaypoint().isJointWaypoint()); - EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); - if (instr2.getPathProfile().empty()) - { - EXPECT_EQ(mi.getProfile(), instr2.getProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); - } - else - { - EXPECT_EQ(mi.getProfile(), instr2.getPathProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); - } - } - const MoveInstructionPoly& mi = move_instructions.back(); - EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); - EXPECT_EQ(mi.getProfile(), instr2.getProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); -} - -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, - CartesianCartesian_AssignJointPosition) // NOLINT -{ - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); - MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); - - CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); - - InstructionPoly instr3; - - SimplePlannerFixedSizeAssignNoIKPlanProfile profile(10, 10); - std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); - auto fwd_kin = env_->getJointGroup(manip_info_.manipulator); - Eigen::VectorXd position = env_->getCurrentJointValues(fwd_kin->getJointNames()); - EXPECT_EQ(move_instructions.size(), 10); - for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) - { - const MoveInstructionPoly& mi = move_instructions.at(i); - EXPECT_TRUE(mi.getWaypoint().isJointWaypoint()); - EXPECT_TRUE(position.isApprox(mi.getWaypoint().as().getPosition(), 1e-5)); - if (instr2.getPathProfile().empty()) - { - EXPECT_EQ(mi.getProfile(), instr2.getProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); - } - else - { - EXPECT_EQ(mi.getProfile(), instr2.getPathProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); - } - } - const MoveInstructionPoly& mi = move_instructions.back(); - EXPECT_TRUE(position.isApprox(mi.getWaypoint().as().getSeed().position, 1e-5)); - EXPECT_EQ(mi.getProfile(), instr2.getProfile()); - EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} From 2b195045766b0aacb52924db7cc45ee2b68c9a17 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 8 Jan 2025 09:18:56 +0100 Subject: [PATCH 18/19] Order CMakeLists --- tesseract_motion_planners/simple/CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 126c7f3e93..e7c47bce88 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -2,15 +2,14 @@ add_library( ${PROJECT_NAME}_simple src/interpolation.cpp src/simple_motion_planner.cpp - src/profile/simple_planner_profile.cpp - src/profile/simple_planner_lvs_plan_profile.cpp - src/profile/simple_planner_lvs_no_ik_plan_profile.cpp + src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp src/profile/simple_planner_fixed_size_assign_plan_profile.cpp src/profile/simple_planner_fixed_size_plan_profile.cpp src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp src/profile/simple_planner_lvs_assign_plan_profile.cpp src/profile/simple_planner_lvs_no_ik_plan_profile.cpp - src/profile/simple_planner_lvs_plan_profile.cpp) + src/profile/simple_planner_lvs_plan_profile.cpp + src/profile/simple_planner_profile.cpp) target_link_libraries(${PROJECT_NAME}_simple PUBLIC ${PROJECT_NAME}_core Boost::boost) target_compile_options(${PROJECT_NAME}_simple PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) target_compile_options(${PROJECT_NAME}_simple PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) From 30730aa5d31b606e8b443f6672df0defa7ebab7f Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 8 Jan 2025 22:46:35 +0100 Subject: [PATCH 19/19] Refactor for consistency/clarity --- .../simple/src/interpolation.cpp | 64 ++++++++++--------- ...r_fixed_size_assign_no_ik_plan_profile.cpp | 21 +++--- ...planner_fixed_size_assign_plan_profile.cpp | 6 +- ...simple_planner_fixed_size_plan_profile.cpp | 19 +++--- ..._planner_lvs_assign_no_ik_plan_profile.cpp | 19 +++--- ...simple_planner_lvs_assign_plan_profile.cpp | 4 +- .../simple_planner_lvs_no_ik_plan_profile.cpp | 27 ++++---- .../simple_planner_lvs_plan_profile.cpp | 27 ++++---- 8 files changed, 94 insertions(+), 93 deletions(-) diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 7766a23d7f..cb5c829a20 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -373,6 +373,8 @@ std::vector interpolateCartCartWaypoint(const KinematicGrou tesseract_common::enforceLimits(seed, base.manip->getLimits().joint_limits); std::array sol; + auto& j1 = sol[0]; + auto& j2 = sol[1]; const auto& base_cwp = base.instruction.getWaypoint().as(); const auto& prev_cwp = prev.instruction.getWaypoint().as(); const bool base_has_seed = base_cwp.hasSeed(); @@ -380,18 +382,18 @@ std::vector interpolateCartCartWaypoint(const KinematicGrou if (base_has_seed && prev_has_seed) { - sol[0] = prev_cwp.getSeed().position; - sol[1] = base_cwp.getSeed().position; + j1 = prev_cwp.getSeed().position; + j2 = base_cwp.getSeed().position; } else if (!base_has_seed && prev_has_seed) { - sol[0] = prev_cwp.getSeed().position; - sol[1] = getClosestJointSolution(base, sol[0]); + j1 = prev_cwp.getSeed().position; + j2 = getClosestJointSolution(base, j1); } else if (base_has_seed && !prev_has_seed) { - sol[1] = base_cwp.getSeed().position; - sol[0] = getClosestJointSolution(prev, sol[1]); + j2 = base_cwp.getSeed().position; + j1 = getClosestJointSolution(prev, j2); } else { @@ -399,42 +401,42 @@ std::vector interpolateCartCartWaypoint(const KinematicGrou } Eigen::MatrixXd states; - if (sol[0].size() != 0 && sol[1].size() != 0) + if ((j1.size() != 0) && (j2.size() != 0)) { if (base.instruction.isLinear()) { if (linear_steps > 1) - states = interpolate(sol[0], sol[1], linear_steps); + states = interpolate(j1, j2, linear_steps); else - states = sol[1].replicate(1, 2); + states = j2.replicate(1, 2); } else if (base.instruction.isFreespace()) { if (freespace_steps > 1) - states = interpolate(sol[0], sol[1], freespace_steps); + states = interpolate(j1, j2, freespace_steps); else - states = sol[1].replicate(1, 2); + states = j2.replicate(1, 2); } else { throw std::runtime_error("SimplePlannerFixedSizePlanProfile: Unsupported MoveInstructionType!"); } } - else if (sol[0].size() != 0) + else if (j1.size() != 0) { if (base.instruction.isLinear()) - states = sol[0].replicate(1, linear_steps + 1); + states = j1.replicate(1, linear_steps + 1); else if (base.instruction.isFreespace()) - states = sol[0].replicate(1, freespace_steps + 1); + states = j1.replicate(1, freespace_steps + 1); else throw std::runtime_error("SimplePlannerFixedSizePlanProfile: Unsupported MoveInstructionType!"); } - else if (sol[1].size() != 0) + else if (j2.size() != 0) { if (base.instruction.isLinear()) - states = sol[1].replicate(1, linear_steps + 1); + states = j2.replicate(1, linear_steps + 1); else if (base.instruction.isFreespace()) - states = sol[1].replicate(1, freespace_steps + 1); + states = j2.replicate(1, freespace_steps + 1); else throw std::runtime_error("SimplePlannerFixedSizePlanProfile: Unsupported MoveInstructionType!"); } @@ -676,24 +678,26 @@ std::vector interpolateCartCartWaypoint(const KinematicGrou int steps = std::max(trans_steps, rot_steps); std::array sol; + auto& j1 = sol[0]; + auto& j2 = sol[1]; const auto& base_cwp = base.instruction.getWaypoint().as(); const auto& prev_cwp = prev.instruction.getWaypoint().as(); const bool base_has_seed = base_cwp.hasSeed(); const bool prev_has_seed = prev_cwp.hasSeed(); if (base_has_seed && prev_has_seed) { - sol[0] = prev_cwp.getSeed().position; - sol[1] = base_cwp.getSeed().position; + j1 = prev_cwp.getSeed().position; + j2 = base_cwp.getSeed().position; } else if (!base_has_seed && prev_has_seed) { - sol[0] = prev_cwp.getSeed().position; - sol[1] = getClosestJointSolution(base, sol[0]); + j1 = prev_cwp.getSeed().position; + j2 = getClosestJointSolution(base, j1); } else if (base_has_seed && !prev_has_seed) { - sol[1] = base_cwp.getSeed().position; - sol[0] = getClosestJointSolution(prev, sol[1]); + j2 = base_cwp.getSeed().position; + j1 = getClosestJointSolution(prev, j2); } else { @@ -701,9 +705,9 @@ std::vector interpolateCartCartWaypoint(const KinematicGrou } Eigen::MatrixXd states; - if (sol[0].size() != 0 && sol[1].size() != 0) + if ((j1.size() != 0) && (j2.size() != 0)) { - double joint_dist = (sol[1] - sol[0]).norm(); + double joint_dist = (j2 - j1).norm(); int state_steps = int(joint_dist / state_lvs_length) + 1; steps = std::max(steps, state_steps); @@ -712,23 +716,23 @@ std::vector interpolateCartCartWaypoint(const KinematicGrou steps = std::min(steps, max_steps); // Interpolate - states = interpolate(sol[0], sol[1], steps); + states = interpolate(j1, j2, steps); } - else if (sol[0].size() != 0) + else if (j1.size() != 0) { // Check min steps requirement steps = std::max(steps, min_steps); // Interpolate - states = sol[0].replicate(1, steps + 1); + states = j1.replicate(1, steps + 1); } - else if (sol[1].size() != 0) + else if (j2.size() != 0) { // Check min steps requirement steps = std::max(steps, min_steps); // Interpolate - states = sol[1].replicate(1, steps + 1); + states = j2.replicate(1, steps + 1); } else { diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp index 68808f0773..80066ba72d 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp @@ -33,9 +33,10 @@ #include #include #include - #include +#include + namespace tesseract_planning { SimplePlannerFixedSizeAssignNoIKPlanProfile::SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps, @@ -58,31 +59,31 @@ std::vector SimplePlannerFixedSizeAssignNoIKPlanProfile::ge Eigen::MatrixXd states; if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) { - const Eigen::VectorXd& jp = base.extractJointPosition(); + const Eigen::VectorXd& j2 = base.extractJointPosition(); if (base.instruction.isLinear()) - states = jp.replicate(1, linear_steps + 1); + states = j2.replicate(1, linear_steps + 1); else if (base.instruction.isFreespace()) - states = jp.replicate(1, freespace_steps + 1); + states = j2.replicate(1, freespace_steps + 1); else throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); } else if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint) { - const Eigen::VectorXd& jp = prev.extractJointPosition(); + const Eigen::VectorXd& j1 = prev.extractJointPosition(); if (base.instruction.isLinear()) - states = jp.replicate(1, linear_steps + 1); + states = j1.replicate(1, linear_steps + 1); else if (base.instruction.isFreespace()) - states = jp.replicate(1, freespace_steps + 1); + states = j1.replicate(1, freespace_steps + 1); else throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); } else if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) { - const Eigen::VectorXd& jp = base.extractJointPosition(); + const Eigen::VectorXd& j2 = base.extractJointPosition(); if (base.instruction.isLinear()) - states = jp.replicate(1, linear_steps + 1); + states = j2.replicate(1, linear_steps + 1); else if (base.instruction.isFreespace()) - states = jp.replicate(1, freespace_steps + 1); + states = j2.replicate(1, freespace_steps + 1); else throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); } diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp index 0eb022b0a1..67643bcd0b 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp @@ -24,7 +24,6 @@ * limitations under the License. */ -#include #include #include #include @@ -36,7 +35,6 @@ #include #include -#include #include namespace tesseract_planning @@ -124,7 +122,7 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre for (auto& pose : poses) pose = base.working_frame_transform.inverse() * pose; - assert(static_cast(poses.size()) == states.cols()); + assert(poses.size() == states.cols()); return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction); } @@ -143,4 +141,4 @@ void SimplePlannerFixedSizeAssignPlanProfile::serialize(Archive& ar, const unsig #include TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) \ No newline at end of file +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp index 3b45c2c06f..65a9f2e6be 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp @@ -33,7 +33,6 @@ #include #include -#include #include namespace tesseract_planning @@ -51,19 +50,19 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo base(base_instruction, *env, global_manip_info); - if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) - return interpolateJointJointWaypoint(info1, info2, linear_steps, freespace_steps); + if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + return interpolateJointJointWaypoint(prev, base, linear_steps, freespace_steps); - if (!info1.has_cartesian_waypoint && info2.has_cartesian_waypoint) - return interpolateJointCartWaypoint(info1, info2, linear_steps, freespace_steps); + if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint) + return interpolateJointCartWaypoint(prev, base, linear_steps, freespace_steps); - if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) - return interpolateCartJointWaypoint(info1, info2, linear_steps, freespace_steps); + if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + return interpolateCartJointWaypoint(prev, base, linear_steps, freespace_steps); - return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, env->getState()); + return interpolateCartCartWaypoint(prev, base, linear_steps, freespace_steps, env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp index bd5113d760..3bdb49f34f 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp @@ -24,15 +24,17 @@ * limitations under the License. */ +#include #include #include #include #include -#include #include #include #include +#include + namespace tesseract_planning { SimplePlannerLVSAssignNoIKPlanProfile::SimplePlannerLVSAssignNoIKPlanProfile( @@ -62,40 +64,34 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_ Eigen::VectorXd j1; Eigen::Isometry3d p1_world; - bool has_j1 = false; if (prev.has_cartesian_waypoint) { p1_world = prev.extractCartesianPose(); if (prev.instruction.getWaypoint().as().hasSeed()) { j1 = prev.instruction.getWaypoint().as().getSeed().position; - has_j1 = true; } } else { j1 = prev.extractJointPosition(); p1_world = prev.calcCartesianPose(j1); - has_j1 = true; } Eigen::VectorXd j2; Eigen::Isometry3d p2_world; - bool has_j2 = false; if (base.has_cartesian_waypoint) { p2_world = base.extractCartesianPose(); if (base.instruction.getWaypoint().as().hasSeed()) { j2 = base.instruction.getWaypoint().as().getSeed().position; - has_j2 = true; } } else { j2 = base.extractJointPosition(); p2_world = base.calcCartesianPose(j2); - has_j2 = true; } double trans_dist = (p2_world.translation() - p1_world.translation()).norm(); @@ -105,7 +101,7 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_ int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; int steps = std::max(trans_steps, rot_steps); - if (has_j1 && has_j2) + if ((j1.size() != 0) && (j2.size() != 0)) { double joint_dist = (j2 - j1).norm(); int joint_steps = int(joint_dist / state_longest_valid_segment_length) + 1; @@ -115,16 +111,19 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_ steps = std::min(steps, max_steps); Eigen::MatrixXd states; - if (has_j2) + if (j2.size() != 0) { + // Replicate base_instruction joint position states = j2.replicate(1, steps + 1); } - else if (has_j1) + else if (j1.size() != 0) { + // Replicate prev_instruction joint position states = j1.replicate(1, steps + 1); } else { + // Replicate current joint position Eigen::VectorXd seed = env->getCurrentJointValues(base.manip->getJointNames()); tesseract_common::enforceLimits(seed, base.manip->getLimits().joint_limits); states = seed.replicate(1, steps + 1); diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp index bc0c3d6940..d29d702c9b 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp @@ -24,15 +24,17 @@ * limitations under the License. */ +#include #include #include #include #include -#include #include #include #include +#include + namespace tesseract_planning { SimplePlannerLVSAssignPlanProfile::SimplePlannerLVSAssignPlanProfile(double state_longest_valid_segment_length, diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp index d11487843b..46775a8062 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp @@ -33,7 +33,6 @@ #include #include -#include #include namespace tesseract_planning @@ -59,38 +58,38 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - JointGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); - JointGroupInstructionInfo info2(base_instruction, *env, global_manip_info); + JointGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + JointGroupInstructionInfo base(base_instruction, *env, global_manip_info); - if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) - return interpolateJointJointWaypoint(info1, - info2, + if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + return interpolateJointJointWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length, min_steps, max_steps); - if (!info1.has_cartesian_waypoint && info2.has_cartesian_waypoint) - return interpolateJointCartWaypoint(info1, - info2, + if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint) + return interpolateJointCartWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length, min_steps, max_steps); - if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) - return interpolateCartJointWaypoint(info1, - info2, + if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + return interpolateCartJointWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length, min_steps, max_steps); - return interpolateCartCartWaypoint(info1, - info2, + return interpolateCartCartWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length, diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp index 82f664368d..e9149a10fb 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp @@ -33,7 +33,6 @@ #include #include -#include #include namespace tesseract_planning @@ -59,38 +58,38 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo base(base_instruction, *env, global_manip_info); - if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) - return interpolateJointJointWaypoint(info1, - info2, + if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + return interpolateJointJointWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length, min_steps, max_steps); - if (!info1.has_cartesian_waypoint && info2.has_cartesian_waypoint) - return interpolateJointCartWaypoint(info1, - info2, + if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint) + return interpolateJointCartWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length, min_steps, max_steps); - if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) - return interpolateCartJointWaypoint(info1, - info2, + if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + return interpolateCartJointWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length, min_steps, max_steps); - return interpolateCartCartWaypoint(info1, - info2, + return interpolateCartCartWaypoint(prev, + base, state_longest_valid_segment_length, translation_longest_valid_segment_length, rotation_longest_valid_segment_length,