From c10624426f3fbc55986add95bc41f509ef4650bd Mon Sep 17 00:00:00 2001
From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com>
Date: Wed, 30 Nov 2022 13:17:58 +0100
Subject: [PATCH] =?UTF-8?q?Updated=20set=20payload,=20zero=20ftsensor=20an?=
 =?UTF-8?q?d=20set=20tool=20voltage=20to=20use=20the=20ur=E2=80=A6=20(#567?=
 =?UTF-8?q?)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Updated set payload, zero ftsensor and set tool voltage to use the urdriver

This makes it possible to call the commands when the robot is in local control if the external control script is running on the robot.

* Update ROS interface docs

* Fix argument passing in include instruction

Co-authored-by: Miguel Prada <miguel.prada@tecnalia.com>
---
 ur_robot_driver/doc/ROS_INTERFACE.md          | 36 +++++++++++++++
 .../ur_robot_driver/hardware_interface.h      |  2 +
 ur_robot_driver/launch/ur10_bringup.launch    |  1 +
 ur_robot_driver/launch/ur10e_bringup.launch   |  1 +
 ur_robot_driver/launch/ur16e_bringup.launch   |  1 +
 ur_robot_driver/launch/ur3_bringup.launch     |  1 +
 ur_robot_driver/launch/ur3e_bringup.launch    |  1 +
 ur_robot_driver/launch/ur5_bringup.launch     |  1 +
 ur_robot_driver/launch/ur5e_bringup.launch    |  1 +
 ur_robot_driver/launch/ur_common.launch       |  2 +
 ur_robot_driver/launch/ur_control.launch      |  2 +
 ur_robot_driver/src/hardware_interface.cpp    | 44 ++++++++++---------
 12 files changed, 72 insertions(+), 21 deletions(-)

diff --git a/ur_robot_driver/doc/ROS_INTERFACE.md b/ur_robot_driver/doc/ROS_INTERFACE.md
index 8c84b90b5..aee071aa7 100644
--- a/ur_robot_driver/doc/ROS_INTERFACE.md
+++ b/ur_robot_driver/doc/ROS_INTERFACE.md
@@ -49,6 +49,10 @@ Robot description launch file.
 
 IP address by which the robot can be reached.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -150,6 +154,10 @@ Robot description launch file.
 
 IP address by which the robot can be reached.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -211,6 +219,10 @@ Recipe file used for the RTDE-inputs. Only change this if you know what you're d
 
 Recipe file used for the RTDE-outputs. Only change this if you know what you're doing.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -308,6 +320,10 @@ Robot description launch file.
 
 IP address by which the robot can be reached.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -401,6 +417,10 @@ Robot description launch file.
 
 IP address by which the robot can be reached.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -458,6 +478,10 @@ Robot description launch file.
 
 IP address by which the robot can be reached.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -551,6 +575,10 @@ Robot description launch file.
 
 IP address by which the robot can be reached.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -608,6 +636,10 @@ Robot description launch file.
 
 IP address by which the robot can be reached.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (default: "50002")
 
 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.
@@ -834,6 +866,10 @@ The robot's IP address.
 
 Path to the urscript code that will be sent to the robot.
 
+##### script_command_port (default: "50004")
+
+Port that will be opened to forward script commands to the robot when in local control mode.
+
 ##### script_sender_port (Required)
 
 The driver will offer an interface to receive the program's URScript on this port.
diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h
index 7a8e3deb6..8fe5cddad 100644
--- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h
+++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h
@@ -52,6 +52,7 @@
 #include <ur_msgs/ToolDataMsg.h>
 #include <ur_msgs/SetIO.h>
 #include <ur_msgs/SetSpeedSliderFraction.h>
+#include <ur_msgs/SetPayload.h>
 
 #include <cartesian_interface/cartesian_command_interface.h>
 #include <cartesian_interface/cartesian_state_handle.h>
