Added UR exceptions InvalidRange and IncompatibleRobotVersion

Added them to the various checks for starting force mode.
I created IncompatibleRobotVersion instead of using VersionMismatch, as the VersionMismatch would not give enough information in my opinion.

include/ur_client_library/exceptions.h

@@ -124,5 +124,40 @@ class TimeoutException : public UrException
 private:
   std::string text_;
 };
+
+class IncompatibleRobotVersion : public UrException
+{
+  public:
+    explicit IncompatibleRobotVersion() = delete;
+    explicit IncompatibleRobotVersion(std::string text, int required_robot_version, int actual_robot_version): std::runtime_error(text)
+    {
+      std::stringstream ss;
+      ss << text << "\n" << "The requested feature is incompatible with the connected robot. Required Polyscope version: " << required_robot_version << ", actual Polyscope version: " << actual_robot_version;
+      text_ = ss.str();
+    }
+    virtual const char* what() const noexcept override
+    {
+      return text_.c_str();
+    }
+
+    private:
+      std::string text_;
+};
+
+class InvalidRange : public UrException
+{
+  private:
+    std::string text_;
+  public:
+    explicit InvalidRange() = delete;
+    explicit InvalidRange(std::string text): std::runtime_error(text)
+    {
+      text_ = text;
+    }
+    virtual const char* what() const noexcept override
+    {
+      return text_.c_str();
+    }
+};
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED

src/ur/ur_driver.cpp

@@ -364,7 +364,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
     std::stringstream ss;
     ss << "Force mode gain scaling factor cannot be set on a CB3 robot.";
     URCL_LOG_ERROR(ss.str().c_str());
-    throw std::invalid_argument(ss.str().c_str());
+    throw IncompatibleRobotVersion(ss.str(), 5, robot_version_.major);
   }
   // Test that the type is either 1, 2 or 3.
   switch (type)
@@ -379,7 +379,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
       std::stringstream ss;
       ss << "The type should be 1, 2 or 3. The type is " << type;
       URCL_LOG_ERROR(ss.str().c_str());
-      throw std::invalid_argument(ss.str().c_str());
+      throw InvalidRange(ss.str().c_str());
   }
   for (unsigned int i = 0; i < selection_vector.size(); ++i)
   {
@@ -388,7 +388,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
       std::stringstream ss;
       ss << "The selection vector should only consist of 0's and 1's";
       URCL_LOG_ERROR(ss.str().c_str());
-      throw std::invalid_argument(ss.str().c_str());
+      throw InvalidRange(ss.str().c_str());
     }
   }
 
@@ -397,15 +397,15 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
     std::stringstream ss;
     ss << "The force mode damping factor should be between 0 and 1, both inclusive.";
     URCL_LOG_ERROR(ss.str().c_str());
-    throw std::invalid_argument(ss.str().c_str());
+    throw InvalidRange(ss.str().c_str());
   }
 
   if (gain_scaling_factor > 2 || gain_scaling_factor < 0)
   {
     std::stringstream ss;
     ss << "The force mode gain scaling factor should be between 0 and 2, both inclusive.";
     URCL_LOG_ERROR(ss.str().c_str());
-    throw std::invalid_argument(ss.str().c_str());
+    throw InvalidRange(ss.str().c_str());
   }
 
   if (script_command_interface_->clientConnected())
@@ -430,7 +430,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
     std::stringstream ss;
     ss << "You should also specify a force mode gain scaling factor to activate force mode on an e-series robot.";
     URCL_LOG_ERROR(ss.str().c_str());
-    throw std::invalid_argument(ss.str().c_str());
+    throw IncompatibleRobotVersion(ss.str(), 3, robot_version_.major);
   }
   // Test that the type is either 1, 2 or 3.
   switch (type)
@@ -445,7 +445,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
       std::stringstream ss;
       ss << "The type should be 1, 2 or 3. The type is " << type;
       URCL_LOG_ERROR(ss.str().c_str());
-      throw std::invalid_argument(ss.str().c_str());
+      throw InvalidRange(ss.str().c_str());
   }
   for (unsigned int i = 0; i < selection_vector.size(); ++i)
   {
@@ -454,7 +454,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
       std::stringstream ss;
       ss << "The selection vector should only consist of 0's and 1's";
       URCL_LOG_ERROR(ss.str().c_str());
-      throw std::invalid_argument(ss.str().c_str());
+      throw InvalidRange(ss.str().c_str());
     }
   }
 
@@ -463,7 +463,7 @@ bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_
     std::stringstream ss;
     ss << "The force mode damping factor should be between 0 and 1, both inclusive.";
     URCL_LOG_ERROR(ss.str().c_str());
-    throw std::invalid_argument(ss.str().c_str());
+    throw InvalidRange(ss.str().c_str());
   }
 
   if (script_command_interface_->clientConnected())