25% speed up of goal critic; 1% speed up from vy striding when not in…

… use
ros-navigation · Sep 21, 2023 · 1b13476 · 1b13476
1 parent 91b688d
commit 1b13476
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 2 deletions.
diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp
@@ -57,7 +57,7 @@ void GoalCritic::score(CriticData & data)
     xt::pow(traj_x - goal_x, 2) +
     xt::pow(traj_y - goal_y, 2));
 
-  data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_);
+  data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_);
 }
 
 }  // namespace mppi::critics

diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
@@ -378,8 +378,10 @@ void Optimizer::updateControlSequence()
   auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis()));
 
   xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate);
-  xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
   xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate);
+  if (isHolonomic()) {
+    xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
+  }
 
   applyControlSequenceConstraints();
 }