More fixes

urmahp · Jul 16, 2024 · af925db · af925db
1 parent 3405529
commit af925db
Show file tree

Hide file tree

Showing 2 changed files with 48 additions and 60 deletions.
diff --git a/resources/external_control.urscript b/resources/external_control.urscript
@@ -414,57 +414,55 @@ thread trajectoryThread():
   spline_qd = [0, 0, 0, 0, 0, 0]
   enter_critical
   trajectory_result = TRAJECTORY_RESULT_SUCCESS
-  while trajectory_result == TRAJECTORY_RESULT_SUCCESS:
-    while trajectory_points_left > 0:
-      #reading trajectory point + blend radius + type of point (cartesian/joint based)
-      local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime())
-      trajectory_points_left = trajectory_points_left - 1
-
-      if raw_point[0] > 0:
-        local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate]
-        local tmptime = raw_point[INDEX_TIME] / MULT_time
-        local blend_radius = raw_point[INDEX_BLEND] / MULT_time
-        local is_last_point = False
-        if trajectory_points_left == 0:
-          blend_radius = 0.0
-          is_last_point = True
-        end
-        # MoveJ point
-        if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT:
-          movej(q, t=tmptime, r=blend_radius)
+  while trajectory_result == TRAJECTORY_RESULT_SUCCESS and trajectory_points_left > 0:
+    #reading trajectory point + blend radius + type of point (cartesian/joint based)
+    local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime())
+    trajectory_points_left = trajectory_points_left - 1
 
-          # reset old acceleration
-          spline_qdd = [0, 0, 0, 0, 0, 0]
-          spline_qd = [0, 0, 0, 0, 0, 0]
+    if raw_point[0] > 0:
+      local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate]
+      local tmptime = raw_point[INDEX_TIME] / MULT_time
+      local blend_radius = raw_point[INDEX_BLEND] / MULT_time
+      local is_last_point = False
+      if trajectory_points_left == 0:
+        blend_radius = 0.0
+        is_last_point = True
+      end
+      # MoveJ point
+      if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT:
+        movej(q, t=tmptime, r=blend_radius)
 
-        # Movel point
-        elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN:
-          movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius)
+        # reset old acceleration
+        spline_qdd = [0, 0, 0, 0, 0, 0]
+        spline_qd = [0, 0, 0, 0, 0, 0]
 
+      # Movel point
+      elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN:
+        movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius)
+
+        # reset old acceleration
+        spline_qdd = [0, 0, 0, 0, 0, 0]
+        spline_qd = [0, 0, 0, 0, 0, 0]
+
+      # Joint spline point
+      elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
+
+        # Cubic spline
+        if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
+          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
+          cubicSplineRun(q, qd, tmptime, is_last_point)
           # reset old acceleration
           spline_qdd = [0, 0, 0, 0, 0, 0]
-          spline_qd = [0, 0, 0, 0, 0, 0]
-
-        # Joint spline point
-        elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
-
-          # Cubic spline
-          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
-            qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
-            cubicSplineRun(q, qd, tmptime, is_last_point)
-            # reset old acceleration
-            spline_qdd = [0, 0, 0, 0, 0, 0]
-
-          # Quintic spline
-          elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
-            qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
-            qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate]
-            quinticSplineRun(q, qd, qdd, tmptime, is_last_point)
-          else:
-            textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])
-            clear_remaining_trajectory_points()
-            trajectory_result = TRAJECTORY_RESULT_FAILURE
-          end
+
+        # Quintic spline
+        elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
+          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
+          qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate]
+          quinticSplineRun(q, qd, qdd, tmptime, is_last_point)
+        else:
+          textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])
+          clear_remaining_trajectory_points()
+          trajectory_result = TRAJECTORY_RESULT_FAILURE
         end
       end
     end

diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp
@@ -338,21 +338,11 @@ TEST_F(UrDriverTest, target_outside_limits_pose)
 
   // Create physically unfeasible target
   urcl::vector6d_t tcp_target = tcp_pose_before;
-  tcp_target[2] += 0.2;
+  tcp_target[2] += 0.3;
 
-  double timeout = 0.2;
-  double cur_time = 0.0;
-  while (g_program_running && cur_time < timeout)
-  {
-    // Send unfeasible targets to the robot
-    readDataPackage(data_pkg);
-    g_ur_driver_->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200));
-    cur_time += step_time_;
-  }
-
-  // We expect the control script to stop running when targets are unfeasible meaning that the timeout shouldn't be
-  // reached
-  EXPECT_LT(cur_time, timeout);
+  // Send unfeasible targets to the robot
+  readDataPackage(data_pkg);
+  g_ur_driver_->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200));
 
   // Ensure that the robot didn't move
   readDataPackage(data_pkg);