rsk2

UniversalRobots · Jun 30, 2023 · dafbdb8 · dafbdb8
1 parent df3f81a
commit dafbdb8
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 16 deletions.
diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp
@@ -81,7 +81,6 @@ void handleRobotProgramState(bool program_running)
 }
 
 // Callback function for trajectory execution.
-// control::TrajectoryResult trajectory_state(TRAJECTORY_RESULT_CANCELED);
 bool g_trajectory_running(false);
 void handleTrajectoryState(control::TrajectoryResult state)
 {
@@ -225,8 +224,8 @@ int main(int argc, char* argv[])
   // CUBIC
   SendTrajectory(p, v, std::vector<vector6d_t>(), time, true);
 
-  trajectory_running = true;
-  while (trajectory_running)
+  g_trajectory_running = true;
+  while (g_trajectory_running)
   {
     std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
     if (data_pkg)
@@ -238,9 +237,7 @@ int main(int argc, char* argv[])
         std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
         throw std::runtime_error(error_msg);
       }
-      // g_joint_positions[0] = s_pos[0];  // Move to initial position of the spline
       bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
-      // bool ret = g_my_driver->writeKeepalive();
 
       if (!ret)
       {
@@ -256,10 +253,9 @@ int main(int argc, char* argv[])
   SendTrajectory(p, v, a, time, true);
   ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
 
-  trajectory_running = true;
-  while (trajectory_running)
+  g_trajectory_running = true;
+  while (g_trajectory_running)
   {
-    // g_my_driver->getDataPackage();
     std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
     if (data_pkg)
     {
@@ -271,7 +267,6 @@ int main(int argc, char* argv[])
         throw std::runtime_error(error_msg);
       }
       bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
-      // bool ret = g_my_driver->writeKeepalive();
 
       if (!ret)
       {

diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h
@@ -54,7 +54,7 @@ enum class TrajectoryResult : int32_t
  */
 enum class TrajectorySplineType : int32_t
 {
-  SPLINE_QUADRATIC = 0.
+  SPLINE_QUADRATIC = 0,
   SPLINE_CUBIC = 1,
   SPLINE_QUINTIC = 2
 };

diff --git a/resources/external_control.urscript b/resources/external_control.urscript
@@ -55,8 +55,9 @@ FREEDRIVE_MODE_STOP = -1
 UNTIL_TOOL_CONTACT_RESULT_SUCCESS = 0
 UNTIL_TOOL_CONTACT_RESULT_CANCELED = 1
 
-CUBIC_SPLINES = 0
-QUINTIC_SPLINES = 1
+SPLINE_QUADRATIC = 0 # Not implemented yet
+SPLINE_CUBIC = 1
+SPLINE_QUINTIC = 2
 
 #Global variables are also showed in the Teach pendants variable list
 global cmd_servo_state = SERVO_UNINITIALIZED
@@ -242,15 +243,15 @@ thread trajectoryThread():
         # Joint spline point
         elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
           # Cubic spline
-          if raw_point[INDEX_SPLINE_TYPE] == CUBIC_SPLINES:
+          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
             qd = [ raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
             cubicSplineRun(q, qd, tmptime)
 
             # reset old acceleration
             last_spline_qdd = [0, 0, 0, 0, 0, 0]
 
           # Quintic spline
-          elif raw_point[INDEX_SPLINE_TYPE] == QUINTIC_SPLINES:
+          elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
             qd = [ raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
             qdd = [ raw_point[13], raw_point[14], raw_point[15], raw_point[16], raw_point[17], raw_point[18]] / MULT_jointstate
             quinticSplineRun(q, qd, qdd, tmptime)
@@ -264,9 +265,9 @@ thread trajectoryThread():
           movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=0.0)
         elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
           # Cubic spline
-          if raw_point[INDEX_SPLINE_TYPE] == CUBIC_SPLINES:
+          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
             cubicSplineRun(q, [0,0,0,0,0,0], tmptime)
-          elif raw_point[INDEX_SPLINE_TYPE] == QUINTIC_SPLINES:
+          elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
             quinticSplineRun(q, [0,0,0,0,0,0], [0,0,0,0,0,0], tmptime)
           end
         end