diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index d86b3817c..4527f0d33 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -25,6 +25,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun - ``setGravity()``: Set the gravity vector for the robot. - ``setTcpOffset()``: Set the TCP offset of the robot. - ``setFrictionScales()``: Set viscous and Coulomb friction scale factors for direct torque control. +- ``setTargetPayload()``: Set the active payload mass, center of gravity, inertia matrix and transition time. Communication protocol ---------------------- @@ -58,6 +59,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 9: setGravity - 10: setTcpOffset - 11: setFrictionScales + - 12: setTargetPayload 1-27 data fields specific to the command ===== ===== @@ -77,7 +79,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr index meaning ===== ===== 1 Payload mass in kg (floating point) - 2-4 Payload center of gravity in m, displacement from the toolmpount (floating point) + 2-4 Payload center of gravity in m, displacement from the tool mount (floating point) ===== ===== .. table:: With setToolVoltage command @@ -180,6 +182,18 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 7-12 Coulomb friction scale factors. One number per joint, range [0-1]. 0 means no compensation for that joint. 1 means full compensation. Default is [0.8, 0.8, 0.7, 0.8, 0.8, 0.8]. (floating point) ===== ===== +.. table:: With setTargetPayload command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1 Payload mass in kg (floating point) + 2-4 Payload center of gravity in m, displacement from the tool mount (floating point) + 5-10 Payload inertia matrix [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] in kg·m² (floating point) + 11 Payload transition time in seconds (floating point) + ===== ===== + .. note:: In URScript the ``socket_read_binary_integer()`` function is used to read the data from the script command socket. The first index in that function's return value is the number of integers read, @@ -188,9 +202,9 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr function. E.g. reading the boolean value for friction compensation in the ``setFrictionCompensation`` command would be done by accessing index 2 of the result array. - All floating point data is encoded into an integer representation and has to be divided by the - ``MULT_JOINTSTATE`` constant to get the actual floating point value. This constant is defined in - ``ReverseInterface`` class. + All floating point data is encoded into an integer representation and has to be divided by either the + ``MULT_JOINTSTATE`` or ``MULT_TIME`` constant to get the actual floating point value. These constants + are defined in the ``ReverseInterface`` class. Data sent from the robot ^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 8ea7e76b7..60580aaf6 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -80,6 +80,9 @@ void sendScriptCommands() run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); }); run_cmd("Setting TCP offset to [0.0, 0.0, 0.10, 0.0, 0.0, 0.0]", []() { g_my_robot->getUrDriver()->setTcpOffset({ 0.0, 0.0, 0.10, 0.0, 0.0, 0.0 }); }); + run_cmd("Setting payload to 2.0kg with custom inertia and 0.5s transition time", []() { + g_my_robot->getUrDriver()->setTargetPayload(2.0, { 0.0, 0.0, 0.1 }, { 0.003, 0.003, 0.003, 0.0, 0.0, 0.0 }, 0.5); + }); if (g_support_set_friction_scales) { diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index c0bef8e34..18b7b4799 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -82,6 +82,7 @@ class ReverseInterface { public: static const int32_t MULT_JOINTSTATE = 1000000; + static const int32_t MULT_TIME = 1000000; ReverseInterface() = delete; /*! diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 92aa4cd9c..d314fe28c 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -91,6 +91,20 @@ class ScriptCommandInterface : public ReverseInterface */ bool setPayload(const double mass, const vector3d_t* cog); + /*! + * \brief Set the target payload mass, center of gravity and inertia matrix + * + * \param mass mass in kilograms + * \param cog Center of Gravity, a vector [CoGx, CoGy, CoGz] specifying the displacement (in meters) from the + * toolmount + * \param inertia Inertia matrix elements [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] + * \param transition_time Duration of the payload property changes in seconds + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setTargetPayload(const double mass, const vector3d_t* cog, const vector6d_t* inertia, + const double transition_time = 0.0); + /*! * \brief Set the gravity vector * @@ -264,6 +278,7 @@ class ScriptCommandInterface : public ReverseInterface SET_GRAVITY = 9, ///< Set gravity vector SET_TCP_OFFSET = 10, ///< Set TCP offset SET_FRICTION_SCALES = 11, ///< Set viscous and Coulomb friction scales for direct_torque + SET_TARGET_PAYLOAD = 12, ///< Set target payload }; /*! diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 5b5f76fd9..26de4d89f 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -63,7 +63,6 @@ std::string trajectoryResultToString(const TrajectoryResult result); class TrajectoryPointInterface : public ReverseInterface { public: - static const int32_t MULT_TIME = 1000000; static const int MESSAGE_LENGTH = 21; TrajectoryPointInterface() = delete; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 9098a5e5d..9ad65933c 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -631,6 +631,21 @@ class UrDriver */ bool setPayload(const float mass, const vector3d_t& cog); + /*! + * \brief Set the target payload mass, center of gravity and inertia matrix + * + * \param mass mass in kilograms + * \param cog Center of Gravity, a vector [CoGx, CoGy, CoGz] specifying the displacement (in meters) from the + * toolmount + * \param inertia Inertia matrix elements [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] + * \param transition_time Duration of the payload property changes in seconds + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setTargetPayload(const float mass, const vector3d_t& cog, + const vector6d_t& inertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, + const double transition_time = 0.0); + /*! * \brief Set the gravity vector. Note: It requires the external control script to be running or * the robot to be in headless mode. diff --git a/resources/external_control.urscript b/resources/external_control.urscript index dc2c6fe69..3986f20f2 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -67,6 +67,7 @@ FT_RTDE_INPUT_ENABLE = 8 SET_GRAVITY = 9 SET_TCP_OFFSET = 10 SET_FRICTION_SCALES = 11 +SET_TARGET_PAYLOAD = 12 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -938,6 +939,17 @@ thread script_commands(): friction_compensation_mode = FRICTION_COMP_MODE_FRICTION_SCALES viscous_scale = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] coulomb_scale = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] + elif command == SET_TARGET_PAYLOAD: + mass = raw_command[2] / MULT_jointstate + cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate] + inertia = [raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate, raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate] + transition_time = raw_command[12] / MULT_time + {% if ROBOT_SOFTWARE_VERSION >= v5.10.0 %} + set_target_payload(mass, cog, inertia, transition_time) + {% else %} + set_payload(mass, cog) + textmsg("PolyScope < 5.10.0. Inertia and transition_time ignored.") + {% endif %} elif command == FT_RTDE_INPUT_ENABLE: if raw_command[2] == 0: enabled = False diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 648e6ee62..600839848 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -91,6 +91,44 @@ bool ScriptCommandInterface::setPayload(const double mass, const vector3d_t* cog return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setTargetPayload(const double mass, const vector3d_t* cog, const vector6d_t* inertia, + const double transition_time) +{ + const int message_length = 12; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_TARGET_PAYLOAD)); + b_pos += append(b_pos, val); + + val = htobe32(static_cast(round(mass * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + + for (auto const& center_of_mass : *cog) + { + val = htobe32(static_cast(round(center_of_mass * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + for (auto const& inertia_val : *inertia) + { + val = htobe32(static_cast(round(inertia_val * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + val = htobe32(static_cast(round(transition_time * MULT_TIME))); + b_pos += append(b_pos, val); + + // writing zeros to allow usage with other script commands + for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++) + { + val = htobe32(0); + b_pos += append(b_pos, val); + } + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + bool ScriptCommandInterface::setGravity(const vector3d_t* gravity) { const int message_length = 4; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 880051339..69352ac50 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -313,6 +313,41 @@ bool UrDriver::setPayload(const float mass, const vector3d_t& cog) } } +bool UrDriver::setTargetPayload(const float mass, const vector3d_t& cog, const vector6d_t& inertia, + const double transition_time) +{ + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setTargetPayload(mass, &cog, &inertia, transition_time); + } + else + { + URCL_LOG_WARN("Script command interface is not running. Falling back to sending plain script code. On e-Series " + "robots this will only work, if the robot is in remote_control mode."); + + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + + if ((getVersion().major > 5) || (getVersion().major == 5 && getVersion().minor >= 10)) + { + cmd << "sec setup():" << std::endl + << " set_target_payload(" << mass << ", [" << cog[0] << ", " << cog[1] << ", " << cog[2] << "] , [" + << inertia[0] << ", " << inertia[1] << ", " << inertia[2] << ", " << inertia[3] << ", " << inertia[4] << ", " + << inertia[5] << "] , " << transition_time << ")" << std::endl + << "end"; + } + else + { + cmd << "sec setup():" << std::endl + << " set_payload(" << mass << ", [" << cog[0] << ", " << cog[1] << ", " << cog[2] << "])" << std::endl + << " textmsg(\"PolyScope < 5.10.0. Inertia and transition_time ignored.\")" << std::endl + << "end"; + } + + return sendScript(cmd.str()); + } +} + bool UrDriver::setGravity(const vector3d_t& gravity) { if (script_command_interface_->clientConnected()) diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 68d143dd6..cf1cd0add 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -225,6 +225,54 @@ TEST_F(ScriptCommandInterfaceTest, test_set_payload) EXPECT_EQ(message_sum, expected_message_sum); } +TEST_F(ScriptCommandInterfaceTest, test_set_target_payload) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + double mass = 1.0; + vector3d_t cog = { 0.2, 0.3, 0.1 }; + vector6d_t inertia = { 0.4, 0.7, 0.8, 0.2, 0.5, 0.6 }; + double transition_time = 0.002; + script_command_interface_->setTargetPayload(mass, &cog, &inertia, transition_time); + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 12 is set target payload + int32_t expected_command = 12; + EXPECT_EQ(command, expected_command); + + // Test mass + double received_mass = (double)message[0] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_mass, mass); + + // Test cog + vector3d_t received_cog; + for (unsigned int i = 0; i < cog.size(); ++i) + { + received_cog[i] = (double)message[i + 1] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_cog[i], cog[i]); + } + + // Test inertia + vector6d_t received_inertia; + for (unsigned int i = 0; i < inertia.size(); ++i) + { + received_inertia[i] = (double)message[i + 4] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_inertia[i], inertia[i]); + } + + // Test transition time + double received_transition_time = (double)message[10] / script_command_interface_->MULT_TIME; + EXPECT_EQ(received_transition_time, transition_time); + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + 11, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + TEST_F(ScriptCommandInterfaceTest, test_set_tool_voltage) { // Wait for the client to connect to the server diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index f6e299134..fd6334781 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -360,6 +360,9 @@ TEST_F(UrDriverTest, set_tcp_offset) EXPECT_DOUBLE_EQ(tcp_offset_received[i], tcp_offset[i]); } + // reset the tcp offset to avoid affecting other tests + ASSERT_TRUE(g_my_robot->getUrDriver()->setTcpOffset({ 0, 0, 0, 0, 0, 0 })); + // Stop program on robot g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); @@ -374,6 +377,36 @@ TEST_F(UrDriverTest, set_tcp_offset) ASSERT_FALSE(g_my_robot->getUrDriver()->setTcpOffset(tcp_offset)); } +TEST_F(UrDriverTest, set_target_payload_fallback_script) +{ + g_my_robot->getUrDriver()->stopControl(); + ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000)); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + double mass = 1.0; + vector3d_t cog = { 0.2, 0.3, 0.1 }; + vector6d_t inertia = { 0.4, 0.7, 0.8, 0.2, 0.5, 0.6 }; + double transition_time = 0.002; + + EXPECT_TRUE(g_my_robot->getUrDriver()->setTargetPayload(mass, cog, inertia, transition_time)); + + // restore empty payload + EXPECT_TRUE(g_my_robot->getUrDriver()->setTargetPayload(0, { 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, 0.002)); +} + +TEST_F(UrDriverTest, set_target_payload) +{ + double mass = 1.0; + vector3d_t cog = { 0.2, 0.3, 0.1 }; + vector6d_t inertia = { 0.4, 0.7, 0.8, 0.2, 0.5, 0.6 }; + double transition_time = 0.002; + + EXPECT_TRUE(g_my_robot->getUrDriver()->setTargetPayload(mass, cog, inertia, transition_time)); + + // restore empty payload + EXPECT_TRUE(g_my_robot->getUrDriver()->setTargetPayload(0, { 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, 0.002)); +} + TEST(UrDriverInitTest, setting_connection_limits_works_correctly) { UrDriverConfiguration config;