Skip to content

Commit 99b67d7

Browse files
christophfroehlichbmagyar
christophfroehlich
and
bmagyar
committed
[JTC] Reject trajectories with nonzero terminal velocity (#567)
* Reject receiving trajectory of last velocity point is non-zero * Update docs * Add tests * Change to parameterized test * Rename parameter * not true -> false --------- Co-authored-by: Bence Magyar <[email protected]>
1 parent 12d46f2 commit 99b67d7

5 files changed

Lines changed: 170 additions & 3 deletions

File tree

‎joint_trajectory_controller/doc/userdoc.rst‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -175,6 +175,11 @@ open_loop_control (boolean)
175175

176176
Default: false
177177

178+
allow_nonzero_velocity_at_trajectory_end (boolean)
179+
If false, the last velocity point has to be zero or the goal will be rejected.
180+
181+
Default: true
182+
178183
constraints (structure)
179184
Default values for tolerances if no explicit values are states in JointTrajectory message.
180185

‎joint_trajectory_controller/src/joint_trajectory_controller.cpp‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
6464
return CallbackReturn::ERROR;
6565
}
6666

67+
// TODO(christophfroehlich): remove deprecation warning
68+
if (params_.allow_nonzero_velocity_at_trajectory_end)
69+
{
70+
RCLCPP_WARN(
71+
get_node()->get_logger(),
72+
"[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to "
73+
"true. The default behavior will change to false.");
74+
}
75+
6776
return CallbackReturn::SUCCESS;
6877
}
6978

@@ -1321,6 +1330,20 @@ bool JointTrajectoryController::validate_trajectory_msg(
13211330
}
13221331
}
13231332

1333+
if (!params_.allow_nonzero_velocity_at_trajectory_end)
1334+
{
1335+
for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
1336+
{
1337+
if (trajectory.points.back().velocities.at(i) != 0.)
1338+
{
1339+
RCLCPP_ERROR(
1340+
get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f",
1341+
trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
1342+
return false;
1343+
}
1344+
}
1345+
}
1346+
13241347
rclcpp::Duration previous_traj_time(0ms);
13251348
for (size_t i = 0; i < trajectory.points.size(); ++i)
13261349
{

‎joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,11 @@ joint_trajectory_controller:
6666
one_of<>: [["splines", "none"]],
6767
}
6868
}
69+
allow_nonzero_velocity_at_trajectory_end: {
70+
type: bool,
71+
default_value: true,
72+
description: "If false, the last velocity point has to be zero or the goal will be rejected",
73+
}
6974
gains:
7075
__map_joints:
7176
p: {

‎joint_trajectory_controller/test/test_trajectory_actions.cpp‎

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -616,6 +616,130 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position)
616616
}
617617
}
618618

