TOTG can't handle mixed units (rad + meters)

moveit · Dec 13, 2022 · fb0cc36 · fb0cc36
1 parent 2e13217
commit fb0cc36
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 0 deletions.
diff --git a/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
@@ -207,6 +207,9 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
                                           const Eigen::VectorXd& max_velocity,
                                           const Eigen::VectorXd& max_acceleration) const;
 
+  /// @brief Check if a combination of revolute and prismatic joints is used. path_tolerance_ is not valid, if so.
+  bool checkMixedUnits(const moveit::core::JointModelGroup* group) const;
+
   const double path_tolerance_;
   const double resample_dt_;
   const double min_angle_change_;

diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -1096,6 +1096,12 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t
     return false;
   }
 
+  if (!checkMixedUnits(group))
+  {
+    RCLCPP_ERROR(LOGGER, "TOTG cannot process a combination of revolute and prismatic joints");
+    return false;
+  }
+
   const unsigned num_points = trajectory.getWayPointCount();
   const std::vector<int>& idx = group->getVariableIndexList();
   const unsigned num_joints = group->getVariableCount();
@@ -1179,4 +1185,23 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t
 
   return true;
 }
+
+bool TimeOptimalTrajectoryGeneration::checkMixedUnits(const moveit::core::JointModelGroup* group) const
+{
+  const std::vector<const moveit::core::JointModel*>& joint_models = group->getJointModels();
+
+  bool have_prismatic =
+      std::any_of(joint_models.cbegin(), joint_models.cend(), [](const moveit::core::JointModel* joint_model) {
+        return joint_model->getType() == moveit::core::JointModel::JointType::PRISMATIC;
+      });
+
+  bool have_revolute =
+      std::any_of(joint_models.cbegin(), joint_models.cend(), [](const moveit::core::JointModel* joint_model) {
+        return joint_model->getType() == moveit::core::JointModel::JointType::REVOLUTE;
+      });
+
+  if (have_prismatic && have_revolute)
+    return false;
+  return true;
+}
 }  // namespace trajectory_processing