Ensure that the targets are reachable within the robots limits (#184)

If there are jumps commanded to the robot the robot will not execute the motion.

This should serve as a safety mechanism to avoid sudden unexpected motions due to illegal input.

Co-authored-by: Felix Exner <[email protected]>

Author: urmahp

examples/full_driver.cpp

@@ -117,12 +117,20 @@ int main(int argc, char* argv[])
 
   g_my_driver->startRTDECommunication();
 
-  double increment = 0.01;
+  // Increment depends on robot version
+  double increment_constant = 0.0005;
+  if (g_my_driver->getVersion().major < 5)
+  {
+    increment_constant = 0.002;
+  }
+  double increment = increment_constant;
 
-  bool passed_slow_part = false;
-  bool passed_fast_part = false;
+  bool first_pass = true;
+  bool passed_negative_part = false;
+  bool passed_positive_part = false;
   URCL_LOG_INFO("Start moving the robot");
-  while (!(passed_slow_part && passed_fast_part))
+  urcl::vector6d_t joint_target = { 0, 0, 0, 0, 0, 0 };
+  while (!(passed_positive_part && passed_negative_part))
   {
     // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
     // robot will effectively be in charge of setting the frequency of this loop.
@@ -139,21 +147,33 @@ int main(int argc, char* argv[])
         throw std::runtime_error(error_msg);
       }
 
-      // Simple motion command of last joint
-      if (g_joint_positions[5] > 3)
+      if (first_pass)
+      {
+        joint_target = g_joint_positions;
+        first_pass = false;
+      }
+
+      // Open loop control. The target is incremented with a constant each control loop
+      if (passed_positive_part == false)
       {
-        passed_fast_part = increment > 0.01 || passed_fast_part;
-        increment = -3;  // this large jump will activate speed scaling
+        increment = increment_constant;
+        if (g_joint_positions[5] >= 2)
+        {
+          passed_positive_part = true;
+        }
       }
-      else if (g_joint_positions[5] < -3)
+      else if (passed_negative_part == false)
       {
-        passed_slow_part = increment < 0.01 || passed_slow_part;
-        increment = 0.02;
+        increment = -increment_constant;
+        if (g_joint_positions[5] <= 0)
+        {
+          passed_negative_part = true;
+        }
       }
-      g_joint_positions[5] += increment;
+      joint_target[5] += increment;
       // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
       // reliable on non-realtime systems. Use with caution in productive applications.
-      bool ret = g_my_driver->writeJointCommand(g_joint_positions, comm::ControlMode::MODE_SERVOJ,
+      bool ret = g_my_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ,
                                                 RobotReceiveTimeout::millisec(100));
       if (!ret)
       {