619+
TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true)
620+
{
621+
std::vector<rclcpp::Parameter> params = {
622+
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
623+
SetUpExecutor(params);
624+
SetUpControllerHardware();
625+
626+
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
627+
// send goal with nonzero last velocities
628+
{
629+
std::vector<JointTrajectoryPoint> points;
630+
JointTrajectoryPoint point1;
631+
point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
632+
point1.positions.resize(joint_names_.size());
633+
point1.velocities.resize(joint_names_.size());
634+
635+
point1.positions[0] = 4.0;
636+
point1.positions[1] = 5.0;
637+
point1.positions[2] = 6.0;
638+
point1.velocities[0] = 4.0;
639+
point1.velocities[1] = 5.0;
640+
point1.velocities[2] = 6.0;
641+
points.push_back(point1);
642+
643+
JointTrajectoryPoint point2;
644+
point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
645+
point2.positions.resize(joint_names_.size());
646+
point2.velocities.resize(joint_names_.size());
647+
648+
point2.positions[0] = 7.0;
649+
point2.positions[1] = 8.0;
650+
point2.positions[2] = 9.0;
651+
point2.velocities[0] = 4.0;
652+
point2.velocities[1] = 5.0;
653+
point2.velocities[2] = 6.0;
654+
points.push_back(point2);
655+
656+
gh_future = sendActionGoal(points, 1.0, goal_options_);
657+
}
658+
controller_hw_thread_.join();
659+
660+
// will be accepted despite nonzero last point
661+
EXPECT_TRUE(gh_future.get());
662+
EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
663+
}
664+
665+
TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false)
666+
{
667+
std::vector<rclcpp::Parameter> params = {
668+
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)};
669+
SetUpExecutor(params);
670+
SetUpControllerHardware();
671+
672+
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
673+
// send goal with nonzero last velocities
674+
{
675+
std::vector<JointTrajectoryPoint> points;
676+
JointTrajectoryPoint point1;
677+
point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
678+
point1.positions.resize(joint_names_.size());
679+
point1.velocities.resize(joint_names_.size());
680+
681+
point1.positions[0] = 4.0;
682+
point1.positions[1] = 5.0;
683+
point1.positions[2] = 6.0;
684+
point1.velocities[0] = 4.0;
685+
point1.velocities[1] = 5.0;
686+
point1.velocities[2] = 6.0;
687+
points.push_back(point1);
688+
689+
JointTrajectoryPoint point2;
690+
point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
691+
point2.positions.resize(joint_names_.size());
692+
point2.velocities.resize(joint_names_.size());
693+
694+
point2.positions[0] = 7.0;
695+
point2.positions[1] = 8.0;
696+
point2.positions[2] = 9.0;
697+
point2.velocities[0] = 4.0;
698+
point2.velocities[1] = 5.0;
699+
point2.velocities[2] = 6.0;
700+
points.push_back(point2);
701+
702+
gh_future = sendActionGoal(points, 1.0, goal_options_);
703+
}
704+
controller_hw_thread_.join();
705+
706+
EXPECT_FALSE(gh_future.get());
707+
708+
// send goal with last velocity being zero
709+
{
710+
std::vector<JointTrajectoryPoint> points;
711+
JointTrajectoryPoint point1;
712+
point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
713+
point1.positions.resize(joint_names_.size());
714+
point1.velocities.resize(joint_names_.size());
715+
716+
point1.positions[0] = 4.0;
717+
point1.positions[1] = 5.0;
718+
point1.positions[2] = 6.0;
719+
point1.velocities[0] = 4.0;
720+
point1.velocities[1] = 5.0;
721+
point1.velocities[2] = 6.0;
722+
points.push_back(point1);
723+
724+
JointTrajectoryPoint point2;
725+
point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
726+
point2.positions.resize(joint_names_.size());
727+
point2.velocities.resize(joint_names_.size());
728+
729+
point2.positions[0] = 7.0;
730+
point2.positions[1] = 8.0;
731+
point2.positions[2] = 9.0;
732+
point2.velocities[0] = 0.0;
733+
point2.velocities[1] = 0.0;
734+
point2.velocities[2] = 0.0;
735+
points.push_back(point2);
736+
737+
gh_future = sendActionGoal(points, 1.0, goal_options_);
738+
}
739+
740+
EXPECT_TRUE(gh_future.get());
741+
}
742+
619743
// position controllers
620744
INSTANTIATE_TEST_SUITE_P(
621745
PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized,

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,8 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
6565
{
6666
rclcpp::executors::MultiThreadedExecutor executor;
6767
SetUpTrajectoryController(executor);
68+
traj_controller_->get_node()->set_parameter(
69+
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
6870

6971
// const auto future_handle_ = std::async(std::launch::async, spin, &executor);
7072

@@ -202,7 +204,9 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
202204
TEST_P(TrajectoryControllerTestParameterized, cleanup)
203205
{
204206
rclcpp::executors::MultiThreadedExecutor executor;
205-
SetUpAndActivateTrajectoryController(executor);
207+
std::vector<rclcpp::Parameter> params = {
208+
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
209+
SetUpAndActivateTrajectoryController(executor, true, params);
206210

207211
// send msg
208212
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
@@ -259,6 +263,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
259263
{
260264
rclcpp::executors::MultiThreadedExecutor executor;
261265
SetUpTrajectoryController(executor, false);
266+
traj_controller_->get_node()->set_parameter(
267+
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
262268

263269
// This call is replacing the way parameters are set via launch
264270
SetParameters();
@@ -450,7 +456,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
450456
{
451457
rclcpp::executors::MultiThreadedExecutor executor;
452458
constexpr double k_p = 10.0;
453-
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false);
459+
std::vector<rclcpp::Parameter> params = {
460+
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
461+
SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false);
454462
subscribeToState();
455463

456464
size_t n_joints = joint_names_.size();
@@ -557,7 +565,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
557565
{
558566
rclcpp::executors::MultiThreadedExecutor executor;
559567
constexpr double k_p = 10.0;
560-
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true);
568+
std::vector<rclcpp::Parameter> params = {
569+
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
570+
SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true);
561571
subscribeToState();
562572

563573
size_t n_joints = joint_names_.size();

0 commit comments

Comments
 (0)