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..37139ad4 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); // decrease realtime requirements since example is running in + // CI // 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..b7b7b92a --- /dev/null +++ b/examples/spline_example.cpp @@ -0,0 +1,288 @@ +// 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_) +{ + 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; + } + + // 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); // decrease realtime requirements since example is running in + // CI + + 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, 2.50000000e+01 / 2, + 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..0a55da08 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); // decrease realtime requirements since example is running in + // CI 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..1f5e50ff 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -49,6 +49,26 @@ enum class TrajectoryResult : int32_t TRAJECTORY_RESULT_FAILURE = 2 ///< Aborted due to error during execution }; +/*! + * Spline types + */ +enum class TrajectorySplineType : int32_t +{ + SPLINE_QUADRATIC = 0, + 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 +77,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 +104,20 @@ 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..dc194537 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,10 @@ FREEDRIVE_MODE_STOP = -1 UNTIL_TOOL_CONTACT_RESULT_SUCCESS = 0 UNTIL_TOOL_CONTACT_RESULT_CANCELED = 1 +SPLINE_QUADRATIC = 0 # Not implemented yet +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 +69,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 +125,7 @@ thread servoThread(): exit_critical end textmsg("ExternalControl: servo thread ended") - stopj(4.0) + stopj(STOPJ_ACCELERATION) end # Helpers for speed control @@ -133,41 +141,140 @@ 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): + textmsg("cubicSplineRun time: ", 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], raw_point[2], raw_point[3], raw_point[4], raw_point[5], 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], raw_point[8], raw_point[9], raw_point[10], raw_point[11], 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], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate + qdd = [ raw_point[13], raw_point[14], raw_point[15], raw_point[16], raw_point[17], raw_point[18]] / MULT_jointstate + quinticSplineRun(q, qd, qdd, tmptime) + 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: + # Cubic 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) + end end end end end + exit_critical socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") textmsg("Trajectory finished") end @@ -192,7 +299,7 @@ thread speedlThread(): speedl(twist, 40.0, steptime) end textmsg("speedl thread ended") - stopj(5.0) + stopj(STOPJ_ACCELERATION) end thread servoThreadP(): @@ -229,7 +336,7 @@ thread servoThreadP(): exit_critical end textmsg("pose servo thread ended") - stopj(4.0) + stopj(STOPJ_ACCELERATION) end def set_servo_pose(pose): @@ -276,7 +383,7 @@ thread script_commands(): zero_ftsensor() elif command == SET_PAYLOAD: mass = raw_command[2] / MULT_jointstate - cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate] + cog = [raw_command[3], raw_command[4], raw_command[5]] / MULT_jointstate set_payload(mass, cog) elif command == SET_TOOL_VOLTAGE: tool_voltage = raw_command[2] / MULT_jointstate @@ -331,6 +438,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 +461,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], params_mult[3], params_mult[4], params_mult[5], params_mult[6], 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], params_mult[3], params_mult[4], params_mult[5], params_mult[6], 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 @@ -378,10 +489,10 @@ while keepalive > 0 and control_mode > MODE_STOPPED: socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") end elif control_mode == MODE_SPEEDL: - twist = [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] + twist = [params_mult[2], params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7]] / MULT_jointstate set_speedl(twist) elif control_mode == MODE_POSE: - pose = p[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] + pose = p[params_mult[2], params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7]] / MULT_jointstate set_servo_pose(pose) elif control_mode == MODE_FREEDRIVE: if params_mult[2] == FREEDRIVE_MODE_START: diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 257422eb..5050bce7 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,86 @@ 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_QUINTIC; + + // 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) + { + for (auto const& vel : *velocities) + { + int32_t val = static_cast(vel * MULT_JOINTSTATE); + val = htobe32(val); + b_pos += append(b_pos, val); + } + } + else + { + // Write zeros if not velocity are given + const int32_t val(0); + for (size_t i = 0; i < positions->size(); ++i) + { + b_pos += append(b_pos, val); + } + } + + 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 + { + // Use cubic splines, when acceleration is not part of the trajectory + spline_type = control::TrajectorySplineType::SPLINE_CUBIC; + 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); }