diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index f671440e..d0327fa9 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -22,10 +22,13 @@ MOVEIT_CLASS_FORWARD(InterfaceState); struct InterfaceState { InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) - : state(ps), - previous_trajectory(1, previous), - next_trajectory(1, next) - {} + : state(ps) + { + if( previous ) + previous_trajectory.push_back(previous); + if( next ) + next_trajectory.push_back(next); + } std::vector previous_trajectory; std::vector next_trajectory;