diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h
index ca8348368ca..73f56d768fb 100644
--- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h
+++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h
@@ -8,7 +8,7 @@ namespace tesseract_planning
 class OMPLWaypointProfile : public WaypointProfile
 {
 public:
-  virtual std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override;
+  std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override;
 
 private:
   friend class boost::serialization::access;
diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
index f2c072a94da..bd6cd12a92b 100644
--- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
+++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
@@ -81,7 +81,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def,
 }
 
 std::vector<ompl::base::ScopedState<>> createOMPLStates(const std::vector<Eigen::VectorXd>& joint_states,
-                                                        ompl::base::SpaceInformationPtr si)
+                                                        const ompl::base::SpaceInformationPtr& si)
 {
   std::vector<ompl::base::ScopedState<>> states;
   states.reserve(joint_states.size());
@@ -98,7 +98,7 @@ std::vector<ompl::base::ScopedState<>> createOMPLStates(const std::vector<Eigen:
   return states;
 }
 
-std::shared_ptr<ompl::base::PlannerData> plan(ompl::geometric::SimpleSetupPtr simple_setup,
+std::shared_ptr<ompl::base::PlannerData> plan(const ompl::geometric::SimpleSetupPtr& simple_setup,
                                               const OMPLPlannerParameters& params,
                                               const unsigned n_output_states)
 {
@@ -220,7 +220,7 @@ std::shared_ptr<ompl::base::PlannerData> plan(ompl::geometric::SimpleSetupPtr si
   // space to be considered equivalent are not merged or connected. Also different planners add edges in different ways
   // and set different vertex/edge properties, so it may not make sense to combine them
   auto planner_data = std::make_shared<ompl::base::PlannerData>(simple_setup->getSpaceInformation());
-  for (auto planner : planners)
+  for (const ompl::base::PlannerPtr& planner : planners)
     planner->getPlannerData(*planner_data);
 
   // The planning data is actually owned by the planner instances themselves. Deep copy the planning information by
@@ -240,7 +240,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra
   // In this case, we need to insert more states into the composite to cover the difference
   // Remember the composite does not include the start state, so compare its size with one less than the size of the
   // trajectory
-  if (seed.size() < trajectory.rows() - 1)
+  if (static_cast<Eigen::Index>(seed.size()) < trajectory.rows() - 1)
   {
     const std::size_t diff = static_cast<std::size_t>(trajectory.rows() - 1) - output.size();
     output.insert(output.end(), diff, output.back());
@@ -259,7 +259,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra
       // Subsequent trajectory states go into the composite instruction
       // The index into the composite of these states is one less than the index of the trajectory state since the first
       // trajectory state was saved outside the composite
-      const std::size_t composite_idx = static_cast<std::size_t>(i - 1);
+      const auto composite_idx = static_cast<std::size_t>(i - 1);
       auto& move_instruction = output.at(composite_idx).as<MoveInstruction>();
       move_instruction.getWaypoint().as<StateWaypoint>().position = trajectory.row(i);
     }
@@ -342,7 +342,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ
         // Get the start waypoint profile and add the states to the SimpleSetup
         if (i == 0)
         {
-          const PlanInstruction& pi = request.instructions.getStartInstruction().as<PlanInstruction>();
+          const auto& pi = request.instructions.getStartInstruction().as<PlanInstruction>();
           const std::string profile_name =
               getProfileString(name_, pi.getProfile(), request.composite_profile_remapping);
           WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name);
@@ -351,8 +351,8 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ
         else
         {
           // Use the last state of the previous trajectory as the single start state for this plan
-          const MoveInstruction& mi = response.results.back().as<CompositeInstruction>().back().as<MoveInstruction>();
-          const StateWaypoint& sw = mi.getWaypoint().as<StateWaypoint>();
+          const auto& mi = response.results.back().as<CompositeInstruction>().back().as<MoveInstruction>();
+          const auto& sw = mi.getWaypoint().as<StateWaypoint>();
           start_states.push_back(sw.position);
         }
 
@@ -365,12 +365,12 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ
 
       // Add the goal waypoint(s)
       {
-        const PlanInstruction& pi = request.instructions[i].as<PlanInstruction>();
+        const auto& pi = request.instructions[i].as<PlanInstruction>();
 
         const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping);
         WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name);
 
-        std::vector<Eigen::VectorXd> states = std::any_cast<std::vector<Eigen::VectorXd>>(p->create(pi, *request.env));
+        auto states = std::any_cast<std::vector<Eigen::VectorXd>>(p->create(pi, *request.env));
         auto ompl_states = createOMPLStates(states, simple_setup->getSpaceInformation());
 
         auto goal_states = std::make_shared<ompl::base::GoalStates>(simple_setup->getSpaceInformation());
@@ -428,14 +428,14 @@ bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request)
 {
   // Check that parameters are valid
   if (request.env == nullptr)
-    std::runtime_error("Environment is invalid (nullptr)");
+    throw std::runtime_error("Environment is invalid (nullptr)");
 
   if (request.instructions.empty())
-    std::runtime_error("Request contains no instructions");
+    throw std::runtime_error("Request contains no instructions");
 
   if (request.instructions.size() != request.seed.size())
-    std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) +
-                       ") does not match seed size (" + std::to_string(request.seed.size()) + ")");
+    throw std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) +
+                             ") does not match seed size (" + std::to_string(request.seed.size()) + ")");
 
   return true;
 }
diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp
index b3b2352ccce..3a4d7e5c321 100644
--- a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp
+++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp
@@ -21,7 +21,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio
 {
   for (const Instruction& inst : instruction)
   {
-    const PlanInstruction& goal_instruction = inst.as<PlanInstruction>();
+    const auto& goal_instruction = inst.as<PlanInstruction>();
     if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE)
       throw std::runtime_error("Only freespace plan instruction types are currently supported");
   }
@@ -87,7 +87,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio
           break;
         }
         default:
-          CONSOLE_BRIDGE_logDebug("DOING NOTHING HERE");
+          CONSOLE_BRIDGE_logDebug("No collision state validator added");
       }
       simple_setup->setStateValidityChecker(csvc);
     }
@@ -124,7 +124,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio
         break;
       }
       default:
-        CONSOLE_BRIDGE_logDebug("Nothing doing");
+        CONSOLE_BRIDGE_logDebug("No collision validator added");
     }
 
     simple_setup->getSpaceInformation()->setMotionValidator(mv);
diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp
index 034738854ca..ddbf416ba4b 100644
--- a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp
+++ b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp
@@ -5,7 +5,7 @@ namespace tesseract_planning
 {
 void checkCollision(const Eigen::VectorXd& state,
                     const tesseract_environment::Environment& env,
-                    tesseract_kinematics::JointGroup::ConstPtr manip)
+                    const tesseract_kinematics::JointGroup::ConstPtr& manip)
 {
   tesseract_collision::DiscreteContactManager::Ptr contact_checker = env.getDiscreteContactManager();
   tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state);
@@ -28,7 +28,8 @@ void checkCollision(const Eigen::VectorXd& state,
   }
 }
 
-Eigen::VectorXd updateLimits(Eigen::Ref<const Eigen::VectorXd> joint_waypoint, tesseract_common::KinematicLimits limits)
+Eigen::VectorXd updateLimits(const Eigen::Ref<const Eigen::VectorXd>& joint_waypoint,
+                             const tesseract_common::KinematicLimits& limits)
 {
   if (!tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits))
     throw std::runtime_error("State violates joint limits");
@@ -65,12 +66,12 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c
   tesseract_common::KinematicLimits limits = manip->getLimits();
   tesseract_kinematics::IKSolutions valid_solutions;
   valid_solutions.reserve(joint_solutions.size());
-  for (std::size_t i = 0; i < joint_solutions.size(); ++i)
+  for (const Eigen::VectorXd& js : joint_solutions)
   {
     try
     {
       // Update the joint solution based on the kinematic limits
-      Eigen::VectorXd solution = updateLimits(joint_solutions[i], limits);
+      Eigen::VectorXd solution = updateLimits(js, limits);
 
       // Ensure the solution is collision-free
       checkCollision(solution, env, manip);
@@ -99,25 +100,27 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c
 std::any OMPLWaypointProfile::create(const Instruction& instruction,
                                      const tesseract_environment::Environment& env) const
 {
-  PlanInstruction plan_instruction = instruction.as<PlanInstruction>();
-  tesseract_common::ManipulatorInfo mi = plan_instruction.getManipulatorInfo();
+  const auto& plan_instruction = instruction.as<PlanInstruction>();
+  const tesseract_common::ManipulatorInfo& mi = plan_instruction.getManipulatorInfo();
   const Waypoint& waypoint = plan_instruction.getWaypoint();
 
   if (isCartesianWaypoint(waypoint))
   {
-    const CartesianWaypoint& cw = waypoint.as<CartesianWaypoint>();
+    const auto& cw = waypoint.as<CartesianWaypoint>();
     return getValidIKSolutions(cw, mi, env);
   }
-  else if (isJointWaypoint(waypoint))
+
+  if (isJointWaypoint(waypoint))
   {
-    const JointWaypoint& jw = waypoint.as<JointWaypoint>();
+    const auto& jw = waypoint.as<JointWaypoint>();
     const Eigen::VectorXd updated_state = updateLimits(jw, env.getJointGroup(mi.manipulator)->getLimits());
     checkCollision(updated_state, env, env.getJointGroup(mi.manipulator));
     return std::vector<Eigen::VectorXd>{ updated_state };
   }
-  else if (isStateWaypoint(waypoint))
+
+  if (isStateWaypoint(waypoint))
   {
-    const StateWaypoint& sw = waypoint.as<StateWaypoint>();
+    const auto& sw = waypoint.as<StateWaypoint>();
     Eigen::Map<const Eigen::VectorXd> state(sw.position.data(), sw.position.size());
     const Eigen::VectorXd updated_state = updateLimits(state, env.getJointGroup(mi.manipulator)->getLimits());
     checkCollision(updated_state, env, env.getJointGroup(mi.manipulator));