@@ -213,6 +214,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
   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);
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> dashboard_client_;
diff --git a/ur_robot_driver/launch/ur10_bringup.launch b/ur_robot_driver/launch/ur10_bringup.launch
index f0fa0ba2d..8686e418c 100644
--- a/ur_robot_driver/launch/ur10_bringup.launch
+++ b/ur_robot_driver/launch/ur10_bringup.launch
@@ -6,6 +6,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
diff --git a/ur_robot_driver/launch/ur10e_bringup.launch b/ur_robot_driver/launch/ur10e_bringup.launch
index cdf2e25f6..2b98f6407 100644
--- a/ur_robot_driver/launch/ur10e_bringup.launch
+++ b/ur_robot_driver/launch/ur10e_bringup.launch
@@ -7,6 +7,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
diff --git a/ur_robot_driver/launch/ur16e_bringup.launch b/ur_robot_driver/launch/ur16e_bringup.launch
index a04738ab1..40b6a7bf8 100644
--- a/ur_robot_driver/launch/ur16e_bringup.launch
+++ b/ur_robot_driver/launch/ur16e_bringup.launch
@@ -7,6 +7,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
diff --git a/ur_robot_driver/launch/ur3_bringup.launch b/ur_robot_driver/launch/ur3_bringup.launch
index 53b60a43a..764b36634 100644
--- a/ur_robot_driver/launch/ur3_bringup.launch
+++ b/ur_robot_driver/launch/ur3_bringup.launch
@@ -6,6 +6,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
diff --git a/ur_robot_driver/launch/ur3e_bringup.launch b/ur_robot_driver/launch/ur3e_bringup.launch
index ab5d8eba6..bbf4ff696 100644
--- a/ur_robot_driver/launch/ur3e_bringup.launch
+++ b/ur_robot_driver/launch/ur3e_bringup.launch
@@ -6,6 +6,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
diff --git a/ur_robot_driver/launch/ur5_bringup.launch b/ur_robot_driver/launch/ur5_bringup.launch
index 86f8b4106..619118c2a 100644
--- a/ur_robot_driver/launch/ur5_bringup.launch
+++ b/ur_robot_driver/launch/ur5_bringup.launch
@@ -6,6 +6,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
diff --git a/ur_robot_driver/launch/ur5e_bringup.launch b/ur_robot_driver/launch/ur5e_bringup.launch
index dd7eb9682..10a03b253 100644
--- a/ur_robot_driver/launch/ur5e_bringup.launch
+++ b/ur_robot_driver/launch/ur5e_bringup.launch
@@ -6,6 +6,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
diff --git a/ur_robot_driver/launch/ur_common.launch b/ur_robot_driver/launch/ur_common.launch
index 577a08415..1699ec2b2 100644
--- a/ur_robot_driver/launch/ur_common.launch
+++ b/ur_robot_driver/launch/ur_common.launch
@@ -8,6 +8,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="kinematics_config" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller robot_status_controller" doc="Controllers that are activated by default."/>
@@ -41,6 +42,7 @@
     <arg name="reverse_port" value="$(arg reverse_port)"/>
     <arg name="script_sender_port" value="$(arg script_sender_port)"/>
     <arg name="trajectory_port" value="$(arg trajectory_port)"/>
+    <arg name="script_command_port" value="$(arg script_command_port)"/>
     <arg name="kinematics_config" value="$(arg kinematics_config)"/>
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
diff --git a/ur_robot_driver/launch/ur_control.launch b/ur_robot_driver/launch/ur_control.launch
index 39927d905..4d842e2e5 100644
--- a/ur_robot_driver/launch/ur_control.launch
+++ b/ur_robot_driver/launch/ur_control.launch
@@ -13,6 +13,7 @@
   <arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
   <arg name="script_sender_port" default="50002" doc="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."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
+  <arg name="script_command_port" default="50004" doc="Port that will be opened by the driver to allow forwarding script commands to the robot."/>
   <arg name="kinematics_config" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description. Pass the same config file that is passed to the robot_description."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller force_torque_sensor_controller robot_status_controller"/>
@@ -40,6 +41,7 @@
     <param name="reverse_port" type="int" value="$(arg reverse_port)"/>
     <param name="script_sender_port" type="int" value="$(arg script_sender_port)"/>
     <param name="trajectory_port" value="$(arg trajectory_port)"/>
