From 30730aa5d31b606e8b443f6672df0defa7ebab7f Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 8 Jan 2025 22:46:35 +0100 Subject: [PATCH] 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,