28 #ifndef STOP_TRAJECTORY_BUILDER_H 29 #define STOP_TRAJECTORY_BUILDER_H 41 template<
class SegmentImpl>
89 template<
class SegmentImpl>
97 template<
class SegmentImpl>
100 const unsigned int number_of_joints =
hold_state_.position.size();
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)
126 Segment& segment {(*hold_traj)[joint_index].front()};
144 #endif // STOP_TRAJECTORY_BUILDER_H Builder creating a trajectory stopping the robot.
Segment::State hold_start_state_
const Segment::State & hold_state_
Stores a reference to the state where the stop motion shall start.
void setGoalHandle(RealtimeGoalHandlePtr rt_goal_handle)
Set the (realtime) goal handle associated to this segment.
Segment::State hold_end_state_
StopTrajectoryBuilder(const typename Segment::Time &stop_traj_duration, const typename Segment::State &hold_state)
Class representing a multi-dimensional quintic spline segment with a start and end time...
std::vector< TrajectoryPerJoint > Trajectory
std::vector< Segment > TrajectoryPerJoint
const Segment::Time stop_traj_duration_
Base class for classes used to construct diffent trajectory types.
bool buildTrajectory(Trajectory *hold_traj) override
Creates a trajectory which reaches the settle position in a fixed time.
RealtimeGoalHandlePtr createGoalHandlePtr() const
Return the set goal handle or a default one, if no goal handle was set.
const boost::optional< typename Segment::Time > & getStartTime() const
Return the set start time.