Parameterising gains (#88)

* added parameters for servoj_gain and servoj_lookahead_time

* changing to ros_error_stream

* lint

* added documentation

ur_robot_driver/doc/ROS_INTERFACE.md

@@ -818,6 +818,14 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
 
     Should the tool's RS485 interface be forwarded to the ROS machine? This is only available on e-Series models. Setting this parameter to TRUE requires multiple other parameters to be set,as well.
 
+ * "**servoj_gain**" (default: "2000")
+
+    Specify gain for servoing to position in joint space. A higher gain can sharpen the trajectory.
+
+ * "**servoj_gain**" (default: "0.03")
+
+    Specify lookahead time for servoing to position in joint space. A longer lookahead time can smooth the trajectory.
+
 #### Published topics
  * "**robot_program_running**" ([std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html))
 

ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h

@@ -62,11 +62,14 @@ public:
    * and the robot controller.
    * \param script_sending_port The driver will offer an interface to receive the program's URScript on this port. If
    * the robot cannot connect to this port, `External Control` will stop immediately.
+   * \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
+   * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
            const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "",
-           const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002);
+           const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000,
+           double servoj_lookahead_time = 0.03);
   /*!
    * \brief Constructs a new UrDriver object.
    *
@@ -82,13 +85,16 @@ public:
    * and the robot controller
    * \param script_sending_port The driver will offer an interface to receive the program's URScript on this port.
    * If the robot cannot connect to this port, `External Control` will stop immediately.
+   * \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
+   * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
            const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001,
-           const uint32_t script_sender_port = 50002)
+           const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03)
     : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
-               std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port)
+               std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain,
+               servoj_lookahead_time)
   {
   }
 

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -104,6 +104,24 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     return false;
   }
 
+  // Specify gain for servoing to position in joint space.
+  // A higher gain can sharpen the trajectory.
+  int servoj_gain = robot_hw_nh.param("servoj_gain", 2000);
+  if ((servoj_gain > 2000) || (servoj_gain < 100))
+  {
+    ROS_ERROR_STREAM("servoj_gain is " << servoj_gain << ", must be in range [100, 2000]");
+    return false;
+  }
+
+  // Specify lookahead time for servoing to position in joint space.
+  // A longer lookahead time can smooth the trajectory.
+  double servoj_lookahead_time = robot_hw_nh.param("servoj_lookahead_time", 0.03);
+  if ((servoj_lookahead_time > 0.2) || (servoj_lookahead_time < 0.03))
+  {
+    ROS_ERROR_STREAM("servoj_lookahead_time is " << servoj_lookahead_time << ", must be in range [0.03, 0.2]");
+    return false;
+  }
+
   // 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.
@@ -214,7 +232,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     ur_driver_.reset(new UrDriver(robot_ip_, script_filename, output_recipe_filename, input_recipe_filename,
                                   std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
                                   headless_mode, std::move(tool_comm_setup), calibration_checksum,
-                                  (uint32_t)reverse_port, (uint32_t)script_sender_port));
+                                  (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
+                                  servoj_lookahead_time));
   }
   catch (ur_driver::ToolCommNotAvailable& e)
   {

ur_robot_driver/src/ur/ur_driver.cpp

@@ -51,10 +51,11 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
                               const std::string& output_recipe_file, const std::string& input_recipe_file,
                               std::function<void(bool)> handle_program_state, bool headless_mode,
                               std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
-                              const uint32_t reverse_port, const uint32_t script_sender_port)
+                              const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain,
+                              double servoj_lookahead_time)
   : servoj_time_(0.008)
-  , servoj_gain_(2000)
-  , servoj_lookahead_time_(0.03)
+  , servoj_gain_(servoj_gain)
+  , servoj_lookahead_time_(servoj_lookahead_time)
   , reverse_interface_active_(false)
   , reverse_port_(reverse_port)
   , handle_program_state_(handle_program_state)