diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 95a6599191..e6f1c4b98c 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -63,6 +63,11 @@ start_with_holding (bool) Default: true +holding_position_open_loop: (bool) + If true, holding position is set to the last command instead of the current position from the hardware. + + Default: false + allow_nonzero_velocity_at_trajectory_end (boolean) If false, the last velocity point has to be zero or the goal will be rejected. diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e39bffc73..661a785e1d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1508,7 +1508,15 @@ std::shared_ptr JointTrajectoryController::set_hold_position() { // Command to stay at current position - hold_position_msg_ptr_->points[0].positions = state_current_.positions; + // Position states always exist + if (params_.holding_position_open_loop) + { + hold_position_msg_ptr_->points[0].positions = last_commanded_state_.positions; + } + else + { + hold_position_msg_ptr_->points[0].positions = state_current_.positions; + } // set flag, otherwise tolerances will be checked with holding position too rt_is_holding_.writeFromNonRT(true); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index ce17ba8bf9..51ea8aa2e9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -78,6 +78,11 @@ joint_trajectory_controller: default_value: true, description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", } + holding_position_open_loop: { + type: bool, + default_value: false, + description: "If true, holding position is set to the last command instead of the current position from the hardware.", + } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: true,