diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8f67cdb5..ca6ed4d6 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -12,13 +12,13 @@ jobs: BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' CTEST_OUTPUT_ON_FAILURE: 1 ROBOT_MODEL: 'ur5' - URSIM_VERSION: '3.12.1' + URSIM_VERSION: '3.14.3' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/cb3' - DOCKER_RUN_OPTS: --network ursim_net BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' CTEST_OUTPUT_ON_FAILURE: 1 ROBOT_MODEL: 'ur5e' - URSIM_VERSION: '5.5.1' + URSIM_VERSION: '5.9.4' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' steps: diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index bf1af8c9..653d126f 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -25,7 +25,7 @@ jobs: DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS_Driver#master https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS_Driver/master/.melodic.rosinstall" DOCKER_RUN_OPTS: --network ursim_net BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - URSIM_VERSION: '5.5.1' + URSIM_VERSION: '5.9.4' ROBOT_MODEL: 'ur5e' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' - ROS_DISTRO: noetic @@ -35,7 +35,7 @@ jobs: BUILDER: catkin_tools DOCKER_RUN_OPTS: --network ursim_net BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - URSIM_VERSION: '5.5.1' + URSIM_VERSION: '5.9.4' ROBOT_MODEL: 'ur5e' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' - ROS_DISTRO: galactic @@ -44,7 +44,7 @@ jobs: DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#galactic" DOCKER_RUN_OPTS: --network ursim_net BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - URSIM_VERSION: '5.5.1' + URSIM_VERSION: '5.9.4' ROBOT_MODEL: 'ur5e' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' NOT_TEST_DOWNSTREAM: true @@ -54,7 +54,7 @@ jobs: DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#humble https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS2_Driver/humble/Universal_Robots_ROS2_Driver-not-released.humble.repos" DOCKER_RUN_OPTS: --network ursim_net BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - URSIM_VERSION: '5.5.1' + URSIM_VERSION: '5.9.4' ROBOT_MODEL: 'ur5e' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' NOT_TEST_DOWNSTREAM: true @@ -64,7 +64,7 @@ jobs: DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#main https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS2_Driver/main/Universal_Robots_ROS2_Driver-not-released.rolling.repos" DOCKER_RUN_OPTS: --network ursim_net BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - URSIM_VERSION: '5.5.1' + URSIM_VERSION: '5.9.4' ROBOT_MODEL: 'ur5e' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' NOT_TEST_DOWNSTREAM: true diff --git a/README.md b/README.md index f7fd515e..c49b75a8 100644 --- a/README.md +++ b/README.md @@ -32,8 +32,8 @@ implemented in order to create external applications leveraging the versatility robotic manipulators. ## Requirements - * **Polyscope** (The software running on the robot controller) version **3.12.0** (for CB3-Series), - or **5.5.1** (for e-Series) or higher. If you use an older Polyscope version it is suggested to + * **Polyscope** (The software running on the robot controller) version **3.14.3** (for CB3-Series), + or **5.9.4** (for e-Series) or higher. If you use an older Polyscope version it is suggested to update your robot. If for some reason (please tell us in the issues why) you cannot upgrade your robot, please see the [version compatibility table](doc/polyscope_compatibility.md) for a compatible tag. diff --git a/doc/polyscope_compatibility.md b/doc/polyscope_compatibility.md index 9aad4b1f..b4e023b2 100644 --- a/doc/polyscope_compatibility.md +++ b/doc/polyscope_compatibility.md @@ -14,4 +14,4 @@ your robot. |Polyscope version | Maximum tag | Breaking changes | |------------------|-------------|------------------| -| < 3.12.0 / 5.5.1 | [polyscope_compat_break_1](https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/polyscope_compat_break_1) | [tcp_offset in RTDE interface](https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/110)| +| < 3.14.3 / 5.9.4 | [polyscope_compat_break_1](https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/polyscope_compat_break_1) | [tcp_offset in RTDE interface](https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/110)| diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 18aadb24..eb4c313d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -35,6 +35,11 @@ add_executable(dashboard_example target_compile_options(dashboard_example PUBLIC ${CXX17_FLAG}) target_link_libraries(dashboard_example ur_client_library::urcl) +add_executable(spline_example +spline_example.cpp) +target_compile_options(spline_example PUBLIC ${CXX17_FLAG}) +target_link_libraries(spline_example ur_client_library::urcl) + add_executable(tool_contact_example tool_contact_example.cpp) target_compile_options(tool_contact_example PUBLIC ${CXX17_FLAG}) diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 3a2135ab..272d1864 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -110,6 +110,8 @@ int main(int argc, char* argv[]) const bool HEADLESS = true; g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_my_driver->setKeepaliveCount(5); // This is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Do not use this in productive applications. // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp new file mode 100644 index 00000000..47ffa81a --- /dev/null +++ b/examples/spline_example.cpp @@ -0,0 +1,294 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2023 Universal Robots A/S +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include +#include + +#include +#include + +using namespace urcl; + +// In a real-world example it would be better to get those values from command line parameters / a +// better configuration system such as Boost.Program_options +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_driver; +std::unique_ptr g_my_dashboard; +vector6d_t g_joint_positions; + +void SendTrajectory(const std::vector& p_p, const std::vector& p_v, + const std::vector& p_a, const std::vector& time, bool use_spline_interpolation_) +{ + assert(p_p.size() == time.size()); + + URCL_LOG_INFO("Starting joint-based trajectory forward"); + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, p_p.size()); + + for (size_t i = 0; i < p_p.size() && p_p.size() == time.size() && p_p[i].size() == 6; i++) + { + // MoveJ + if (!use_spline_interpolation_) + { + g_my_driver->writeTrajectoryPoint(p_p[i], false, time[i]); + } + else // Use spline interpolation + { + // QUINTIC + if (p_v.size() == time.size() && p_a.size() == time.size() && p_v[i].size() == 6 && p_a[i].size() == 6) + { + g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]); + } + // CUBIC + else if (p_v.size() == time.size() && p_v[i].size() == 6) + { + g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); + } + else + { + g_my_driver->writeTrajectorySplinePoint(p_p[i], time[i]); + } + } + } + URCL_LOG_INFO("Finished Sending Trajectory"); +} + +// 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; +} + +// Callback function for trajectory execution. +bool g_trajectory_running(false); +void handleTrajectoryState(control::TrajectoryResult state) +{ + // trajectory_state = state; + g_trajectory_running = false; + std::string report = "?"; + switch (state) + { + case control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: + report = "success"; + break; + case control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: + report = "canceled"; + break; + case control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: + default: + report = "failure"; + break; + } + std::cout << "\033[1;32mTrajectory report: " << report << "\033[0m\n" << std::endl; +} + +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]); + } + + // Making the robot ready for the program by: + // Connect the the robot Dashboard + g_my_dashboard.reset(new 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; + } + + // if the robot is not powered on and ready + std::string robotModeRunning("RUNNING"); + while (!g_my_dashboard->commandRobotMode(robotModeRunning)) + { + // 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; + } + + // Now the robot is ready to receive a program + + std::unique_ptr tool_comm_setup; + const bool HEADLESS = true; + g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, + std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_my_driver->setKeepaliveCount(5); // This is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Do not use this in productive applications. + + g_my_driver->registerTrajectoryDoneCallback(&handleTrajectoryState); + + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as + // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main + // loop. + + g_my_driver->startRTDECommunication(); + + std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + + if (data_pkg) + { + // Read current joint positions from robot data + if (!data_pkg->getData("actual_q", g_joint_positions)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + } + + const unsigned number_of_points = 5; + const double s_pos[number_of_points] = { 4.15364583e-03, 4.15364583e-03, -1.74088542e-02, 1.44817708e-02, 0.0 }; + const double s_vel[number_of_points] = { -2.01015625e-01, 4.82031250e-02, 1.72812500e-01, -3.49453125e-01, + 8.50000000e-02 }; + const double s_acc[number_of_points] = { 2.55885417e+00, -4.97395833e-01, 1.71276042e+00, -5.36458333e-02, + -2.69817708e+00 }; + const double s_time[number_of_points] = { 1.0000000e+00, 4.00000000e+00, 8.00100000e+00, 1.25000000e+01, + 4.00000000e+00 }; + + bool ret = false; + URCL_LOG_INFO("Switch to Forward mode"); + ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; + } + + std::vector p, v, a; + std::vector time; + + unsigned int joint_to_control = 5; + for (unsigned i = 0; i < number_of_points; ++i) + { + vector6d_t p_i = g_joint_positions; + p_i[joint_to_control] = s_pos[i]; + p.push_back(p_i); + + vector6d_t v_i; + v_i[joint_to_control] = s_vel[i]; + v.push_back(v_i); + + vector6d_t a_i; + a_i[joint_to_control] = s_acc[i]; + a.push_back(a_i); + + time.push_back(s_time[i]); + } + + // CUBIC + SendTrajectory(p, v, std::vector(), time, true); + + g_trajectory_running = true; + while (g_trajectory_running) + { + std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + if (data_pkg) + { + // Read current joint positions from robot data + if (!data_pkg->getData("actual_q", g_joint_positions)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); + + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; + } + } + } + + URCL_LOG_INFO("CUBIC Movement done"); + + // QUINTIC + SendTrajectory(p, v, a, time, true); + ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); + + g_trajectory_running = true; + while (g_trajectory_running) + { + std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + if (data_pkg) + { + // Read current joint positions from robot data + if (!data_pkg->getData("actual_q", g_joint_positions)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); + + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; + } + } + } + + URCL_LOG_INFO("QUINTIC Movement done"); + + ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; + } + return 0; +} diff --git a/examples/tool_contact_example.cpp b/examples/tool_contact_example.cpp index 483d5206..e47de086 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_example.cpp @@ -114,6 +114,8 @@ int main(int argc, char* argv[]) const bool HEADLESS = true; g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_my_driver->setKeepaliveCount(5); // This is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Do not use this in productive applications. g_my_driver->registerToolContactResultCallback(&handleToolContactResult); diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 7afd34e0..e64e0035 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -49,6 +49,25 @@ enum class TrajectoryResult : int32_t TRAJECTORY_RESULT_FAILURE = 2 ///< Aborted due to error during execution }; +/*! + * Spline types + */ +enum class TrajectorySplineType : int32_t +{ + SPLINE_CUBIC = 1, + SPLINE_QUINTIC = 2 +}; + +/*! + * Motion Types + */ +enum class TrajectoryMotionType : int32_t +{ + JOINT_POINT = 0, + CARTESIAN_POINT = 1, + JOINT_POINT_SPLINE = 2 +}; + /*! * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full * trajectories are forwarded to the robot controller and are executed there. @@ -57,8 +76,6 @@ class TrajectoryPointInterface : public ReverseInterface { public: static const int32_t MULT_TIME = 1000; - static const int32_t JOINT_POINT = 0; - static const int32_t CARTESIAN_POINT = 1; TrajectoryPointInterface() = delete; /*! @@ -86,6 +103,21 @@ class TrajectoryPointInterface : public ReverseInterface bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius, const bool cartesian); + /*! + * \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 + * only), cubic (positions and velocities) or quintic (positions, velocities and accelerations) interpolation. + * + * \param positions A vector of joint or Cartesian target positions for the robot. + * \param velocities A vector of joint or Cartesian target velocities for the robot. + * \param accelerations A vector of joint or Cartesian target accelerations for the robot. + * \param goal_time The goal time to reach the target point. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities, + const vector6d_t* accelerations, const float goal_time); + void setTrajectoryEndCallback(std::function callback) { handle_trajectory_end_ = callback; @@ -99,6 +131,7 @@ 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 a53b4f8e..3da480d6 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -211,16 +211,51 @@ class UrDriver /*! * \brief Writes a trajectory point onto the dedicated socket. * - * \param values Desired joint or cartesian positions + * \param positions Desired joint or cartesian positions * \param cartesian True, if the point sent is cartesian, false if joint-based * \param goal_time Time for the robot to reach this point * \param blend_radius The radius to be used for blending between control points * * \returns True on successful write. */ - bool writeTrajectoryPoint(const vector6d_t& values, const bool cartesian, const float goal_time = 0.0, + 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 spline point for quintic spline interpolation onto the dedicated socket. + * + * \param positions Desired joint positions + * \param velocities Desired joint velocities + * \param accelerations Desired joint accelerations + * \param goal_time Time for the robot to reach this point + * + * \returns True on successful write. + */ + bool writeTrajectorySplinePoint(const vector6d_t& positions, const vector6d_t& velocities, + const vector6d_t& accelerations, const float goal_time = 0.0); + + /*! + * \brief Writes a trajectory spline point for cubic spline interpolation onto the dedicated socket. + * + * \param positions Desired joint positions + * \param velocities Desired joint velocities + * \param goal_time Time for the robot to reach this point + * + * \returns True on successful write. + */ + bool writeTrajectorySplinePoint(const vector6d_t& positions, const vector6d_t& velocities, + const float goal_time = 0.0); + + /*! + * \brief Writes a trajectory spline point for quadratic spline interpolation onto the dedicated socket. + * + * \param positions Desired joint positions + * \param goal_time Time for the robot to reach this point + * + * \returns True on successful write. + */ + bool writeTrajectorySplinePoint(const vector6d_t& positions, const float goal_time = 0.0); + /*! * \brief Writes a control message in trajectory forward mode. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index f49010a2..18415eaf 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -8,6 +8,8 @@ textmsg("ExternalControl: steptime=", steptime) MULT_jointstate = {{JOINT_STATE_REPLACE}} MULT_time = {{TIME_REPLACE}} +STOPJ_ACCELERATION = 4.0 + #Constants SERVO_UNINITIALIZED = -1 SERVO_IDLE = 0 @@ -31,7 +33,8 @@ TRAJECTORY_MODE_CANCEL = -1 TRAJECTORY_POINT_JOINT = 0 TRAJECTORY_POINT_CARTESIAN = 1 -TRAJECTORY_DATA_DIMENSION = 7 +TRAJECTORY_POINT_JOINT_SPLINE = 2 +TRAJECTORY_DATA_DIMENSION = 3*6 + 1 TRAJECTORY_RESULT_SUCCESS = 0 TRAJECTORY_RESULT_CANCELED = 1 @@ -52,6 +55,9 @@ FREEDRIVE_MODE_STOP = -1 UNTIL_TOOL_CONTACT_RESULT_SUCCESS = 0 UNTIL_TOOL_CONTACT_RESULT_CANCELED = 1 +SPLINE_CUBIC = 1 +SPLINE_QUINTIC = 2 + #Global variables are also showed in the Teach pendants variable list global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] @@ -62,6 +68,7 @@ global extrapolate_count = 0 global extrapolate_max_count = 0 global control_mode = MODE_UNINITIALIZED global trajectory_points_left = 0 +global last_spline_qdd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False # Global thread variables @@ -117,7 +124,7 @@ thread servoThread(): exit_critical end textmsg("ExternalControl: servo thread ended") - stopj(4.0) + stopj(STOPJ_ACCELERATION) end # Helpers for speed control @@ -133,41 +140,145 @@ thread speedThread(): speedj(qd, 40.0, steptime) end textmsg("ExternalControl: speedj thread ended") - stopj(5.0) + stopj(STOPJ_ACCELERATION) +end + +def cubicSplineRun(end_q, end_qd, time): + # Zero time means zero length and therefore no motion to execute + if time > 0.0: + local start_q = get_target_joint_positions() + local start_qd = get_target_joint_speeds() + + # Coefficients0 is not included, since we do not need to calculate the position + local coefficients1 = start_qd + local coefficients2 = (-3 * start_q + end_q * 3 - start_qd * 2 * time - end_qd * time) / pow(time, 2) + local coefficients3 = (2 * start_q - 2 * end_q + start_qd * time + end_qd * time) / pow(time, 3) + local coefficients4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + local coefficients5 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time) + end +end + +def quinticSplineRun(end_q, end_qd, end_qdd, time): + # Zero time means zero length and therefore no motion to execute + if time > 0.0: + local start_q = get_target_joint_positions() + local start_qd = get_target_joint_speeds() + local start_qdd = last_spline_qdd + + # Pre-calculate coefficients + local TIME2 = pow(time, 2) + # Coefficients0 is not included, since we do not need to calculate the position + local coefficients1 = start_qd + local coefficients2 = 0.5 * start_qdd + local coefficients3 = (-20.0 * start_q + 20.0 * end_q - 3.0 * start_qdd * TIME2 + end_qdd * TIME2 - 12.0 * start_qd * time - 8.0 * end_qd * time) / (2.0 * pow(time, 3)) + local coefficients4 = (30.0 * start_q - 30.0 * end_q + 3.0 * start_qdd * TIME2 - 2.0 * end_qdd * TIME2 + 16.0 * start_qd * time + 14.0 * end_qd * time) / (2.0 * pow(time, 4)) + local coefficients5 = (-12.0 * start_q + 12.0 * end_q - start_qdd * TIME2 + end_qdd * TIME2 - 6.0 * start_qd * time - 6.0 * end_qd * time) / (2.0 * pow(time, 5)) + jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time) + end +end + +def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime): + # Initialize incremental time variable + local splineTimerTraveled = 0.0 + + # Interpolate the spline in hole time steps + while (splineTotalTravelTime - splineTimerTraveled) > get_steptime(): + splineTimerTraveled = splineTimerTraveled + get_steptime() + jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime()) + end + + # Last part of the spline which uses less than one time step + local timeLeftToTravel = splineTotalTravelTime - splineTimerTraveled + + # To round off the float to the steptime step when it is very close to that number (1e-15) + if timeLeftToTravel == get_steptime(): + timeLeftToTravel = get_steptime() + end + + jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, timeLeftToTravel) +end + +def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep): + local qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5 + last_spline_qdd = 2.0 * coefficients2 + 6.0 * splineTimerTraveled * coefficients3 + 12.0 * pow(splineTimerTraveled, 2) * coefficients4 + 20.0 * pow(splineTimerTraveled, 3) * coefficients5 + speedj(qd, 1000, timestep) end -thread jointTrajectoryThread(): +thread trajectoryThread(): textmsg("Executing trajectory. Number of points: ", trajectory_points_left) + local INDEX_TIME = TRAJECTORY_DATA_DIMENSION + local INDEX_BLEND = INDEX_TIME + 1 + # same index as blend parameter, depending on point type + local INDEX_SPLINE_TYPE = INDEX_BLEND + local INDEX_POINT_TYPE = INDEX_BLEND + 1 + last_spline_qdd = [0, 0, 0, 0, 0, 0] + enter_critical while trajectory_points_left > 0: - enter_critical #reading trajectory point + blend radius + type of point (cartesian/joint based) - raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket") + local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime()) trajectory_points_left = trajectory_points_left - 1 - exit_critical + if raw_point[0] > 0: - q = [ raw_point[1] / MULT_jointstate, raw_point[2] / MULT_jointstate, raw_point[3] / MULT_jointstate, raw_point[4] / MULT_jointstate, raw_point[5] / MULT_jointstate, raw_point[6] / MULT_jointstate] - tmptime = raw_point[7] / MULT_time - blend_radius = raw_point[8] / MULT_time - + local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate] + local tmptime = raw_point[INDEX_TIME] / MULT_time + local blend_radius = raw_point[INDEX_BLEND] / MULT_time if trajectory_points_left > 0: - if raw_point[9] == TRAJECTORY_POINT_JOINT: - # textmsg("Executing movej with blending") + # MoveJ point + if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT: movej(q, t=tmptime, r=blend_radius) - elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN: - # textmsg("Executing movel with blending") + + # reset old acceleration + last_spline_qdd = [0, 0, 0, 0, 0, 0] + + # 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) + + # reset old acceleration + last_spline_qdd = [0, 0, 0, 0, 0, 0] + + # Joint spline point + elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE: + + # Cubic spline + if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC: + qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate] + cubicSplineRun(q, qd, tmptime) + # reset old acceleration + last_spline_qdd = [0, 0, 0, 0, 0, 0] + + # Quintic spline + elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC: + qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate] + qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate] + quinticSplineRun(q, qd, qdd, tmptime) + else: + textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) + clear_remaining_trajectory_points() + socket_send_int(TRAJECTORY_RESULT_FAILURE, "trajectory_socket") + end end - else: - if raw_point[9] == TRAJECTORY_POINT_JOINT: - # textmsg("Executing movej without blending") + # Last trajectory point + else: + if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT: movej(q, t=tmptime, r=0.0) - elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN: - # textmsg("Executing movel without blending") + 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=0.0) + elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE: + if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC: + cubicSplineRun(q, [0,0,0,0,0,0], tmptime) + elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC: + quinticSplineRun(q, [0,0,0,0,0,0], [0,0,0,0,0,0], tmptime) + else: + textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) + socket_send_int(TRAJECTORY_RESULT_FAILURE, "trajectory_socket") + end end end end end + exit_critical socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") textmsg("Trajectory finished") end @@ -192,7 +303,7 @@ thread speedlThread(): speedl(twist, 40.0, steptime) end textmsg("speedl thread ended") - stopj(5.0) + stopj(STOPJ_ACCELERATION) end thread servoThreadP(): @@ -229,7 +340,7 @@ thread servoThreadP(): exit_critical end textmsg("pose servo thread ended") - stopj(4.0) + stopj(STOPJ_ACCELERATION) end def set_servo_pose(pose): @@ -331,6 +442,7 @@ while keepalive > 0 and control_mode > MODE_STOPPED: # Clear remaining trajectory points if control_mode == MODE_FORWARD: kill thread_trajectory + stopj(STOPJ_ACCELERATION) clear_remaining_trajectory_points() # Stop freedrive elif control_mode == MODE_FREEDRIVE: @@ -353,24 +465,27 @@ while keepalive > 0 and control_mode > MODE_STOPPED: thread_move = run speedThread() elif control_mode == MODE_FORWARD: kill thread_move + stopj(STOPJ_ACCELERATION) elif control_mode == MODE_SPEEDL: thread_move = run speedlThread() elif control_mode == MODE_POSE: thread_move = run servoThreadP() end end + + # Update the motion commands with new parameters if control_mode == MODE_SERVOJ: - q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + q = [params_mult[2]/ MULT_jointstate, params_mult[3]/ MULT_jointstate, params_mult[4]/ MULT_jointstate, params_mult[5]/ MULT_jointstate, params_mult[6]/ MULT_jointstate, params_mult[7]/ MULT_jointstate] set_servo_setpoint(q) elif control_mode == MODE_SPEEDJ: - qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + qd = [params_mult[2]/ MULT_jointstate, params_mult[3]/ MULT_jointstate, params_mult[4]/ MULT_jointstate, params_mult[5]/ MULT_jointstate, params_mult[6]/ MULT_jointstate, params_mult[7]/ MULT_jointstate] set_speed(qd) elif control_mode == MODE_FORWARD: if params_mult[2] == TRAJECTORY_MODE_RECEIVE: kill thread_trajectory clear_remaining_trajectory_points() trajectory_points_left = params_mult[3] - thread_trajectory = run jointTrajectoryThread() + thread_trajectory = run trajectoryThread() elif params_mult[2] == TRAJECTORY_MODE_CANCEL: textmsg("cancel received") kill thread_trajectory diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 257422eb..4581678a 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -27,6 +27,7 @@ //---------------------------------------------------------------------- #include +#include namespace urcl { @@ -43,7 +44,7 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, { return false; } - uint8_t buffer[sizeof(int32_t) * 9]; + uint8_t buffer[sizeof(int32_t) * MESSAGE_LENGTH]; uint8_t* b_pos = buffer; if (positions != nullptr) @@ -60,6 +61,10 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, 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(goal_time * MULT_TIME); val = htobe32(val); b_pos += append(b_pos, val); @@ -70,11 +75,11 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, if (cartesian) { - val = CARTESIAN_POINT; + val = static_cast(control::TrajectoryMotionType::CARTESIAN_POINT); } else { - val = JOINT_POINT; + val = static_cast(control::TrajectoryMotionType::JOINT_POINT); } val = htobe32(val); @@ -85,6 +90,81 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities, + const vector6d_t* accelerations, const float goal_time) +{ + if (client_fd_ == -1) + { + return false; + } + + control::TrajectorySplineType spline_type = control::TrajectorySplineType::SPLINE_CUBIC; + + // 6 positions, 6 velocities, 6 accelerations, 1 goal time, spline type, 1 point type + uint8_t buffer[sizeof(int32_t) * MESSAGE_LENGTH] = { 0 }; + uint8_t* b_pos = buffer; + if (positions != nullptr) + { + for (auto const& pos : *positions) + { + int32_t val = static_cast(pos * MULT_JOINTSTATE); + val = htobe32(val); + b_pos += append(b_pos, val); + } + } + else + { + throw urcl::UrException("TrajectoryPointInterface::writeTrajectorySplinePoint is only getting a nullptr for " + "positions\n"); + } + + if (velocities != nullptr) + { + spline_type = control::TrajectorySplineType::SPLINE_CUBIC; + for (auto const& vel : *velocities) + { + int32_t val = static_cast(vel * MULT_JOINTSTATE); + val = htobe32(val); + b_pos += append(b_pos, val); + } + } + else + { + throw urcl::UrException("TrajectoryPointInterface::writeTrajectorySplinePoint is only getting a nullptr for " + "velocities\n"); + } + + if (accelerations != nullptr) + { + spline_type = control::TrajectorySplineType::SPLINE_QUINTIC; + for (auto const& acc : *accelerations) + { + int32_t val = static_cast(acc * MULT_JOINTSTATE); + val = htobe32(val); + b_pos += append(b_pos, val); + } + } + else + { + b_pos += 6 * sizeof(int32_t); + } + + int32_t val = static_cast(goal_time * MULT_TIME); + val = htobe32(val); + b_pos += append(b_pos, val); + + val = static_cast(spline_type); + val = htobe32(val); + b_pos += append(b_pos, val); + + val = static_cast(control::TrajectoryMotionType::JOINT_POINT_SPLINE); + val = htobe32(val); + b_pos += append(b_pos, val); + + size_t written; + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + void TrajectoryPointInterface::connectionCallback(const int filedescriptor) { if (client_fd_ < 0) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 8bc30351..2e73a40f 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -264,10 +264,27 @@ bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMo return reverse_interface_->write(&values, control_mode); } -bool UrDriver::writeTrajectoryPoint(const vector6d_t& values, const bool cartesian, const float goal_time, +bool UrDriver::writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time, const float blend_radius) { - return trajectory_interface_->writeTrajectoryPoint(&values, goal_time, blend_radius, cartesian); + return trajectory_interface_->writeTrajectoryPoint(&positions, goal_time, blend_radius, cartesian); +} + +bool UrDriver::writeTrajectorySplinePoint(const vector6d_t& positions, const vector6d_t& velocities, + const vector6d_t& accelerations, const float goal_time) +{ + return trajectory_interface_->writeTrajectorySplinePoint(&positions, &velocities, &accelerations, goal_time); +} + +bool UrDriver::writeTrajectorySplinePoint(const vector6d_t& positions, const vector6d_t& velocities, + const float goal_time) +{ + return trajectory_interface_->writeTrajectorySplinePoint(&positions, &velocities, nullptr, goal_time); +} + +bool UrDriver::writeTrajectorySplinePoint(const vector6d_t& positions, const float goal_time) +{ + return trajectory_interface_->writeTrajectorySplinePoint(&positions, nullptr, nullptr, goal_time); } bool UrDriver::writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action, diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index 061a9fd1..18483e57 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -59,13 +59,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test TCPSocket::write(buffer, sizeof(buffer), written); } - void readMessage(vector6int32_t& pos, int32_t& goal_time, int32_t& blend_radius, int32_t& cartesian) + void readMessage(vector6int32_t& pos, vector6int32_t& vel, vector6int32_t& acc, int32_t& goal_time, + int32_t& blend_radius_or_spline_type, int32_t& motion_type) { // Read message - uint8_t buf[sizeof(int32_t) * 9]; + uint8_t buf[sizeof(int32_t) * 21]; uint8_t* b_pos = buf; size_t read = 0; - size_t remainder = sizeof(int32_t) * 9; + size_t remainder = sizeof(int32_t) * 21; while (remainder > 0) { TCPSocket::setOptions(getSocketFD()); @@ -88,51 +89,88 @@ class TrajectoryPointInterfaceTest : public ::testing::Test b_pos += sizeof(int32_t); } + // Read velocity + for (unsigned int i = 0; i < pos.size(); ++i) + { + std::memcpy(&val, b_pos, sizeof(int32_t)); + vel[i] = be32toh(val); + b_pos += sizeof(int32_t); + } + + // Read acceleration + for (unsigned int i = 0; i < pos.size(); ++i) + { + std::memcpy(&val, b_pos, sizeof(int32_t)); + acc[i] = be32toh(val); + b_pos += sizeof(int32_t); + } + // Decode goal time std::memcpy(&val, b_pos, sizeof(int32_t)); goal_time = be32toh(val); b_pos += sizeof(int32_t); - // Decode blend radius + // Decode blend radius or spline type std::memcpy(&val, b_pos, sizeof(int32_t)); - blend_radius = be32toh(val); + blend_radius_or_spline_type = be32toh(val); b_pos += sizeof(int32_t); - // Decode cartesian + // Decode motion type std::memcpy(&val, b_pos, sizeof(int32_t)); - cartesian = be32toh(val); + motion_type = be32toh(val); } vector6int32_t getPosition() { - int32_t goal_time, blend_radius, cartesian; - vector6int32_t pos; - readMessage(pos, goal_time, blend_radius, cartesian); + 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 pos; } + vector6int32_t getVelocity() + { + 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 vel; + } + int32_t getGoalTime() { - int32_t goal_time, blend_radius, cartesian; - vector6int32_t pos; - readMessage(pos, goal_time, blend_radius, cartesian); + 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 goal_time; } int32_t getBlendRadius() { - int32_t goal_time, blend_radius, cartesian; - vector6int32_t pos; - readMessage(pos, goal_time, blend_radius, cartesian); - return blend_radius; + 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 blend_radius_or_spline_type; } - int32_t getCartesian() + int32_t getMotionType() { - int32_t goal_time, blend_radius, cartesian; - vector6int32_t pos; - readMessage(pos, goal_time, blend_radius, cartesian); - return cartesian; + 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 motion_type; + } + + struct TrajData + { + vector6int32_t pos, vel, acc; + int32_t goal_time, blend_radius_or_spline_type, motion_type; + }; + + TrajData getData() + { + TrajData spl; + readMessage(spl.pos, spl.vel, spl.acc, spl.goal_time, spl.blend_radius_or_spline_type, spl.motion_type); + return spl; } protected: @@ -204,6 +242,128 @@ TEST_F(TrajectoryPointInterfaceTest, write_postions) EXPECT_EQ(send_positions[5], ((double)received_positions[5]) / traj_point_interface_->MULT_JOINTSTATE); } +TEST_F(TrajectoryPointInterfaceTest, write_quintic_joint_spline) +{ + urcl::vector6d_t send_pos = { 1.2, 3.1, 2.2, -1.4, -2.1, -3.2 }; + urcl::vector6d_t send_vel = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + urcl::vector6d_t send_acc = { 3.2, 1.1, 1.2, -3.4, -1.1, -1.2 }; + float send_goal_time = 0.5; + traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &send_vel, &send_acc, send_goal_time); + Client::TrajData received_data = client_->getData(); + + // Position + EXPECT_EQ(send_pos[0], ((double)received_data.pos[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[1], ((double)received_data.pos[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[2], ((double)received_data.pos[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[3], ((double)received_data.pos[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[4], ((double)received_data.pos[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[5], ((double)received_data.pos[5]) / traj_point_interface_->MULT_JOINTSTATE); + + // Velocities + EXPECT_EQ(send_vel[0], ((double)received_data.vel[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[1], ((double)received_data.vel[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[2], ((double)received_data.vel[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[3], ((double)received_data.vel[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[4], ((double)received_data.vel[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[5], ((double)received_data.vel[5]) / traj_point_interface_->MULT_JOINTSTATE); + + // Velocities + EXPECT_EQ(send_acc[0], ((double)received_data.acc[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[1], ((double)received_data.acc[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[2], ((double)received_data.acc[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[3], ((double)received_data.acc[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[4], ((double)received_data.acc[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE); + + // Goal time + EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_TIME)); + + // Spline type + EXPECT_EQ(static_cast(control::TrajectorySplineType::SPLINE_QUINTIC), + received_data.blend_radius_or_spline_type); + + // Motion type + EXPECT_EQ(static_cast(control::TrajectoryMotionType::JOINT_POINT_SPLINE), received_data.motion_type); +} + +TEST_F(TrajectoryPointInterfaceTest, write_cubic_joint_spline) +{ + urcl::vector6d_t send_pos = { 1.2, 3.1, 2.2, -1.4, -2.1, -3.2 }; + urcl::vector6d_t send_vel = { 2.2, 2.1, 3.2, -2.4, -3.1, -2.3 }; + urcl::vector6d_t send_acc = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + float send_goal_time = 1.5; + traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &send_vel, nullptr, send_goal_time); + Client::TrajData received_data = client_->getData(); + + // Position + EXPECT_EQ(send_pos[0], ((double)received_data.pos[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[1], ((double)received_data.pos[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[2], ((double)received_data.pos[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[3], ((double)received_data.pos[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[4], ((double)received_data.pos[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_pos[5], ((double)received_data.pos[5]) / traj_point_interface_->MULT_JOINTSTATE); + + // Velocities + EXPECT_EQ(send_vel[0], ((double)received_data.vel[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[1], ((double)received_data.vel[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[2], ((double)received_data.vel[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[3], ((double)received_data.vel[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[4], ((double)received_data.vel[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[5], ((double)received_data.vel[5]) / traj_point_interface_->MULT_JOINTSTATE); + + // Velocities + EXPECT_EQ(send_acc[0], ((double)received_data.acc[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[1], ((double)received_data.acc[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[2], ((double)received_data.acc[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[3], ((double)received_data.acc[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[4], ((double)received_data.acc[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE); + + // Goal time + EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_TIME); + + // Spline type + EXPECT_EQ(static_cast(control::TrajectorySplineType::SPLINE_CUBIC), + received_data.blend_radius_or_spline_type); + + // Motion type + EXPECT_EQ(static_cast(control::TrajectoryMotionType::JOINT_POINT_SPLINE), received_data.motion_type); +} + +TEST_F(TrajectoryPointInterfaceTest, write_splines_velocities) +{ + urcl::vector6d_t send_pos = { 1.2, 3.1, 2.2, -1.4, -2.1, -3.2 }; + urcl::vector6d_t send_vel = { 2.2, 2.1, 3.2, -2.4, -3.1, -2.2 }; + urcl::vector6d_t send_acc = { 3.2, 1.1, 1.2, -3.4, -1.1, -1.2 }; + float send_goal_time = 0.5; + traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &send_vel, &send_acc, send_goal_time); + vector6int32_t received_velocities = client_->getVelocity(); + + EXPECT_EQ(send_vel[0], ((double)received_velocities[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[1], ((double)received_velocities[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[2], ((double)received_velocities[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[3], ((double)received_velocities[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[4], ((double)received_velocities[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[5], ((double)received_velocities[5]) / traj_point_interface_->MULT_JOINTSTATE); +} + +TEST_F(TrajectoryPointInterfaceTest, write_splines_accelerations) +{ + urcl::vector6d_t send_pos = { 1.2, 3.1, 2.2, -1.4, -2.1, -3.2 }; + urcl::vector6d_t send_vel = { 2.2, 2.1, 3.2, -2.4, -3.1, -2.2 }; + urcl::vector6d_t send_acc = { 3.2, 1.1, 1.2, -3.4, -1.1, -1.2 }; + float send_goal_time = 0.5; + traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &send_vel, &send_acc, send_goal_time); + vector6int32_t received_velocities = client_->getVelocity(); + + EXPECT_EQ(send_vel[0], ((double)received_velocities[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[1], ((double)received_velocities[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[2], ((double)received_velocities[2]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[3], ((double)received_velocities[3]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[4], ((double)received_velocities[4]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_vel[5], ((double)received_velocities[5]) / traj_point_interface_->MULT_JOINTSTATE); +} + TEST_F(TrajectoryPointInterfaceTest, write_goal_time) { urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 }; @@ -230,14 +390,14 @@ TEST_F(TrajectoryPointInterfaceTest, write_cartesian) urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 }; bool send_cartesian = true; traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, send_cartesian); - bool received_cartesian = bool(client_->getCartesian()); + bool received_cartesian = bool(client_->getMotionType()); EXPECT_EQ(send_cartesian, received_cartesian); // Write joint point send_cartesian = false; traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, send_cartesian); - received_cartesian = bool(client_->getCartesian()); + received_cartesian = bool(client_->getMotionType()); EXPECT_EQ(send_cartesian, received_cartesian); }