Address review

sjahr · Oct 26, 2023 · 2bfaf1e · 2bfaf1e
1 parent a0b3a64
commit 2bfaf1e
Show file tree

Hide file tree

Showing 5 changed files with 11 additions and 11 deletions.
diff --git a/moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp b/moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp
@@ -47,10 +47,10 @@ namespace cost_functions
 {
 [[nodiscard]] ::planning_interface::StateCostFn
 createMinJointDisplacementCostFn(moveit::core::RobotState& robot_state, const std::string& group_name,
-                              const planning_scene::PlanningSceneConstPtr& planning_scene);
+                                 const planning_scene::PlanningSceneConstPtr& planning_scene);
 
 [[nodiscard]] ::planning_interface::StateCostFn
 createClearanceCostFn(moveit::core::RobotState& robot_state, const std::string& group_name,
-                   const planning_scene::PlanningSceneConstPtr& planning_scene);
+                      const planning_scene::PlanningSceneConstPtr& planning_scene);
 }  // namespace cost_functions
 }  // namespace moveit
diff --git a/moveit_core/cost_functions/src/cost_functions.cpp b/moveit_core/cost_functions/src/cost_functions.cpp
@@ -42,10 +42,11 @@ namespace cost_functions
 {
 [[nodiscard]] ::planning_interface::StateCostFn
 createClearanceCostFn(moveit::core::RobotState& robot_state, const std::string& group_name,
-                   const planning_scene::PlanningSceneConstPtr& planning_scene)
+                      const planning_scene::PlanningSceneConstPtr& planning_scene)
 {
   // Create cost function
   return [robot_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable {
+    assert(state_vector.size() == robot_state.getRobotModel()->getJointModelGroup(group_name)->getActiveVariableCount());
     robot_state.setJointGroupActivePositions(group_name, state_vector);
     auto const shortest_distance_to_collision = planning_scene->distanceToCollision(robot_state);
 
@@ -61,11 +62,12 @@ createClearanceCostFn(moveit::core::RobotState& robot_state, const std::string&
 
 [[nodiscard]] ::planning_interface::StateCostFn
 createMinJointDisplacementCostFn(moveit::core::RobotState& robot_state, const std::string& group_name,
-                              const planning_scene::PlanningSceneConstPtr& planning_scene)
+                                 const planning_scene::PlanningSceneConstPtr& planning_scene)
 {
   return [robot_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable {
+    assert(state_vector.size() == robot_state.getRobotModel()->getJointModelGroup(group_name)->getActiveVariableCount());
     robot_state.setJointGroupActivePositions(group_name, state_vector);
-    auto const current_state = planning_scene->getCurrentState();
+    const auto& current_state = planning_scene->getCurrentState();
 
     return current_state.distance(robot_state, robot_state.getJointModelGroup(group_name));
   };

diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp
@@ -89,8 +89,8 @@ CostFn get_cost_function_from_state_validator(const StateValidatorFn& state_vali
     // with a gaussian, penalizing neighboring valid states as well.
     for (int timestep = 0; timestep < values.cols() - 1; ++timestep)
     {
-      Eigen::VectorXd current = values.col(timestep);
-      Eigen::VectorXd next = values.col(timestep + 1);
+      const Eigen::VectorXd& current = values.col(timestep);
+      const Eigen::VectorXd& next = values.col(timestep + 1);
       const double segment_distance = (next - current).norm();
       double interpolation_fraction = 0.0;
       const double interpolation_step = std::min(0.5, interpolation_step_size / segment_distance);
@@ -230,7 +230,7 @@ CostFn get_cost_function_from_moveit_state_cost_fn(const ::planning_interface::S
 {
   CostFn cost_fn = [&](const Eigen::MatrixXd& values, Eigen::VectorXd& costs, bool& validity) {
     validity = true;  // TODO(sjahr): Discuss if that is always the case
-    for (int timestep = 0; timestep < values.cols() - 1; ++timestep)
+    for (int timestep = 0; timestep < values.cols(); ++timestep)
     {
       Eigen::VectorXd current_state_vector = values.col(timestep);
       costs(timestep) = state_cost_function(current_state_vector);

diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp
@@ -152,7 +152,6 @@ stomp::TaskPtr createStompTask(const stomp::StompConfiguration& config, StompPla
   // TODO(henningkayser): parameterize cost penalties
   using namespace stomp_moveit;
   std::vector<CostFn> cost_functions;
-  cost_functions.reserve(3);  // Expect a collision, a constraint and a state cost function
   cost_functions.push_back(costs::get_collision_cost_function(planning_scene, group, 1.0 /* collision penalty */));
 
   if (!constraints.empty())
@@ -163,7 +162,6 @@ stomp::TaskPtr createStompTask(const stomp::StompConfiguration& config, StompPla
 
   if (context.getMotionPlanRequest().state_cost_function != nullptr)
   {
-    std::cout << "Using user defined state cost function!" << std::endl;
     cost_functions.push_back(
         costs::get_cost_function_from_moveit_state_cost_fn(context.getMotionPlanRequest().state_cost_function));
   }

diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
@@ -238,7 +238,7 @@ class PlanningComponent
   const std::string group_name_;
   // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources
   const moveit::core::JointModelGroup* joint_model_group_;
-  planning_interface::StateCostFn state_cost_function_;
+  planning_interface::StateCostFn state_cost_function_ = nullptr;
 
   // Planning
   // The start state used in the planning motion request