Trajectory blender implementing transition window algorithm. More...

#include <trajectory_blender_transition_window.h>

Inheritance diagram for pilz::TrajectoryBlenderTransitionWindow:

Public Member Functions
virtual bool	blend (const pilz::TrajectoryBlendRequest &req, pilz::TrajectoryBlendResponse &res) override
	Blend two trajectories using transition window. The trajectories have to be equally and uniformly discretized. More...

	TrajectoryBlenderTransitionWindow (const LimitsContainer &planner_limits)

virtual	~TrajectoryBlenderTransitionWindow ()

Public Member Functions inherited from pilz::TrajectoryBlender
	TrajectoryBlender (const pilz::LimitsContainer &planner_limits)

virtual	~TrajectoryBlender ()

Private Member Functions
void	blendTrajectoryCartesian (const pilz::TrajectoryBlendRequest &req, const std::size_t first_interse_index, const std::size_t second_interse_index, const std::size_t blend_align_index, double sampling_time, pilz::CartesianTrajectory &trajectory) const
	blend two trajectories in Cartesian space, result in a MultiDOFJointTrajectory which consists of a list of transforms for the blend phase. More...

void	determineTrajectoryAlignment (const pilz::TrajectoryBlendRequest &req, std::size_t first_interse_index, std::size_t second_interse_index, std::size_t &blend_align_index) const
	Determine how the second trajectory should be aligned with the first trajectory for blend. Let tau_1 be the time of the first trajectory from the first_interse_index to the end and tau_2 the time of the second trajectory from the beginning to the second_interse_index: More...

bool	searchIntersectionPoints (const pilz::TrajectoryBlendRequest &req, std::size_t &first_interse_index, std::size_t &second_interse_index) const
	searchBlendPoint More...

bool	validateRequest (const pilz::TrajectoryBlendRequest &req, double &sampling_time, moveit_msgs::MoveItErrorCodes &error_code) const
	validate trajectory blend request More...

Static Private Attributes
static constexpr double	epsilon = 1e-4

Additional Inherited Members
Protected Attributes inherited from pilz::TrajectoryBlender
const pilz::LimitsContainer	limits_

Detailed Description

Trajectory blender implementing transition window algorithm.

See doc/MotionBlendAlgorithmDescription.pdf for a mathematical description of the algorithmn.

Definition at line 34 of file trajectory_blender_transition_window.h.

Constructor & Destructor Documentation

pilz::TrajectoryBlenderTransitionWindow::TrajectoryBlenderTransitionWindow ( const LimitsContainer & planner_limits )

inline

Definition at line 37 of file trajectory_blender_transition_window.h.

virtual pilz::TrajectoryBlenderTransitionWindow::~TrajectoryBlenderTransitionWindow ( )

inlinevirtual

Definition at line 42 of file trajectory_blender_transition_window.h.

Member Function Documentation

bool pilz::TrajectoryBlenderTransitionWindow::blend	(	const pilz::TrajectoryBlendRequest &	req,
		pilz::TrajectoryBlendResponse &	res
	)

overridevirtual

Blend two trajectories using transition window. The trajectories have to be equally and uniformly discretized.

Parameters

req

following fields need to be filled for a valid request:

group_name : name of the planning group
link_name : name of the target link
first_trajectory: Joint trajectory stops at end point. The last point must be the same as the first point of the second trajectory.
second trajectory: Joint trajectory stops at end point. The first point must be the same as the last point of the first trajectory.
blend_radius: The blend radius determines a sphere with the intersection point of the two trajectories as the center. Trajectory blending happens inside of this sphere.

res

following fields are returned as response by the blend algorithm

group_name : name of the planning group
first_trajectory: Part of the first original trajectory which is outside of the blend sphere.
blend_trajectory: Joint trajectory connecting the first and second trajectories without stop. The first waypoint has non-zero time from start.
second trajectory: Part of the second original trajectory which is outside of the blend sphere. The first waypoint has non-zero time from start. error_code: information of failed blend

Returns: true if succeed

Implements pilz::TrajectoryBlender.

Definition at line 23 of file trajectory_blender_transition_window.cpp.

void pilz::TrajectoryBlenderTransitionWindow::blendTrajectoryCartesian	(	const pilz::TrajectoryBlendRequest &	req,
		const std::size_t	first_interse_index,
		const std::size_t	second_interse_index,
		const std::size_t	blend_align_index,
		double	sampling_time,
		pilz::CartesianTrajectory &	trajectory
	)		const

private

blend two trajectories in Cartesian space, result in a MultiDOFJointTrajectory which consists of a list of transforms for the blend phase.

Parameters

req
first_interse_index
second_interse_index
blend_begin_index
sampling_time
trajectory	the resulting blend trajectory inside the blending sphere

Definition at line 190 of file trajectory_blender_transition_window.cpp.

void pilz::TrajectoryBlenderTransitionWindow::determineTrajectoryAlignment	(	const pilz::TrajectoryBlendRequest &	req,
		std::size_t	first_interse_index,
		std::size_t	second_interse_index,
		std::size_t &	blend_align_index
	)		const

private

Determine how the second trajectory should be aligned with the first trajectory for blend. Let tau_1 be the time of the first trajectory from the first_interse_index to the end and tau_2 the time of the second trajectory from the beginning to the second_interse_index:

if tau_1 > tau_2:
align the end of the first trajectory with second_interse_index

   first traj:  |-------------|--------!--------------|
   second traj:                        |--------------|-------------------|
   blend phase:               |-----------------------|

else:
align the first_interse_index with the beginning of the second trajectory

   first traj:  |-------------|-----------------------|
   second traj:               |-----------------------!----------|-------------------|
   blend phase:               |----------------------------------|

Parameters

req	trajectory blend request
first_interse_index	index of the intersection point between first trajectory and blend sphere
second_interse_index	index of the intersection point between second trajectory and blend sphere
blend_align_index	index on the first trajectory, to which the first point on the second trajectory should be aligned to for motion blend. It is now always same as first_interse_index
blend_time	time of the motion blend period

Definition at line 283 of file trajectory_blender_transition_window.cpp.

bool pilz::TrajectoryBlenderTransitionWindow::searchIntersectionPoints	(	const pilz::TrajectoryBlendRequest &	req,
		std::size_t &	first_interse_index,
		std::size_t &	second_interse_index
	)		const

private

searchBlendPoint

Parameters

req	trajectory blend request
first_interse_index	index of the first point of the first trajectory that is inside the blend sphere
second_interse_index	index of the last point of the second trajectory that is still inside the blend sphere

Definition at line 253 of file trajectory_blender_transition_window.cpp.

bool pilz::TrajectoryBlenderTransitionWindow::validateRequest	(	const pilz::TrajectoryBlendRequest &	req,
		double &	sampling_time,
		moveit_msgs::MoveItErrorCodes &	error_code
	)		const

private

validate trajectory blend request

Parameters

req
sampling_time	get the same sampling time of the two input trajectories
error_code

Returns

Definition at line 124 of file trajectory_blender_transition_window.cpp.

Member Data Documentation

constexpr double pilz::TrajectoryBlenderTransitionWindow::epsilon = 1e-4

staticprivate

Definition at line 141 of file trajectory_blender_transition_window.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Static Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation