Refactor for consistency/clarity

tesseract_motion_planners/simple/src/interpolation.cpp

@@ -373,68 +373,70 @@ std::vector<MoveInstructionPoly> interpolateCartCartWaypoint(const KinematicGrou
   tesseract_common::enforceLimits<double>(seed, base.manip->getLimits().joint_limits);
 
   std::array<Eigen::VectorXd, 2> sol;
+  auto& j1 = sol[0];
+  auto& j2 = sol[1];
   const auto& base_cwp = base.instruction.getWaypoint().as<CartesianWaypointPoly>();
   const auto& prev_cwp = prev.instruction.getWaypoint().as<CartesianWaypointPoly>();
   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
   {
     sol = getClosestJointSolution(prev, base, seed);
   }
 
   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,34 +678,36 @@ std::vector<MoveInstructionPoly> interpolateCartCartWaypoint(const KinematicGrou
   int steps = std::max(trans_steps, rot_steps);
 
   std::array<Eigen::VectorXd, 2> sol;
+  auto& j1 = sol[0];
+  auto& j2 = sol[1];
   const auto& base_cwp = base.instruction.getWaypoint().as<CartesianWaypointPoly>();
   const auto& prev_cwp = prev.instruction.getWaypoint().as<CartesianWaypointPoly>();
   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
   {
     sol = getClosestJointSolution(prev, base, seed);
   }
 
   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<MoveInstructionPoly> 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
   {

tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp

@@ -33,9 +33,10 @@
 #include <tesseract_common/kinematic_limits.h>
 #include <tesseract_environment/environment.h>
 #include <tesseract_kinematics/core/kinematic_group.h>
-
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 
+#include <boost/serialization/nvp.hpp>
+
 namespace tesseract_planning
 {
 SimplePlannerFixedSizeAssignNoIKPlanProfile::SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps,
@@ -58,31 +59,31 @@ std::vector<MoveInstructionPoly> 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!");
   }

tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp

@@ -24,7 +24,6 @@
  * limitations under the License.
  */
 
-#include <tesseract_command_language/cartesian_waypoint.h>
 #include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h>
 #include <tesseract_motion_planners/simple/interpolation.h>
 #include <tesseract_motion_planners/core/types.h>
@@ -36,7 +35,6 @@
 #include <tesseract_kinematics/core/kinematic_group.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 
-#include <boost/serialization/base_object.hpp>
 #include <boost/serialization/nvp.hpp>
 
 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<Eigen::Index>(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_common/serialization.h>
 TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile)
-BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile)
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile)

tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp

@@ -33,7 +33,6 @@
 #include <tesseract_environment/environment.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 
-#include <boost/serialization/base_object.hpp>
 #include <boost/serialization/nvp.hpp>
 
 namespace tesseract_planning
@@ -51,19 +50,19 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst
                                             const std::shared_ptr<const tesseract_environment::Environment>& 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 <class Archive>

tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp

@@ -24,15 +24,17 @@
  * limitations under the License.
  */
 
+#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h>
 #include <tesseract_command_language/cartesian_waypoint.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 #include <tesseract_environment/environment.h>
 #include <tesseract_kinematics/core/kinematic_group.h>
-#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h>
 #include <tesseract_motion_planners/simple/interpolation.h>
 #include <tesseract_motion_planners/core/utils.h>
 #include <tesseract_motion_planners/core/types.h>
 
+#include <boost/serialization/nvp.hpp>
+
 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<CartesianWaypointPoly>().hasSeed())
     {
       j1 = prev.instruction.getWaypoint().as<CartesianWaypointPoly>().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<CartesianWaypointPoly>().hasSeed())
     {
       j2 = base.instruction.getWaypoint().as<CartesianWaypointPoly>().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<double>(seed, base.manip->getLimits().joint_limits);
     states = seed.replicate(1, steps + 1);

tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp

@@ -24,15 +24,17 @@
  * limitations under the License.
  */
 
+#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h>
 #include <tesseract_command_language/cartesian_waypoint.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 #include <tesseract_environment/environment.h>
 #include <tesseract_kinematics/core/kinematic_group.h>
-#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h>
 #include <tesseract_motion_planners/simple/interpolation.h>
 #include <tesseract_motion_planners/core/utils.h>
 #include <tesseract_motion_planners/core/types.h>
 
+#include <boost/serialization/nvp.hpp>
+
 namespace tesseract_planning
 {
 SimplePlannerLVSAssignPlanProfile::SimplePlannerLVSAssignPlanProfile(double state_longest_valid_segment_length,