diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 39541301..e68cd492 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -59,3 +59,8 @@ add_executable(script_sender_example script_sender.cpp) target_compile_options(script_sender_example PUBLIC ${CXX17_FLAG}) target_link_libraries(script_sender_example ur_client_library::urcl) + +add_executable(trajectory_point_interface_example +trajectory_point_interface.cpp) +target_compile_options(trajectory_point_interface_example PUBLIC ${CXX17_FLAG}) +target_link_libraries(trajectory_point_interface_example ur_client_library::urcl) diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp new file mode 100644 index 00000000..5a7173b2 --- /dev/null +++ b/examples/trajectory_point_interface.cpp @@ -0,0 +1,213 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2024 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +#include "ur_client_library/types.h" +#include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/log.h" +#include "ur_client_library/control/trajectory_point_interface.h" +#include "ur_client_library/ur/dashboard_client.h" + +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; +const std::string SCRIPT_FILE = "resources/external_control.urscript"; +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; + +std::unique_ptr g_my_dashboard; +std::unique_ptr g_my_driver; + +std::atomic g_trajectory_done = false; + +// We need a callback function to register. See UrDriver's parameters for details. +void handleRobotProgramState(bool program_running) +{ + // Print the text in green so we see it better + std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; +} + +void trajDoneCallback(const urcl::control::TrajectoryResult& result) +{ + URCL_LOG_INFO("Trajectory done with result %d", result); + ; + g_trajectory_done = true; +} + +int main(int argc, char* argv[]) +{ + urcl::setLogLevel(urcl::LogLevel::INFO); + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + // --------------- INITIALIZATION BEGIN ------------------- + // Making the robot ready for the program by: + // Connect the robot Dashboard + g_my_dashboard.reset(new urcl::DashboardClient(robot_ip)); + if (!g_my_dashboard->connect()) + { + URCL_LOG_ERROR("Could not connect to dashboard"); + return 1; + } + + // // Stop program, if there is one running + if (!g_my_dashboard->commandStop()) + { + URCL_LOG_ERROR("Could not send stop program command"); + return 1; + } + + // Power it off + if (!g_my_dashboard->commandPowerOff()) + { + URCL_LOG_ERROR("Could not send Power off command"); + return 1; + } + + // Power it on + if (!g_my_dashboard->commandPowerOn()) + { + URCL_LOG_ERROR("Could not send Power on command"); + return 1; + } + + // Release the brakes + if (!g_my_dashboard->commandBrakeRelease()) + { + URCL_LOG_ERROR("Could not send BrakeRelease command"); + return 1; + } + + std::unique_ptr tool_comm_setup; + const bool headless = true; + g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, + headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + // --------------- INITIALIZATION END ------------------- + + g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback); + + URCL_LOG_INFO("Running MoveJ motion"); + // --------------- MOVEJ TRAJECTORY ------------------- + { + g_trajectory_done = false; + // Trajectory definition + std::vector points{ { -1.57, -1.57, 0, 0, 0, 0 }, { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } }; + std::vector motion_durations{ 5.0, 2.0 }; + std::vector blend_radii{ 0.1, 0.1 }; + + // Trajectory execution + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + points.size() * 2); + for (size_t i = 0; i < points.size(); i++) + { + g_my_driver->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]); + } + + // Same motion, but parametrized with acceleration and velocity + motion_durations = { 0.0, 0.0 }; + for (size_t i = 0; i < points.size(); i++) + { + g_my_driver->writeTrajectoryPoint(points[i], 2.0, 2.0, false, motion_durations[i], blend_radii[i]); + } + + while (!g_trajectory_done) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + } + } + // --------------- END MOVEJ TRAJECTORY ------------------- + + URCL_LOG_INFO("Running MoveL motion"); + // --------------- MOVEL TRAJECTORY ------------------- + { + g_trajectory_done = false; + // Trajectory definition + std::vector points{ { -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, + { -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 } }; + std::vector motion_durations{ 5.0, 5.0 }; + std::vector blend_radii{ 0.0, 0.0 }; + + // Trajectory execution + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + points.size()); + for (size_t i = 0; i < points.size(); i++) + { + // setting the cartesian parameter makes it interpret the 6d vector as a pose and use movel + g_my_driver->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]); + } + + while (!g_trajectory_done) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + } + } + // --------------- END MOVEL TRAJECTORY ------------------- + + URCL_LOG_INFO("Running a spline motion"); + // --------------- SPLINE TRAJECTORY ------------------- + { + g_trajectory_done = false; + // Trajectory definition + std::vector positions{ { -1.57, -1.57, 0, 0, 0, 0 }, + { -1.57, -1.57, -1.57, 0, 0, 0 }, + { -1.57, -1.57, 0, 0, 0, 0 }, + { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } }; + std::vector velocities{ + { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 1.5, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 } + }; + std::vector motion_durations{ 3.0, 3.0, 3.0, 3.0 }; + + // Trajectory execution + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + positions.size()); + for (size_t i = 0; i < positions.size(); i++) + { + g_my_driver->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]); + } + + while (!g_trajectory_done) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + } + } + // --------------- END MOVEJ TRAJECTORY ------------------- + + g_my_driver->stopControl(); + return 0; +} diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index e64e0035..a7768a4a 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -29,6 +29,8 @@ #ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED #define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED +#include + #include "ur_client_library/control/reverse_interface.h" #include "ur_client_library/comm/control_mode.h" #include "ur_client_library/types.h" @@ -44,6 +46,7 @@ namespace control enum class TrajectoryResult : int32_t { + TRAJECTORY_RESULT_UNKNOWN = -1, ///< No result received, yet TRAJECTORY_RESULT_SUCCESS = 0, ///< Successful execution TRAJECTORY_RESULT_CANCELED = 1, ///< Canceled by user TRAJECTORY_RESULT_FAILURE = 2 ///< Aborted due to error during execution @@ -76,6 +79,7 @@ class TrajectoryPointInterface : public ReverseInterface { public: static const int32_t MULT_TIME = 1000; + static const int MESSAGE_LENGTH = 21; TrajectoryPointInterface() = delete; /*! @@ -103,6 +107,23 @@ class TrajectoryPointInterface : public ReverseInterface bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius, const bool cartesian); + /*! + * \brief Writes information for a robot motion to the robot to be read by the URScript program. + * + * \param positions A vector of joint or cartesian targets for the robot + * \param acceleration Joint acceleration of leading axis [rad/s^2] / tool acceleration [m/s^2] + * for Cartesian motions + * \param velocity Joint speed of leading axis [rad/s] / tool speed [m/s] for Cartesian motions + * \param goal_time The goal time to reach the target. When a non-zero goal time is specified, + * this has priority over speed and acceleration settings. + * \param blend_radius The radius to be used for blending between control points + * \param cartesian True, if the written point is specified in Cartesian space, false if in joint space + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool writeTrajectoryPoint(const vector6d_t* positions, const float acceleration = 1.4, const float velocity = 1.05, + const float goal_time = 0, const float blend_radius = 0, const bool cartesian = false); + /*! * \brief Writes needed information to the robot to be read by the URScript program including * velocity and acceleration information. Depending on the information given the robot will do quadratic (positions @@ -131,7 +152,6 @@ class TrajectoryPointInterface : public ReverseInterface virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) override; private: - static const int MESSAGE_LENGTH = 21; std::function handle_trajectory_end_; }; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 358e1b58..45d7dd2f 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -272,6 +272,23 @@ class UrDriver bool writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time = 0.0, const float blend_radius = 0.052); + /*! + * \brief Writes a trajectory point onto the dedicated socket. + * + * \param positions Desired joint or cartesian positions + * \param cartesian True, if the point sent is cartesian, false if joint-based + * \param acceleration Joint acceleration of leading axis [rad/s^2] / tool acceleration [m/s^2] + * for Cartesian motions + * \param velocity Joint speed of leading axis [rad/s] / tool speed [m/s] for Cartesian motions + * \param goal_time Time for the robot to reach this point. When a non-zero goal time is specified, + * this has priority over speed and acceleration settings. + * \param blend_radius The radius to be used for blending between control points + * + * \returns True on successful write. + */ + bool writeTrajectoryPoint(const vector6d_t& positions, float acceleration, float velocity, const bool cartesian, + const float goal_time = 0.0, const float blend_radius = 0.052); + /*! * \brief Writes a trajectory spline point for quintic spline interpolation onto the dedicated socket. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 05dae6c3..2bc3ab93 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -479,7 +479,9 @@ thread trajectoryThread(): end # MoveJ point if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT: - movej(q, t = tmptime, r = blend_radius) + acceleration = raw_point[7] / MULT_jointstate + velocity = raw_point[13] / MULT_jointstate + movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] @@ -487,7 +489,9 @@ thread trajectoryThread(): # Movel point elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN: - movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t = tmptime, r = blend_radius) + acceleration = raw_point[7] / MULT_jointstate + velocity = raw_point[13] / MULT_jointstate + movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index e88b724c..2dcbc013 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -38,7 +38,8 @@ TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInter { } -bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, +bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float acceleration, + const float velocity, const float goal_time, const float blend_radius, const bool cartesian) { if (client_fd_ == -1) @@ -56,16 +57,24 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, val = htobe32(val); b_pos += append(b_pos, val); } + for (size_t i = 0; i < positions->size(); ++i) + { + int32_t val = static_cast(round(velocity * MULT_JOINTSTATE)); + val = htobe32(val); + b_pos += append(b_pos, val); + } + for (size_t i = 0; i < positions->size(); ++i) + { + int32_t val = static_cast(round(acceleration * MULT_JOINTSTATE)); + val = htobe32(val); + b_pos += append(b_pos, val); + } } else { b_pos += 6 * sizeof(int32_t); } - // Fill in velocity and acceleration, not used for this point type - b_pos += 6 * sizeof(int32_t); - b_pos += 6 * sizeof(int32_t); - int32_t val = static_cast(round(goal_time * MULT_TIME)); val = htobe32(val); b_pos += append(b_pos, val); @@ -91,6 +100,12 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, + const float blend_radius, const bool cartesian) +{ + return writeTrajectoryPoint(positions, 1.4, 1.05, goal_time, blend_radius, cartesian); +} + bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities, const vector6d_t* accelerations, const float goal_time) { diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 4e05a487..88a485fd 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -232,6 +232,13 @@ bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMo return reverse_interface_->write(&values, control_mode, robot_receive_timeout); } +bool UrDriver::writeTrajectoryPoint(const vector6d_t& positions, const float acceleration, const float velocity, + const bool cartesian, const float goal_time, const float blend_radius) +{ + return trajectory_interface_->writeTrajectoryPoint(&positions, acceleration, velocity, goal_time, blend_radius, + cartesian); +} + bool UrDriver::writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time, const float blend_radius) { diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index c335b3e4..c834e470 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -63,16 +63,15 @@ class TrajectoryPointInterfaceTest : public ::testing::Test int32_t& blend_radius_or_spline_type, int32_t& motion_type) { // Read message - uint8_t buf[sizeof(int32_t) * 21]; + uint8_t buf[sizeof(int32_t) * urcl::control::TrajectoryPointInterface::MESSAGE_LENGTH]; uint8_t* b_pos = buf; size_t read = 0; - size_t remainder = sizeof(int32_t) * 21; + size_t remainder = sizeof(int32_t) * urcl::control::TrajectoryPointInterface::MESSAGE_LENGTH; while (remainder > 0) { if (!TCPSocket::read(b_pos, remainder, read)) { - std::cout << "Failed to read from socket, this should not happen during a test!" << std::endl; - break; + throw(std::runtime_error("Failed to read from socket, this should not happen during a test!")); } b_pos += read; remainder -= read; @@ -135,6 +134,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test return vel; } + vector6int32_t getAcceleration() + { + int32_t goal_time, blend_radius_or_spline_type, motion_type; + vector6int32_t pos, vel, acc; + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type); + return acc; + } + int32_t getGoalTime() { int32_t goal_time, blend_radius_or_spline_type, motion_type; @@ -218,13 +225,13 @@ class TrajectoryPointInterfaceTest : public ::testing::Test private: std::condition_variable trajectory_end_; std::mutex trajectory_end_mutex_; - control::TrajectoryResult result_; + control::TrajectoryResult result_ = control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; }; TEST_F(TrajectoryPointInterfaceTest, write_postions) { urcl::vector6d_t send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; - traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, 0); + traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, false); vector6int32_t received_positions = client_->getPosition(); EXPECT_EQ(send_positions[0], ((double)received_positions[0]) / traj_point_interface_->MULT_JOINTSTATE); @@ -361,9 +368,30 @@ TEST_F(TrajectoryPointInterfaceTest, write_goal_time) { urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 }; float send_goal_time = 0.5; - traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, 0); + traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, false); + int32_t received_goal_time = client_->getGoalTime(); + + EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME); +} + +TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity) +{ + urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 }; + float send_move_acceleration = 0.123; + float send_move_velocity = 0.456; + float send_goal_time = 0.5; + traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity, + send_goal_time, 0, 0); + int32_t received_move_acceleration = client_->getAcceleration()[0]; + traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity, + send_goal_time, 0, 0); + int32_t received_move_velocity = client_->getVelocity()[0]; + traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity, + send_goal_time, 0, 0); int32_t received_goal_time = client_->getGoalTime(); + EXPECT_EQ(send_move_acceleration, ((float)received_move_acceleration) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_move_velocity, ((float)received_move_velocity) / traj_point_interface_->MULT_JOINTSTATE); EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME); } @@ -371,7 +399,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_blend_radius) { urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 }; float send_blend_radius = 0.5; - traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, 0); + traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, false); int32_t received_blend_radius = client_->getBlendRadius(); EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_TIME);