Implemented spline interpolation in joint space (#543)

* Added service to activate or deactivate spline interpolation
* Added velocities to test move function in order to force cubic spline interpolation
* Default to using spline interpolation when doing trajectory forwarding.

I think this approach is superior to hardcoding a blend radius with MoveJ
commands. This way we will result in a path that is closer to the one planned
by MoveIt!

* Position only spline interpolation got explicitly removed.

---------

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

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -38,6 +38,7 @@
 #include <std_msgs/Float64.h>
 #include <std_msgs/String.h>
 #include <std_srvs/Trigger.h>
+#include <std_srvs/SetBool.h>
 #include <realtime_tools/realtime_publisher.h>
 #include <tf2_msgs/TFMessage.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -213,6 +214,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
   void commandCallback(const std_msgs::StringConstPtr& msg);
   bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res);
+  bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res);
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> dashboard_client_;
@@ -236,6 +238,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   ros::ServiceServer deactivate_srv_;
   ros::ServiceServer tare_sensor_srv_;
   ros::ServiceServer set_payload_srv_;
+  ros::ServiceServer activate_spline_interpolation_srv_;
 
   hardware_interface::JointStateInterface js_interface_;
   scaled_controllers::ScaledPositionJointInterface spj_interface_;
@@ -325,6 +328,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   std::atomic<bool> cartesian_forward_controller_running_;
   std::atomic<bool> twist_controller_running_;
   std::atomic<bool> pose_controller_running_;
+  std::atomic<bool> use_spline_interpolation_;
 
   PausingState pausing_state_;
   double pausing_ramp_up_increment_;

ur_robot_driver/launch/ur_common.launch

@@ -24,6 +24,7 @@
   <arg name="robot_description_file" doc="Robot description launch file."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
+  <arg name="use_spline_interpolation" default="true" doc="True if splines should be used as interpolation on the robot controller when forwarding trajectory, if false movej or movel commands are used"/>
 
   <!-- robot model -->
   <include file="$(arg robot_description_file)">
@@ -57,5 +58,6 @@
     <arg name="tool_device_name" value="$(arg tool_device_name)"/>
     <arg name="tool_tcp_port" value="$(arg tool_tcp_port)"/>
     <arg name="ur_hardware_interface_node_required" value="$(arg ur_hardware_interface_node_required)"/>
+    <arg name="use_spline_interpolation" value="$(arg use_spline_interpolation)"/>
   </include>
 </launch>

ur_robot_driver/launch/ur_control.launch

@@ -33,6 +33,7 @@
   <arg name="servoj_gain" default="2000" doc="Specify gain for servoing to position in joint space. A higher gain can sharpen the trajectory."/>
   <arg name="servoj_lookahead_time" default="0.03" doc="Specify lookahead time for servoing to position in joint space. A longer lookahead time can smooth the trajectory."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
+  <arg name="use_spline_interpolation" default="true" doc="True if splines should be used as interpolation on the robot controller when forwarding trajectory, if false movej or movel commands are used"/>
 
   <!-- Load hardware interface -->
   <node name="ur_hardware_interface" pkg="ur_robot_driver" type="ur_robot_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="$(arg ur_hardware_interface_node_required)">
@@ -58,6 +59,7 @@
     <param name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
     <param name="servoj_gain" value="$(arg servoj_gain)"/>
     <param name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)"/>
+    <param name="use_spline_interpolation" value="$(arg use_spline_interpolation)"/>
   </node>
 
   <!-- Starts socat to bridge the robot's tool communication interface to a local tty device -->

ur_robot_driver/scripts/test_move

@@ -124,10 +124,15 @@ class TrajectoryClient:
         position_list = [[0, -1.57, -1.57, 0, 0, 0]]
         position_list.append([0.2, -1.57, -1.57, 0, 0, 0])
         position_list.append([-0.5, -1.57, -1.2, 0, 0, 0])
