diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 0b63020236..838cfbd454 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -6,7 +6,12 @@ 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_assign_no_ik_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) 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_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..609595b8d0 --- /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,70 @@ +/** + * @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 std::shared_ptr& env, + 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; + +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_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..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 @@ -1,13 +1,13 @@ /** - * @file simple_planner_default_plan_profile.h + * @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/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/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..6449251fba --- /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,88 @@ +/** + * @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 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. */ + 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; + +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 new file mode 100644 index 0000000000..86ce0e0e61 --- /dev/null +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h @@ -0,0 +1,88 @@ +/** + * @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 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. */ + 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; + +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 new file mode 100644 index 0000000000..68808f0773 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp @@ -0,0 +1,140 @@ +/** + * @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 + +#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 std::shared_ptr& env, + const tesseract_common::ManipulatorInfo& global_manip_info) const +{ + 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) + { + 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 = env->getCurrentJointValues(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); +} + +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_fixed_size_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp index 6fcd5097e7..2a630b3489 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 @@ -54,77 +55,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) - { - 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) + 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!"); + 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 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_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..bd5113d760 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp @@ -0,0 +1,162 @@ +/** + * @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 +#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 std::shared_ptr& env, + const tesseract_common::ManipulatorInfo& global_manip_info) const +{ + JointGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + JointGroupInstructionInfo base(base_instruction, *env, 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); + } + steps = std::max(steps, min_steps); + steps = std::min(steps, max_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 = env->getCurrentJointValues(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); +} + +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 new file mode 100644 index 0000000000..bc0c3d6940 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp @@ -0,0 +1,190 @@ +/** + * @file simple_planner_lvs_assign_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 +#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 std::shared_ptr& env, + const tesseract_common::ManipulatorInfo& global_manip_info) const +{ + KinematicGroupInstructionInfo prev(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo base(base_instruction, *env, 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, env->getCurrentJointValues(prev.manip->getJointNames())); + } + } + else + { + // Use base_instruction as seed + j1 = getClosestJointSolution(prev, base.extractJointPosition()); + } + } + tesseract_common::enforceLimits(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, 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); + } + + 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); +} + +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) 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 diff --git a/tesseract_motion_planners/simple/test/CMakeLists.txt b/tesseract_motion_planners/simple/test/CMakeLists.txt index f489def804..dc7d7f4d3f 100644 --- a/tesseract_motion_planners/simple/test/CMakeLists.txt +++ b/tesseract_motion_planners/simple/test/CMakeLists.txt @@ -1,62 +1,24 @@ 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 - PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_interpolation_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 - VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_simple_planner_fixed_size_interpolation_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) +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_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 - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_simple_planner_fixed_size_assign_position_unit - PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_simple_planner_fixed_size_assign_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 - VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_simple_planner_fixed_size_assign_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_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}) -target_code_coverage( - ${PROJECT_NAME}_simple_planner_lvs_interpolation_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(${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) 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_plan_unit.cpp similarity index 75% 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_plan_unit.cpp index 5373142863..42f4d61fd1 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_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 @@ -23,52 +23,22 @@ * 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 -#include -using namespace tesseract_environment; using namespace tesseract_planning; -class TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit : 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(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, 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_); @@ -80,7 +50,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 +77,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, CartesianJoint_AssignJointPosition) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -119,7 +89,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 +115,8 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile()); } -TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCartesian_AssignJointPosition) // NOLINT +TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignNoIKPlanProfileUnit, + CartesianCartesian_AssignJointPosition) // NOLINT { CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -157,7 +128,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); 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 82% 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..1c0b364b78 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 @@ -23,52 +23,22 @@ * 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 TesseractPlanningSimplePlannerFixedSizeInterpolationUnit : 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(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 +77,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 +120,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 +160,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 91% 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..b8baf8e938 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 @@ -23,52 +23,22 @@ * 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 TesseractPlanningSimplePlannerLVSInterpolationUnit : 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(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 +91,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 +156,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 +211,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 +277,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 +330,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 +395,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 +450,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); 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(); + } +};