diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 92aa4cd9..d314fe28 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/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 9098a5e5..2219bbce 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -631,6 +631,20 @@ 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, + 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 dc2c6fe6..6bb2f545 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,12 @@ 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_jointstate + set_target_payload(mass, cog, inertia, transition_time) 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 648e6ee6..19bb2605 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_JOINTSTATE))); + 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 88005133..1e1b97f1 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -313,6 +313,28 @@ 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 '.' + 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"; + 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 68d143dd..9cbdf812 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_JOINTSTATE; + 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