Skip to content

Commit

Permalink
[JTC] Continue with last trajectory-point on success (#842)
Browse files Browse the repository at this point in the history 
(cherry picked from commit e4f1700)
  • Loading branch information
@christophfroehlich
christophfroehlich committed Dec 5, 2023
1 parent 7d9079b commit 04ad4ca
Show file tree
Hide file tree
Showing 6 changed files with 162 additions and 76 deletions.
4 changes: 3 additions & 1 deletion joint_trajectory_controller/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,12 +150,14 @@ Actions [#f1]_
<controller_name>/follow_joint_trajectory [control_msgs::action::FollowJointTrajectory]
Action server for commanding the controller


The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired.

Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances.
When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`).
If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held.

The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances.

.. _Subscriber:

Subscriber [#f1]_
Expand Down
12 changes: 11 additions & 1 deletion ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

// Timeout to consider commands old
double cmd_timeout_;
// Are we holding position?
// True if holding position or repeating last trajectory point in case of success
realtime_tools::RealtimeBuffer<bool> rt_is_holding_;
// TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
bool subscriber_is_active_ = false;
Expand Down Expand Up @@ -244,9 +244,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void preempt_active_goal();

/** @brief set the current position with zero velocity and acceleration as new command
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_hold_position();

/** @brief set last trajectory point to be repeated at success
*
* no matter if it has nonzero velocity or acceleration
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_success_trajectory_point();

JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool reset();

Expand Down
16 changes: 15 additions & 1 deletion joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -371,7 +371,7 @@ controller_interface::return_type JointTrajectoryController::update(
RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
traj_msg_external_point_ptr_.initRT(set_success_trajectory_point());
}
else if (!within_goal_time)
{
Expand Down Expand Up @@ -1488,6 +1488,20 @@ JointTrajectoryController::set_hold_position()
return hold_position_msg_ptr_;
}

std::shared_ptr<trajectory_msgs::msg::JointTrajectory>
JointTrajectoryController::set_success_trajectory_point()
{
// set last command to be repeated at success, no matter if it has nonzero velocity or
// acceleration
hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back();
hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0);

// set flag, otherwise tolerances will be checked with success_trajectory_point too
rt_is_holding_.writeFromNonRT(true);

return hold_position_msg_ptr_;
}

bool JointTrajectoryController::contains_interface_type(
const std::vector<std::string> & interface_type_list, const std::string & interface_type)
{
Expand Down
136 changes: 103 additions & 33 deletions joint_trajectory_controller/test/test_trajectory_actions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,11 +68,12 @@ class TestTrajectoryActions : public TrajectoryControllerTest

void SetUpExecutor(
const std::vector<rclcpp::Parameter> & parameters = {},
bool separate_cmd_and_state_values = false, double kp = 0.0)
bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0)
{
setup_executor_ = true;

SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp);
SetUpAndActivateTrajectoryController(
executor_, parameters, separate_cmd_and_state_values, kp, ff);

SetUpActionClient();

Expand Down Expand Up @@ -218,7 +219,10 @@ class TestTrajectoryActionsTestParameterized

TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal)
{
SetUpExecutor();
// deactivate velocity tolerance
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)};
SetUpExecutor(params, false, 1.0, 0.0);
SetUpControllerHardware();

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
Expand All @@ -228,8 +232,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds(0.5);
point.positions.resize(joint_names_.size());

point.positions = point_positions;

points.push_back(point);
Expand All @@ -247,20 +249,53 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa
// it should be holding the last position goal
// i.e., active but trivial trajectory (one point only)
// note: the action goal also is a trivial trajectory
if (traj_controller_->has_position_command_interface())
{
expectHoldingPoint(point_positions);
}
else
expectCommandPoint(point_positions);
}

TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal)
{
// deactivate velocity tolerance and allow velocity at trajectory end
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0),
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
SetUpExecutor(params, false, 1.0, 0.0);
SetUpControllerHardware();

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
std::vector<double> point_positions{1.0, 2.0, 3.0};
std::vector<double> point_velocities{1.0, 1.0, 1.0};
{
// no integration to position state interface from velocity/acceleration
expectHoldingPoint(INITIAL_POS_JOINTS);
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds(0.5);
point.positions = point_positions;
point.velocities = point_velocities;

points.push_back(point);

gh_future = sendActionGoal(points, 1.0, goal_options_);
}
controller_hw_thread_.join();

EXPECT_TRUE(gh_future.get());
EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);

// run an update
updateControllerAsync(rclcpp::Duration::from_seconds(0.01));

// it should be holding the last position goal
// i.e., active but trivial trajectory (one point only)
// note: the action goal also is a trivial trajectory
expectCommandPoint(point_positions, point_velocities);
}

TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal)
{
SetUpExecutor();
// deactivate velocity tolerance
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)};
SetUpExecutor({params}, false, 1.0, 0.0);
SetUpControllerHardware();

// add feedback
Expand All @@ -277,15 +312,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point1;
point1.time_from_start = rclcpp::Duration::from_seconds(0.2);
point1.positions.resize(joint_names_.size());

point1.positions = points_positions.at(0);
points.push_back(point1);

JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds(0.3);
point2.positions.resize(joint_names_.size());

point2.positions = points_positions.at(1);
points.push_back(point2);

Expand All @@ -302,15 +333,57 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal

// it should be holding the last position goal
// i.e., active but trivial trajectory (one point only)
if (traj_controller_->has_position_command_interface())
{
expectHoldingPoint(points_positions.at(1));
}
else
expectCommandPoint(points_positions.at(1));
}

TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal)
{
// deactivate velocity tolerance and allow velocity at trajectory end
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0),
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
SetUpExecutor(params, false, 1.0, 0.0);
SetUpControllerHardware();

// add feedback
bool feedback_recv = false;
goal_options_.feedback_callback =
[&](
rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>::SharedPtr,
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback>) { feedback_recv = true; };

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};
std::vector<std::vector<double>> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}};
{
// no integration to position state interface from velocity/acceleration
expectHoldingPoint(INITIAL_POS_JOINTS);
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point1;
point1.time_from_start = rclcpp::Duration::from_seconds(0.2);
point1.positions = points_positions.at(0);
point1.velocities = points_velocities.at(0);
points.push_back(point1);

JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds(0.3);
point2.positions = points_positions.at(1);
point2.velocities = points_velocities.at(1);
points.push_back(point2);

gh_future = sendActionGoal(points, 1.0, goal_options_);
}
controller_hw_thread_.join();

EXPECT_TRUE(feedback_recv);
EXPECT_TRUE(gh_future.get());
EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);

// run an update
updateControllerAsync(rclcpp::Duration::from_seconds(0.01));

// it should be holding the last position goal
// i.e., active but trivial trajectory (one point only)
expectCommandPoint(points_positions.at(1), points_velocities.at(1));
}

/**
Expand Down Expand Up @@ -354,7 +427,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success)

// it should be holding the last position goal
// i.e., active but trivial trajectory (one point only)
expectHoldingPoint(points_positions.at(0));
expectCommandPoint(points_positions.at(0));
}

/**
Expand Down Expand Up @@ -413,7 +486,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success)

// it should be holding the last position goal
// i.e., active but trivial trajectory (one point only)
expectHoldingPoint(points_positions.at(1));
expectCommandPoint(points_positions.at(1));
}

TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail)
Expand Down Expand Up @@ -464,8 +537,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail)

// it should be holding the position (being the initial one)
// i.e., active but trivial trajectory (one point only)
std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
expectHoldingPoint(initial_positions);
expectCommandPoint(INITIAL_POS_JOINTS);
}

TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail)
Expand Down Expand Up @@ -513,8 +585,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail)

// it should be holding the position (being the initial one)
// i.e., active but trivial trajectory (one point only)
std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
expectHoldingPoint(initial_positions);
expectCommandPoint(INITIAL_POS_JOINTS);
}

TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail)
Expand Down Expand Up @@ -559,8 +630,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol

// it should be holding the position (being the initial one)
// i.e., active but trivial trajectory (one point only)
std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
expectHoldingPoint(initial_positions);
expectCommandPoint(INITIAL_POS_JOINTS);
}

TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position)
Expand Down Expand Up @@ -607,7 +677,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position)

// it should be holding the last position,
// i.e., active but trivial trajectory (one point only)
expectHoldingPoint(cancelled_position);
expectCommandPoint(cancelled_position);
}

TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true)
Expand Down
14 changes: 7 additions & 7 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -248,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
// it should still be holding the position at time of deactivation
// i.e., active but trivial trajectory (one point only)
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
expectHoldingPoint(deactivated_positions);
expectCommandPoint(deactivated_positions);

executor.cancel();
}
Expand Down Expand Up @@ -478,7 +478,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
ASSERT_TRUE(traj_controller_->has_active_traj());
// one point, being the position at startup
std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
expectHoldingPoint(initial_positions);
expectCommandPoint(initial_positions);

executor.cancel();
}
Expand Down Expand Up @@ -838,12 +838,12 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
// should hold last position with zero velocity
if (traj_controller_->has_position_command_interface())
{
expectHoldingPoint(points.at(2));
expectCommandPoint(points.at(2));
}
else
{
// no integration to position state interface from velocity/acceleration
expectHoldingPoint(INITIAL_POS_JOINTS);
expectCommandPoint(INITIAL_POS_JOINTS);
}

executor.cancel();
Expand Down Expand Up @@ -1814,7 +1814,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail)
updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));

// it should have aborted and be holding now
expectHoldingPoint(joint_state_pos_);
expectCommandPoint(joint_state_pos_);
}

TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
Expand Down Expand Up @@ -1846,14 +1846,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME));

// it should have aborted and be holding now
expectHoldingPoint(joint_state_pos_);
expectCommandPoint(joint_state_pos_);

// what happens if we wait longer but it harms the tolerance again?
auto hold_position = joint_state_pos_;
joint_state_pos_.at(0) = -3.3;
updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time);
// it should be still holding the old point
expectHoldingPoint(hold_position);
expectCommandPoint(hold_position);
}

// position controllers
Expand Down
Loading

0 comments on commit 04ad4ca

Please sign in to comment.