Added support for QUADRATIC though using CUBIC

To avoid sudden jump in velocity
Also updated the spline example

examples/full_driver.cpp

@@ -110,8 +110,8 @@ int main(int argc, char* argv[])
   const bool HEADLESS = true;
   g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
                                  std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
-  g_my_driver->setKeepaliveCount(5);  // This is for example purposes only. This will make the example running more 
-                                                              // reliable on non-realtime systems. Do not use this in productive applications.
+  g_my_driver->setKeepaliveCount(5);  // This is for example purposes only. This will make the example running more
+                                      // reliable on non-realtime systems. Do not use this in productive applications.
 
   // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
   // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main

examples/spline_example.cpp

@@ -42,6 +42,8 @@ vector6d_t g_joint_positions;
 void SendTrajectory(const std::vector<vector6d_t>& p_p, const std::vector<vector6d_t>& p_v,
                     const std::vector<vector6d_t>& p_a, const std::vector<double>& time, bool use_spline_interpolation_)
 {
+  assert(p_p.size() == time.size());
+
   URCL_LOG_INFO("Starting joint-based trajectory forward");
   g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, p_p.size());
 
@@ -130,18 +132,23 @@ int main(int argc, char* argv[])
     return 1;
   }
 
-  // Power it off
-  if (!g_my_dashboard->commandPowerOff())
+  // if the robot is not powered on and ready
+  std::string robotModeRunning("RUNNING");
+  while (!g_my_dashboard->commandRobotMode(robotModeRunning))
   {
-    URCL_LOG_ERROR("Could not send Power off command");
-    return 1;
-  }
+    // Power it off
+    if (!g_my_dashboard->commandPowerOff())
+    {
+      URCL_LOG_ERROR("Could not send Power off command");
+      return 1;
+    }
 
-  // Power it on
-  if (!g_my_dashboard->commandPowerOn())
-  {
-    URCL_LOG_ERROR("Could not send Power on command");
-    return 1;
+    // Power it on
+    if (!g_my_dashboard->commandPowerOn())
+    {
+      URCL_LOG_ERROR("Could not send Power on command");
+      return 1;
+    }
   }
 
   // Release the brakes
@@ -187,7 +194,7 @@ int main(int argc, char* argv[])
                                            8.50000000e-02 };
   const double s_acc[number_of_points] = { 2.55885417e+00, -4.97395833e-01, 1.71276042e+00, -5.36458333e-02,
                                            -2.69817708e+00 };
-  const double s_time[number_of_points] = { 1.0000000e+00, 4.00000000e+00, 8.00100000e+00, 2.50000000e+01 / 2,
+  const double s_time[number_of_points] = { 1.0000000e+00, 4.00000000e+00, 8.00100000e+00, 1.25000000e+01,
                                             4.00000000e+00 };
 
   bool ret = false;
@@ -220,6 +227,34 @@ int main(int argc, char* argv[])
     time.push_back(s_time[i]);
   }
 
+  // QUADRATIC
+  SendTrajectory(p, std::vector<vector6d_t>(), std::vector<vector6d_t>(), time, true);
+
+  g_trajectory_running = true;
+  while (g_trajectory_running)
+  {
+    std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
+    if (data_pkg)
+    {
+      // Read current joint positions from robot data
+      if (!data_pkg->getData("actual_q", g_joint_positions))
+      {
+        // This throwing should never happen unless misconfigured
+        std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
+        throw std::runtime_error(error_msg);
+      }
+      bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+
+      if (!ret)
+      {
+        URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+        return 1;
+      }
+    }
+  }
+
+  URCL_LOG_INFO("QUADRATIC Movement done");
+
   // CUBIC
   SendTrajectory(p, v, std::vector<vector6d_t>(), time, true);
 

resources/external_control.urscript

@@ -55,7 +55,7 @@ FREEDRIVE_MODE_STOP = -1
 UNTIL_TOOL_CONTACT_RESULT_SUCCESS = 0
 UNTIL_TOOL_CONTACT_RESULT_CANCELED = 1
 
-SPLINE_QUADRATIC = 0 # Not implemented yet
+SPLINE_QUADRATIC = 0 # Implemented with Cubic spline
 SPLINE_CUBIC = 1
 SPLINE_QUINTIC = 2
 
@@ -145,7 +145,6 @@ thread speedThread():
 end
 
 def cubicSplineRun(end_q, end_qd, time):
-  textmsg("cubicSplineRun time: ", time)
   # Zero time means zero length and therefore no motion to execute
   if time > 0.0:
     local start_q = get_target_joint_positions()
@@ -242,11 +241,17 @@ thread trajectoryThread():
 
         # Joint spline point
         elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
+          # Quadratic spline
+          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUADRATIC:
+            # Implemented with cubic slines to get a jump in velocity
+            cubicSplineRun(q, [0, 0, 0, 0, 0, 0], tmptime)
+            # reset old acceleration
+            last_spline_qdd = [0, 0, 0, 0, 0, 0]
+
           # Cubic spline
-          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
+          elif 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)
-
             # reset old acceleration
             last_spline_qdd = [0, 0, 0, 0, 0, 0]
 
@@ -264,8 +269,9 @@ thread trajectoryThread():
         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=0.0)
         elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
-          # Cubic spline
-          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
+          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUADRATIC:
+            cubicSplineRun(q, [0,0,0,0,0,0], tmptime)
+          elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
             cubicSplineRun(q, [0,0,0,0,0,0], tmptime)
           elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
             quinticSplineRun(q, [0,0,0,0,0,0], [0,0,0,0,0,0], tmptime)

src/control/trajectory_point_interface.cpp

@@ -98,7 +98,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
     return false;
   }
 
-  control::TrajectorySplineType spline_type = control::TrajectorySplineType::SPLINE_QUINTIC;
+  control::TrajectorySplineType spline_type = control::TrajectorySplineType::SPLINE_QUADRATIC;
 
   // 6 positions, 6 velocities, 6 accelerations, 1 goal time, spline type, 1 point type
   uint8_t buffer[sizeof(int32_t) * MESSAGE_LENGTH] = { 0 };
@@ -120,6 +120,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
 
   if (velocities != nullptr)
   {
+    spline_type = control::TrajectorySplineType::SPLINE_CUBIC;
     for (auto const& vel : *velocities)
     {
       int32_t val = static_cast<int32_t>(vel * MULT_JOINTSTATE);
@@ -149,8 +150,6 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
   }
   else
   {
-    // Use cubic splines, when acceleration is not part of the trajectory
-    spline_type = control::TrajectorySplineType::SPLINE_CUBIC;
     b_pos += 6 * sizeof(int32_t);
   }