From 8a9e3db724eb544c4586cf1a1885a11d527692b3 Mon Sep 17 00:00:00 2001 From: Joachim Schleicher Date: Mon, 5 Mar 2018 15:58:45 +0100 Subject: [PATCH] preserve jump, if stop_trajectory_duration==0 old behaviour for stop_trajectory_duration==0 --- .../joint_trajectory_controller_impl.h | 77 +++++++++++-------- 1 file changed, 47 insertions(+), 30 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 75fa64412..6e6e5c629 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -743,47 +743,64 @@ template void JointTrajectoryController:: setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh) { - // Settle position in a fixed time. We do the following: - // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time - // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity - // - Create segment that goes from current state to above zero velocity state, in the desired time - // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types - assert(joint_names_.size() == hold_trajectory_ptr_->size()); typename Segment::State hold_start_state_ = typename Segment::State(1); typename Segment::State hold_end_state_ = typename Segment::State(1); - - const typename Segment::Time start_time = time.toSec(); - const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_; - const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_; - - // Create segment that goes from current (pos,vel) to (pos,-vel) const unsigned int n_joints = joints_.size(); - for (unsigned int i = 0; i < n_joints; ++i) + + if(stop_trajectory_duration_ == 0.0) + { + // stop at current actual position + for (unsigned int i = 0; i < n_joints; ++i) + { + hold_start_state_.position[0] = joints_[i].getPosition(); + hold_start_state_.velocity[0] = 0.0; + hold_start_state_.acceleration[0] = 0.0; + (*hold_trajectory_ptr_)[i].front().init(time.toSec(), hold_start_state_, + time.toSec(), hold_start_state_); + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } + } + else { - // If there is a time delay in the system it is better to use calculate the hold trajectory starting from the - // desired position. Otherwise there would be a jerk in the motion. - hold_start_state_.position[0] = desired_state_.position[i]; - hold_start_state_.velocity[0] = desired_state_.velocity[i]; - hold_start_state_.acceleration[0] = 0.0; + // Settle position in a fixed time. We do the following: + // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time + // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity + // - Create segment that goes from current state to above zero velocity state, in the desired time + // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types - hold_end_state_.position[0] = desired_state_.position[i]; - hold_end_state_.velocity[0] = -desired_state_.velocity[i]; - hold_end_state_.acceleration[0] = 0.0; + const typename Segment::Time start_time = time.toSec(); + const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_; + const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_; - (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, - end_time_2x, hold_end_state_); + // Create segment that goes from current (pos,vel) to (pos,-vel) + for (unsigned int i = 0; i < n_joints; ++i) + { + // If there is a time delay in the system it is better to calculate the hold trajectory starting from the + // desired position. Otherwise there would be a jerk in the motion. + hold_start_state_.position[0] = desired_state_.position[i]; + hold_start_state_.velocity[0] = desired_state_.velocity[i]; + hold_start_state_.acceleration[0] = 0.0; - // Sample segment at its midpoint, that should have zero velocity - (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_); + hold_end_state_.position[0] = desired_state_.position[i]; + hold_end_state_.velocity[0] = -desired_state_.velocity[i]; + hold_end_state_.acceleration[0] = 0.0; - // Now create segment that goes from current state to one with zero end velocity - (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, - end_time, hold_end_state_); + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + end_time_2x, hold_end_state_); - // Set goal handle for the segment - (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + // Sample segment at its midpoint, that should have zero velocity + (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_); + + // Now create segment that goes from current state to one with zero end velocity + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + end_time, hold_end_state_); + + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } } curr_trajectory_box_.set(hold_trajectory_ptr_); }