45 std::vector<robot_trajectory::RobotTrajectoryPtr> res_vec{ traj_cont_ };
48 assert(!res_vec.empty());
49 appendWithStrictTimeIncrease(*(res_vec.back()), *traj_tail_);
61 result.
append(source, 0.0);
71 void PlanComponentsBuilder::blend(
const planning_scene::PlanningSceneConstPtr&
planning_scene,
72 const robot_trajectory::RobotTrajectoryPtr& other,
const double blend_radius)
76 throw NoBlenderSetException(
"No blender set");
79 assert(other->getGroupName() == traj_tail_->getGroupName());
86 blend_request.
group_name = traj_tail_->getGroupName();
87 blend_request.
link_name = getSolverTipFrame(model_->getJointModelGroup(blend_request.
group_name));
90 if (!blender_->blend(
planning_scene, blend_request, blend_response))
92 throw BlendingFailedException(
"Blending failed");
96 appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.
first_trajectory);
103 const robot_trajectory::RobotTrajectoryPtr& other,
const double blend_radius)
107 throw NoRobotModelSetException(
"No robot model set");
114 traj_cont_.emplace_back(std::make_shared<robot_trajectory::RobotTrajectory>(model_, other->getGroupName()));
119 if (other->getGroupName() != traj_tail_->getGroupName())
121 appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_);
124 traj_cont_.emplace_back(std::make_shared<robot_trajectory::RobotTrajectory>(model_, other->getGroupName()));
129 if (blend_radius <= 0.0)
131 appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_);
std::vector< robot_trajectory::RobotTrajectoryPtr > build() const
void append(const planning_scene::PlanningSceneConstPtr &planning_scene, const robot_trajectory::RobotTrajectoryPtr &other, const double blend_radius)
Appends the specified trajectory to the trajectory container under construction.
Maintain a sequence of waypoints and the time durations between these waypoints.
const std::string & getGroupName() const
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
const moveit::core::RobotState & getFirstWayPoint() const
RobotTrajectory & append(const RobotTrajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
Add a specified part of a trajectory to the end of the current trajectory. The default (when start_in...
std::size_t getWayPointCount() const
double getWayPointDurationFromPrevious(std::size_t index) const
const moveit::core::RobotState & getWayPoint(std::size_t index) const
const moveit::core::RobotState & getLastWayPoint() const
bool isRobotStateEqual(const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const std::string &joint_group_name, double epsilon)
Check if the two robot states have the same joint position/velocity/acceleration.
This namespace includes the central class for representing planning contexts.
robot_trajectory::RobotTrajectoryPtr second_trajectory
robot_trajectory::RobotTrajectoryPtr first_trajectory
robot_trajectory::RobotTrajectoryPtr first_trajectory
robot_trajectory::RobotTrajectoryPtr blend_trajectory
robot_trajectory::RobotTrajectoryPtr second_trajectory