+
+        velocity_list = [[0.2, 0, 0, 0, 0, 0]]
+        velocity_list.append([-0.2, 0, 0, 0, 0, 0])
+        velocity_list.append([0, 0, 0, 0, 0, 0])
         duration_list = [3.0, 7.0, 10.0]
         for i, position in enumerate(position_list):
             point = JointTrajectoryPoint()
             point.positions = position
+            point.velocities = velocity_list[i]
             point.time_from_start = rospy.Duration(duration_list[i])
             goal.trajectory.points.append(point)
 

ur_robot_driver/src/hardware_interface.cpp

@@ -176,6 +176,10 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     return false;
   }
 
+  // True if splines should be used as interpolation on the robot controller when forwarding trajectory, if false movej
+  // or movel commands are used
+  use_spline_interpolation_ = robot_hw_nh.param<bool>("use_spline_interpolation", "true");
+
   // Whenever the runtime state of the "External Control" program node in the UR-program changes, a
   // message gets published here. So this is equivalent to the information whether the robot accepts
   // commands from ROS side.
@@ -451,6 +455,11 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // Setup the mounted payload through a ROS service
   set_payload_srv_ = robot_hw_nh.advertiseService("set_payload", &HardwareInterface::setPayload, this);
 
+  // Call this to activate or deactivate using spline interpolation locally on the UR controller, when forwarding
+  // trajectories to the UR robot.
+  activate_spline_interpolation_srv_ = robot_hw_nh.advertiseService(
+      "activate_spline_interpolation", &HardwareInterface::activateSplineInterpolation, this);
+
   ur_driver_->startRTDECommunication();
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");
 
@@ -1190,6 +1199,21 @@ void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
   }
 }
 
+bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res)
+{
+  use_spline_interpolation_ = req.data;
+  if (use_spline_interpolation_)
+  {
+    res.message = "Activated spline interpolation in forward trajectory mode.";
+  }
+  else
+  {
+    res.message = "Deactivated spline interpolation in forward trajectory mode.";
+  }
+  res.success = true;
+  return true;
+}
+
 void HardwareInterface::publishRobotAndSafetyMode()
 {
   if (robot_mode_pub_)
@@ -1255,7 +1279,46 @@ void HardwareInterface::startJointInterpolation(const hardware_interface::JointT
     p[4] = point.positions[4];
     p[5] = point.positions[5];
     double next_time = point.time_from_start.toSec();
-    ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time);
+    if (!use_spline_interpolation_)
+    {
+      ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time);
+    }
+    else  // Use spline interpolation
+    {
+      if (point.velocities.size() == 6 && point.accelerations.size() == 6)
+      {
+        urcl::vector6d_t v, a;
+        v[0] = point.velocities[0];
+        v[1] = point.velocities[1];
+        v[2] = point.velocities[2];
+        v[3] = point.velocities[3];
+        v[4] = point.velocities[4];
+        v[5] = point.velocities[5];
+
+        a[0] = point.accelerations[0];
+        a[1] = point.accelerations[1];
+        a[2] = point.accelerations[2];
+        a[3] = point.accelerations[3];
+        a[4] = point.accelerations[4];
+        a[5] = point.accelerations[5];
+        ur_driver_->writeTrajectorySplinePoint(p, v, a, next_time - last_time);
+      }
+      else if (point.velocities.size() == 6)
+      {
+        urcl::vector6d_t v;
+        v[0] = point.velocities[0];
+        v[1] = point.velocities[1];
+        v[2] = point.velocities[2];
+        v[3] = point.velocities[3];
+        v[4] = point.velocities[4];
+        v[5] = point.velocities[5];
+        ur_driver_->writeTrajectorySplinePoint(p, v, next_time - last_time);
+      }
+      else
+      {
+        ROS_ERROR_THROTTLE(1, "Spline interpolation using positions only is not supported.");
+      }
+    }
     last_time = next_time;
   }
   ROS_DEBUG("Finished Sending Trajectory");