From b6610251a2696260082382f4361a7cfcad38487b Mon Sep 17 00:00:00 2001
From: John Stechschulte <john@picknik.ai>
Date: Tue, 18 May 2021 15:09:56 -0600
Subject: [PATCH] RobotState interpolation: warn if interpolation parameter is
 out of range [0, 1] (#2664)

---
 .../robot_model/include/moveit/robot_model/robot_model.h | 9 +++++++++
 moveit_core/robot_model/src/robot_model.cpp              | 6 +++++-
 moveit_core/robot_state/src/robot_state.cpp              | 7 ++++++-
 3 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h
index f79317bd734..318518f78ea 100644
--- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h
+++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h
@@ -62,6 +62,15 @@ namespace core
 {
 MOVEIT_CLASS_FORWARD(RobotModel);  // Defines RobotModelPtr, ConstPtr, WeakPtr... etc
 
+static inline void checkInterpolationParamBounds(const char LOGNAME[], double t)
+{
+  if (std::isnan(t) || std::isinf(t))
+  {
+    throw Exception("Interpolation parameter is NaN or inf.");
+  }
+  ROS_WARN_STREAM_COND_NAMED(t < 0. || t > 1., LOGNAME, "Interpolation parameter is not in the range [0, 1]: " << t);
+}
+
 /** \brief Definition of a kinematic model. This class is not thread
     safe, however multiple instances can be created */
 class RobotModel
diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp
index 7d8dd786fec..c28c4d1c0c8 100644
--- a/moveit_core/robot_model/src/robot_model.cpp
+++ b/moveit_core/robot_model/src/robot_model.cpp
@@ -49,7 +49,10 @@ namespace moveit
 {
 namespace core
 {
-const std::string LOGNAME = "robot_model";
+namespace
+{
+constexpr char LOGNAME[] = "robot_model";
+}  // namespace
 
 RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model)
 {
@@ -1275,6 +1278,7 @@ double RobotModel::distance(const double* state1, const double* state2) const
 
 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
 {
+  moveit::core::checkInterpolationParamBounds(LOGNAME, t);
   // we interpolate values only for active joint models (non-mimic)
   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
     active_joint_model_vector_[i]->interpolate(from + active_joint_model_start_index_[i],
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
index 1bd5b129800..748f63bfce6 100644
--- a/moveit_core/robot_state/src/robot_state.cpp
+++ b/moveit_core/robot_state/src/robot_state.cpp
@@ -55,7 +55,10 @@ namespace core
  * valid paths from paths with large joint space jumps. */
 static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10;
 
-const std::string LOGNAME = "robot_state";
+namespace
+{
+constexpr char LOGNAME[] = "robot_state";
+}  // namespace
 
 RobotState::RobotState(const RobotModelConstPtr& robot_model)
   : robot_model_(robot_model)
@@ -910,6 +913,7 @@ double RobotState::distance(const RobotState& other, const JointModelGroup* join
 
 void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const
 {
+  moveit::core::checkInterpolationParamBounds(LOGNAME, t);
   robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions());
 
   memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() * sizeof(unsigned char));
@@ -918,6 +922,7 @@ void RobotState::interpolate(const RobotState& to, double t, RobotState& state)
 
 void RobotState::interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const
 {
+  moveit::core::checkInterpolationParamBounds(LOGNAME, t);
   const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
   for (std::size_t i = 0; i < jm.size(); ++i)
   {