diff --git a/doc/architecture/trajectory_point_interface.rst b/doc/architecture/trajectory_point_interface.rst index f16900d84..43c518716 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -57,11 +57,11 @@ representations in 21 datafields. The data fields have the following meaning: - For all MOVEC variants this field contains the via point (same joint-vs-pose interpretation as the target at indices 0-5, see the motion type at index 20). - - trajectory point velocities (multiplied by ``MULT_JOINTSTATE``) for spline joint types + - trajectory point velocities (multiplied by ``MULT_VEL_ACC``) for spline joint types 12-17 Depending on the motion type, this represents either - - trajectory point accelerations (multiplied by ``MULT_JOINTSTATE``) for spline joint + - trajectory point accelerations (multiplied by ``MULT_VEL_ACC``) for spline joint types. - for all other motion types @@ -112,12 +112,19 @@ where - ``MULT_JOINTSTATE``: 1000000 - ``MULT_TIME``: 1000000 +- ``MULT_VEL_ACC``: 100000000 .. note:: With ``MULT_TIME`` being 1000000, the maximum duration that can be sent is 2147 seconds, while precision is cut off at 1 microsecond. (The same applies to the blend radius, respectively being max 2147 m and 1 μm precision.) +.. note:: + Spline point velocities and accelerations use the finer ``MULT_VEL_ACC`` scaling, giving 1e-8 + resolution with a maximum magnitude of ~21.47 rad/s (rad/s²). The coarser ``MULT_JOINTSTATE`` + scaling quantizes near-zero accelerations, causing reconstructed acceleration + profile to become jagged, which can trigger controller faults. Values exceeding the maximum magnitude are rejected by the library. + .. note:: The ``*_POSE`` / ``*_JOINT`` motion-type variants let callers mix joint-space and Cartesian targets freely through the high-level APIs (see :ref:`instruction_executor` and the diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 26de4d89f..629bcbe26 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -63,6 +63,8 @@ std::string trajectoryResultToString(const TrajectoryResult result); class TrajectoryPointInterface : public ReverseInterface { public: + // Spline vel/acc need finer resolution: quantising near-zero values at 1e-6 causes controller faults. + static const int32_t MULT_VEL_ACC = 100000000; static const int MESSAGE_LENGTH = 21; TrajectoryPointInterface() = delete; @@ -148,6 +150,7 @@ class TrajectoryPointInterface : public ReverseInterface private: const double MAX_GOAL_TIME_ = static_cast(std::numeric_limits::max()) / MULT_TIME; + const double MAX_VEL_ACC_ = static_cast(std::numeric_limits::max()) / MULT_VEL_ACC; std::list> trajectory_end_callbacks_; uint32_t next_done_callback_id_ = 0; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 3423bbe52..083b5f94f 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -7,6 +7,7 @@ steptime = get_steptime() textmsg("ExternalControl: steptime=", steptime) MULT_jointstate = {{JOINT_STATE_REPLACE}} MULT_time = {{TIME_REPLACE}} +MULT_velacc = {{VEL_ACC_REPLACE}} DEBUG = False @@ -668,7 +669,7 @@ thread trajectoryThread(): # 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] + qd = [ raw_point[7] / MULT_velacc, raw_point[8] / MULT_velacc, raw_point[9] / MULT_velacc, raw_point[10] / MULT_velacc, raw_point[11] / MULT_velacc, raw_point[12] / MULT_velacc] is_robot_moving = cubicSplineRun(q, qd, tmptime, is_last_point, is_first_point) # reset old acceleration @@ -676,8 +677,8 @@ thread trajectoryThread(): # 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] + qd = [ raw_point[7] / MULT_velacc, raw_point[8] / MULT_velacc, raw_point[9] / MULT_velacc, raw_point[10] / MULT_velacc, raw_point[11] / MULT_velacc, raw_point[12] / MULT_velacc] + qdd = [ raw_point[13]/ MULT_velacc, raw_point[14]/ MULT_velacc, raw_point[15]/ MULT_velacc, raw_point[16]/ MULT_velacc, raw_point[17]/ MULT_velacc, raw_point[18]/ MULT_velacc] is_robot_moving = quinticSplineRun(q, qd, qdd, tmptime, is_last_point, is_first_point) else: textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 83ca2c8cb..7c5053417 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include #include @@ -97,6 +99,23 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtype == MotionType::SPLINE) + { + auto spline_primitive = std::static_pointer_cast(primitive); + auto within_range = [this](const vector6d_t& values) { + return std::all_of(values.begin(), values.end(), + [this](const double value) { return std::abs(value) <= MAX_VEL_ACC_; }); + }; + if (!within_range(spline_primitive->target_velocities) || + (spline_primitive->target_accelerations.has_value() && + !within_range(spline_primitive->target_accelerations.value()))) + { + URCL_LOG_ERROR("Spline point velocity or acceleration out of range. Maximum allowed magnitude is %f.", + MAX_VEL_ACC_); + return false; + } + } + if (client_fd_ == -1) { return false; @@ -180,24 +199,18 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(round(pos * MULT_JOINTSTATE)); - buffer[index] = htobe32(val); - index++; - } - for (auto const& item : second_block) - { - int32_t val = static_cast(round(item * MULT_JOINTSTATE)); - buffer[index] = htobe32(val); - index++; - } - for (auto const& item : third_block) - { - int32_t val = static_cast(round(item * MULT_JOINTSTATE)); - buffer[index] = htobe32(val); - index++; - } + auto write_block = [&buffer, &index](const vector6d_t& block, const int32_t mult) { + for (auto const& item : block) + { + buffer[index++] = htobe32(static_cast(round(item * mult))); + } + }; + + // Only spline points carry per-sample vel/acc in the second and third block. + const int32_t vel_acc_mult = primitive->type == MotionType::SPLINE ? MULT_VEL_ACC : MULT_JOINTSTATE; + write_block(first_block, MULT_JOINTSTATE); + write_block(second_block, vel_acc_mult); + write_block(third_block, vel_acc_mult); int32_t val = static_cast(round(primitive->duration.count() * MULT_TIME)); buffer[index] = htobe32(val); diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 69352ac50..d65bd4c46 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -49,6 +49,7 @@ namespace urcl static const std::string BEGIN_REPLACE("BEGIN_REPLACE"); static const std::string JOINT_STATE_REPLACE("JOINT_STATE_REPLACE"); static const std::string TIME_REPLACE("TIME_REPLACE"); +static const std::string VEL_ACC_REPLACE("VEL_ACC_REPLACE"); static const std::string SERVO_J_REPLACE("SERVO_J_REPLACE"); static const std::string SERVER_IP_REPLACE("SERVER_IP_REPLACE"); static const std::string SERVER_PORT_REPLACE("SERVER_PORT_REPLACE"); @@ -111,6 +112,7 @@ void UrDriver::init(const UrDriverConfiguration& config) control::ScriptReader::DataDict data; data[JOINT_STATE_REPLACE] = std::to_string(control::ReverseInterface::MULT_JOINTSTATE); data[TIME_REPLACE] = std::to_string(control::TrajectoryPointInterface::MULT_TIME); + data[VEL_ACC_REPLACE] = std::to_string(control::TrajectoryPointInterface::MULT_VEL_ACC); std::ostringstream out; out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; data[SERVO_J_REPLACE] = out.str(); diff --git a/tests/test_script_reader.cpp b/tests/test_script_reader.cpp index 9ce37bd83..09e9ca3b1 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -460,6 +460,7 @@ TEST_F(ScriptReaderTest, TestParsingExternalControl) data["BEGIN_REPLACE"] = ""; data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; data["SERVER_IP_REPLACE"] = "1.2.3.4"; data["SERVER_PORT_REPLACE"] = "50001"; @@ -500,6 +501,7 @@ TEST_F(ScriptReaderTest, TestDirectTorquePopupOnOldVersion) data["BEGIN_REPLACE"] = ""; data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; data["SERVER_IP_REPLACE"] = "1.2.3.4"; data["SERVER_PORT_REPLACE"] = "50001"; @@ -527,6 +529,7 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope523 data["BEGIN_REPLACE"] = ""; data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; data["SERVER_IP_REPLACE"] = "1.2.3.4"; data["SERVER_PORT_REPLACE"] = "50001"; @@ -557,6 +560,7 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope101 data["BEGIN_REPLACE"] = ""; data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; data["SERVER_IP_REPLACE"] = "1.2.3.4"; data["SERVER_PORT_REPLACE"] = "50001"; @@ -588,6 +592,7 @@ TEST_F(ScriptReaderTest, TestFrictionScalesConstantsAndHandler) data["BEGIN_REPLACE"] = ""; data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; data["SERVER_IP_REPLACE"] = "1.2.3.4"; data["SERVER_PORT_REPLACE"] = "50001"; @@ -861,6 +866,7 @@ TEST_F(ScriptReaderTest, TestProduceAllScriptFiles) data["BEGIN_REPLACE"] = ""; data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; data["SERVER_IP_REPLACE"] = "1.2.3.4"; data["SERVER_PORT_REPLACE"] = "50001"; diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index f648eac78..93581489d 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -429,20 +429,20 @@ TEST_F(TrajectoryPointInterfaceTest, write_quintic_joint_spline) 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); + EXPECT_EQ(send_vel[0], ((double)received_data.vel[0]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[1], ((double)received_data.vel[1]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[2], ((double)received_data.vel[2]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[3], ((double)received_data.vel[3]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[4], ((double)received_data.vel[4]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[5], ((double)received_data.vel[5]) / traj_point_interface_->MULT_VEL_ACC); // 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); + EXPECT_EQ(send_acc[0], ((double)received_data.acc[0]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[1], ((double)received_data.acc[1]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[2], ((double)received_data.acc[2]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[3], ((double)received_data.acc[3]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[4], ((double)received_data.acc[4]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_VEL_ACC); // Goal time (segment duration, ``MULT_TIME`` on the wire) EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_TIME)); @@ -473,20 +473,20 @@ TEST_F(TrajectoryPointInterfaceTest, write_cubic_joint_spline) 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); + EXPECT_EQ(send_vel[0], ((double)received_data.vel[0]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[1], ((double)received_data.vel[1]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[2], ((double)received_data.vel[2]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[3], ((double)received_data.vel[3]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[4], ((double)received_data.vel[4]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[5], ((double)received_data.vel[5]) / traj_point_interface_->MULT_VEL_ACC); // 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); + EXPECT_EQ(send_acc[0], ((double)received_data.acc[0]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[1], ((double)received_data.acc[1]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[2], ((double)received_data.acc[2]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[3], ((double)received_data.acc[3]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[4], ((double)received_data.acc[4]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_VEL_ACC); // Goal time EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_TIME); @@ -508,12 +508,12 @@ TEST_F(TrajectoryPointInterfaceTest, write_splines_velocities) 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); + EXPECT_EQ(send_vel[0], ((double)received_velocities[0]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[1], ((double)received_velocities[1]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[2], ((double)received_velocities[2]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[3], ((double)received_velocities[3]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[4], ((double)received_velocities[4]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[5], ((double)received_velocities[5]) / traj_point_interface_->MULT_VEL_ACC); } TEST_F(TrajectoryPointInterfaceTest, write_splines_accelerations) @@ -525,12 +525,12 @@ TEST_F(TrajectoryPointInterfaceTest, write_splines_accelerations) 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); + EXPECT_EQ(send_vel[0], ((double)received_velocities[0]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[1], ((double)received_velocities[1]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[2], ((double)received_velocities[2]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[3], ((double)received_velocities[3]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[4], ((double)received_velocities[4]) / traj_point_interface_->MULT_VEL_ACC); + EXPECT_EQ(send_vel[5], ((double)received_velocities[5]) / traj_point_interface_->MULT_VEL_ACC); } TEST_F(TrajectoryPointInterfaceTest, write_goal_time) @@ -598,6 +598,43 @@ TEST_F(TrajectoryPointInterfaceTest, write_rejects_goal_time_above_max_encodable almost_too_long_goal_time)); } +// Near-zero spline vel/acc must survive the int32 roundtrip instead of collapsing to zero. +TEST_F(TrajectoryPointInterfaceTest, write_spline_preserves_near_zero_velocity_and_acceleration) +{ + urcl::vector6d_t send_pos = { 0, 0, 0, 0, 0, 0 }; + urcl::vector6d_t send_vel = { 1e-5, -1e-5, 3.4e-6, -3.4e-6, 9.9e-5, -9.9e-5 }; + urcl::vector6d_t send_acc = { 2.3e-5, -2.3e-5, 7e-6, -7e-6, 5.5e-5, -5.5e-5 }; + traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &send_vel, &send_acc, 0.02f); + Client::TrajData received_data = client_->getData(); + + const double resolution = 1.0 / control::TrajectoryPointInterface::MULT_VEL_ACC; + for (size_t i = 0; i < 6; ++i) + { + EXPECT_NE(0, received_data.vel[i]); + EXPECT_NE(0, received_data.acc[i]); + EXPECT_NEAR(send_vel[i], static_cast(received_data.vel[i]) / control::TrajectoryPointInterface::MULT_VEL_ACC, + resolution / 2); + EXPECT_NEAR(send_acc[i], static_cast(received_data.acc[i]) / control::TrajectoryPointInterface::MULT_VEL_ACC, + resolution / 2); + } +} + +// Spline velocities and accelerations are capped by int32 range at MULT_VEL_ACC resolution. +TEST_F(TrajectoryPointInterfaceTest, write_rejects_spline_velocity_or_acceleration_above_max_encodable) +{ + const double max_vel_acc = static_cast(std::numeric_limits::max()) / + static_cast(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); + + urcl::vector6d_t send_pos = { 0, 0, 0, 0, 0, 0 }; + urcl::vector6d_t in_range = { 0, 0, 0, 0, 0, max_vel_acc - 1.0 }; + urcl::vector6d_t out_of_range = { 0, 0, 0, 0, 0, -(max_vel_acc + 1.0) }; + + EXPECT_FALSE(traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &out_of_range, &in_range, 0.02f)); + EXPECT_FALSE(traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &in_range, &out_of_range, 0.02f)); + EXPECT_TRUE(traj_point_interface_->writeTrajectorySplinePoint(&send_pos, &in_range, &in_range, 0.02f)); + client_->getData(); +} + TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity) { urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };