diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 5fead319..d2d6fe19 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -91,6 +91,16 @@ class ScriptCommandInterface : public ReverseInterface */ bool setPayload(const double mass, const vector3d_t* cog); + /*! + * \brief Set the gravity vector + * + * \param gravity Gravity, a vector [x, y, z] specifying the gravity vector (pointing towards + * the Earth's center) given in the robot's base frame + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setGravity(const vector3d_t* gravity); + /*! * \brief Set the tool voltage. * @@ -227,6 +237,7 @@ class ScriptCommandInterface : public ReverseInterface END_TOOL_CONTACT = 6, ///< End detecting tool contact SET_FRICTION_COMPENSATION = 7, ///< Set friction compensation FT_RTDE_INPUT_ENABLE = 8, ///< Enable FT RTDE input + SET_GRAVITY = 9, ///< Set gravity vector }; /*! diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 5aaf5314..e61a8e07 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -596,6 +596,17 @@ class UrDriver */ bool setPayload(const float mass, const vector3d_t& cog); + /*! + * \brief Set the gravity vector. Note: It requires the external control script to be running or + * the robot to be in headless mode. + * + * \param gravity Gravity, a vector [x, y, z] specifying the gravity vector (pointing towards + * the Earth's center) given in the robot's base frame + * + * \returns True on successful write. + */ + bool setGravity(const vector3d_t& gravity); + /*! * \brief Set the tool voltage. 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 301781a4..c4ef62c2 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -56,6 +56,7 @@ START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 SET_FRICTION_COMPENSATION = 7 FT_RTDE_INPUT_ENABLE = 8 +SET_GRAVITY = 9 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -756,6 +757,9 @@ thread script_commands(): mass = raw_command[2] / MULT_jointstate cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate] set_payload(mass, cog) + elif command == SET_GRAVITY: + gravity = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate] + set_gravity(gravity) elif command == SET_TOOL_VOLTAGE: tool_voltage = raw_command[2] / MULT_jointstate set_tool_voltage(tool_voltage) diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 01342c29..ff3521a2 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -89,6 +89,31 @@ bool ScriptCommandInterface::setPayload(const double mass, const vector3d_t* cog return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setGravity(const vector3d_t* gravity) +{ + const int message_length = 4; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_GRAVITY)); + b_pos += append(b_pos, val); + + for (auto const& grav_component : *gravity) + { + val = htobe32(static_cast(round(grav_component * 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::setToolVoltage(const ToolVoltage voltage) { const int message_length = 2; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e1dd6e0b..3f5271f5 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -298,6 +298,26 @@ bool UrDriver::setPayload(const float mass, const vector3d_t& cog) } } +bool UrDriver::setGravity(const vector3d_t& gravity) +{ + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setGravity(&gravity); + } + 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_gravity([" << gravity[0] << ", " << gravity[1] << ", " << gravity[2] << "])" << std::endl + << "end"; + return sendScript(cmd.str()); + return true; + } +} + bool UrDriver::setToolVoltage(const ToolVoltage voltage) { // Test that the tool voltage is either 0, 12 or 24.