28 #ifndef STOP_TRAJECTORY_BUILDER_H
29 #define STOP_TRAJECTORY_BUILDER_H
41 template<
class SegmentImpl>
89 template<
class SegmentImpl>
92 : stop_traj_duration_(stop_traj_duration)
93 , hold_state_(hold_state)
97 template<
class SegmentImpl>
100 const unsigned int number_of_joints = hold_state_.position.size();
114 const typename Segment::Time end_time {start_time + stop_traj_duration_};
115 const typename Segment::Time end_time_2x {start_time + 2.0 * stop_traj_duration_};
116 for (
unsigned int joint_index = 0; joint_index < number_of_joints; ++joint_index)
118 hold_start_state_.position[0] = hold_state_.position[joint_index];
119 hold_start_state_.velocity[0] = hold_state_.velocity[joint_index];
120 hold_start_state_.acceleration[0] = 0.0;
122 hold_end_state_.position[0] = hold_state_.position[joint_index];
123 hold_end_state_.velocity[0] = -hold_state_.velocity[joint_index];
124 hold_end_state_.acceleration[0] = 0.0;
126 Segment& segment {(*hold_traj)[joint_index].front()};
127 segment.init(start_time, hold_start_state_,
128 end_time_2x, hold_end_state_);
130 segment.sample(end_time, hold_end_state_);
132 segment.init(start_time, hold_start_state_,
133 end_time, hold_end_state_);
144 #endif // STOP_TRAJECTORY_BUILDER_H