+    <param name="script_command_port" value="$(arg script_command_port)"/>
     <rosparam command="load" file="$(arg kinematics_config)" />
     <param name="script_file" value="$(arg urscript_file)"/>
     <param name="output_recipe_file" value="$(arg rtde_output_recipe_file)"/>
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
index 3491644e6..b4672ec47 100644
--- a/ur_robot_driver/src/hardware_interface.cpp
+++ b/ur_robot_driver/src/hardware_interface.cpp
@@ -31,7 +31,6 @@
 #include <ur_client_library/ur/tool_communication.h>
 #include <ur_client_library/exceptions.h>
 
-#include <ur_msgs/SetPayload.h>
 #include <trajectory_msgs/JointTrajectoryPoint.h>
 #include <trajectory_msgs/JointTrajectory.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
@@ -110,6 +109,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // Port that will be opened to send trajectory points from the driver to the robot
   int trajectory_port = robot_hw_nh.param("trajectory_port", 50003);
 
+  // Port that will be opened to forward script commands from the driver to the robot
+  int script_command_port = robot_hw_nh.param("script_command_port", 50004);
+
   // When the robot's URDF is being loaded with a prefix, we need to know it here, as well, in order
   // to publish correct frame names for frames reported by the robot directly.
   robot_hw_nh.param<std::string>("tf_prefix", tf_prefix_, "");
@@ -285,7 +287,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
         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), (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
-        servoj_lookahead_time, non_blocking_read_, reverse_ip, trajectory_port));
+        servoj_lookahead_time, non_blocking_read_, reverse_ip, trajectory_port, script_command_port));
   }
   catch (urcl::ToolCommNotAvailable& e)
   {
@@ -440,26 +442,15 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // Calling this service will make the "External Control" program node on the UR-Program return.
   deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
 
-  // Calling this service will zero the robot's ftsensor. Note: On e-Series robots this will only
-  // work when the robot is in remote-control mode.
+  // Calling this service will zero the robot's ftsensor (only available for e-Series).
   tare_sensor_srv_ = robot_hw_nh.advertiseService("zero_ftsensor", &HardwareInterface::zeroFTSensor, this);
 
+  // Setup the mounted payload through a ROS service
+  set_payload_srv_ = robot_hw_nh.advertiseService("set_payload", &HardwareInterface::setPayload, this);
+
   ur_driver_->startRTDECommunication();
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");
 
-  // Setup the mounted payload through a ROS service
-  set_payload_srv_ = robot_hw_nh.advertiseService<ur_msgs::SetPayload::Request, ur_msgs::SetPayload::Response>(
-      "set_payload", [&](ur_msgs::SetPayload::Request& req, ur_msgs::SetPayload::Response& resp) {
-        std::stringstream cmd;
-        cmd.imbue(std::locale::classic());  // Make sure, decimal divider is actually '.'
-        cmd << "sec setup():" << std::endl
-            << " set_payload(" << req.mass << ", [" << req.center_of_gravity.x << ", " << req.center_of_gravity.y
-            << ", " << req.center_of_gravity.z << "])" << std::endl
-            << "end";
-        resp.success = this->ur_driver_->sendScript(cmd.str());
-        return true;
-      });
-
   return true;
 }
 
@@ -1095,6 +1086,10 @@ bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse
   {
     res.success = ur_driver_->getRTDEWriter().sendStandardAnalogOutput(req.pin, req.state);
   }
+  else if (req.fun == req.FUN_SET_TOOL_VOLTAGE && ur_driver_ != nullptr)
+  {
+    res.success = ur_driver_->setToolVoltage(static_cast<urcl::ToolVoltage>(req.state));
+  }
   else
   {
     ROS_ERROR("Cannot execute function %u. This is not (yet) supported.", req.fun);
@@ -1133,14 +1128,21 @@ bool HardwareInterface::zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::Tr
   }
   else
   {
-    res.success = this->ur_driver_->sendScript(R"(sec tareSensor():
-  zero_ftsensor()
-end
-)");
+    res.success = this->ur_driver_->zeroFTSensor();
   }
   return true;
 }
 
+bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res)
+{
+  urcl::vector3d_t cog;
+  cog[0] = req.center_of_gravity.x;
+  cog[1] = req.center_of_gravity.y;
+  cog[2] = req.center_of_gravity.z;
+  res.success = this->ur_driver_->setPayload(req.mass, cog);
+  return true;
+}
+
 void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
 {
   std::string str = msg->data;