choreo_process_planning Namespace Reference

Classes
struct	ConstrainedSegParameters
class	ProcessPlanningManager

Typedefs
typedef std::vector< descartes_core::TrajectoryPtPtr >	DescartesTraj
typedef std::vector< std::vector< double > >	JointVector
typedef std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > >	PoseVector

Functions
bool	addCollisionObject (ros::ServiceClient &planning_scene, const moveit_msgs::CollisionObject &c_obj)
moveit_msgs::AttachedCollisionObject	addFullEndEffectorCollisionObject (bool is_add)
void	adjustTrajectoryTiming (std::vector< choreo_msgs::UnitProcessPlan > &plans, const std::vector< std::string > &joint_names, const std::string world_frame)
void	appendTCPPoseToPlans (const descartes_core::RobotModelPtr model, const std::vector< planning_scene::PlanningScenePtr > &planning_scenes_shrinked, const std::vector< planning_scene::PlanningScenePtr > &planning_scenes_full, std::vector< choreo_msgs::UnitProcessPlan > &plans)
void	appendTCPPoseToPlansPickNPlace (const descartes_core::RobotModelPtr model, const std::vector< std::vector< planning_scene::PlanningScenePtr >> &planning_scenes_transition, const std::vector< std::vector< planning_scene::PlanningScenePtr >> &planning_scenes_subprocess, std::vector< choreo_msgs::UnitProcessPlan > &plans)
void	appendTrajectoryHeaders (const trajectory_msgs::JointTrajectory &orig_traj, trajectory_msgs::JointTrajectory &traj, const double sim_time_scale=0.0)
bool	clearAllCollisionObjects (ros::ServiceClient &planning_scene)
void	CLTRRTforProcessROSTraj (descartes_core::RobotModelPtr model, std::vector< descartes_planner::ConstrainedSegment > &segs, const double clt_rrt_unit_process_timeout, const double clt_rrt_timeout, const std::vector< planning_scene::PlanningScenePtr > &planning_scenes_approach, const std::vector< planning_scene::PlanningScenePtr > &planning_scenes_depart, const std::vector< choreo_msgs::ElementCandidatePoses > &task_seq, std::vector< choreo_msgs::UnitProcessPlan > &plans, const std::string &saved_graph_file_name, bool use_saved_graph)
void	CLTRRTforProcessROSTraj (descartes_core::RobotModelPtr model, const choreo_msgs::AssemblySequencePickNPlace &as_pnp, const double clt_rrt_unit_process_timeout, const double clt_rrt_timeout, const double &linear_vel, const double &linear_disc, const std::vector< std::vector< planning_scene::PlanningScenePtr >> &planning_scenes_subprocess, std::vector< choreo_msgs::UnitProcessPlan > &plans, const std::string &saved_graph_file_name, bool use_saved_graph)
void	constructPlanningScene (const planning_scene::PlanningScenePtr base_scene, const std::vector< moveit_msgs::CollisionObject > &add_cos, const std::vector< moveit_msgs::CollisionObject > &remove_cos, const std::vector< moveit_msgs::AttachedCollisionObject > &attach_objs, const std::vector< moveit_msgs::AttachedCollisionObject > &detach_objs, planning_scene::PlanningScenePtr s)
planning_scene::PlanningScenePtr	constructPlanningScene (const planning_scene::PlanningScenePtr base_scene, const std::vector< moveit_msgs::CollisionObject > &add_cos, const std::vector< moveit_msgs::CollisionObject > &remove_cos, const std::vector< moveit_msgs::AttachedCollisionObject > &attach_objs, const std::vector< moveit_msgs::AttachedCollisionObject > &detach_objs)
void	constructPlanningScenes (moveit::core::RobotModelConstPtr moveit_model, const std::string &world_frame, const choreo_msgs::AssemblySequencePickNPlace &as_pnp, std::vector< std::vector< planning_scene::PlanningScenePtr >> &planning_scenes_transition, std::vector< std::vector< planning_scene::PlanningScenePtr >> &planning_scenes_subprocess)
void	constructPlanningScenes (moveit::core::RobotModelConstPtr moveit_model, const std::vector< choreo_msgs::WireFrameCollisionObject > &wf_collision_objs, std::vector< planning_scene::PlanningScenePtr > &planning_scenes_shrinked_approach, std::vector< planning_scene::PlanningScenePtr > &planning_scenes_shrinked_depart, std::vector< planning_scene::PlanningScenePtr > &planning_scenes_full)
DescartesTraj	createJointPath (const std::vector< double > &start, const std::vector< double > &stop, double dtheta=M_PI/180.0)
Eigen::Affine3d	createNominalTransform (const geometry_msgs::Pose &ref_pose, const geometry_msgs::Point &pt)
Eigen::Affine3d	createNominalTransform (const geometry_msgs::Pose &ref_pose, const double z_adjust=0.0)
Eigen::Affine3d	createNominalTransform (const Eigen::Affine3d &ref_pose, const double z_adjust=0.0)
static std::vector< double >	extractJoints (const descartes_core::RobotModel &model, const descartes_core::TrajectoryPt &pt)
void	fillTrajectoryHeaders (const std::vector< std::string > &joints, trajectory_msgs::JointTrajectory &traj, const std::string world_frame)
double	freeSpaceCostFunction (const std::vector< double > &source, const std::vector< double > &target)
bool	generateMotionPlan (const std::string world_frame, const bool use_saved_graph, const std::string &saved_graph_file_name, const double clt_rrt_unit_process_timeout, const double clt_rrt_timeout, const std::string &move_group_name, const std::vector< double > &start_state, const std::vector< choreo_msgs::ElementCandidatePoses > &task_seq, const std::vector< choreo_msgs::WireFrameCollisionObject > &wf_collision_objs, std::vector< descartes_planner::ConstrainedSegment > &segs, descartes_core::RobotModelPtr model, moveit::core::RobotModelConstPtr moveit_model, ros::ServiceClient &planning_scene_diff_client, std::vector< choreo_msgs::UnitProcessPlan > &plans)
bool	generateMotionPlan (const std::string world_frame, const bool use_saved_graph, const std::string &saved_graph_file_name, const double clt_rrt_unit_process_timeout, const double clt_rrt_timeout, const double &linear_vel, const double &linear_disc, const std::string &move_group_name, const std::vector< double > &start_state, const choreo_msgs::AssemblySequencePickNPlace &as_pnp, descartes_core::RobotModelPtr model, moveit::core::RobotModelConstPtr moveit_model, ros::ServiceClient &planning_scene_diff_client, std::vector< choreo_msgs::UnitProcessPlan > &plans)
std::vector< double >	getCurrentJointState (const std::string &topic)
trajectory_msgs::JointTrajectory	getMoveitPlan (const std::string &group_name, const std::vector< double > &joints_start, const std::vector< double > &joints_stop, moveit::core::RobotModelConstPtr model)
trajectory_msgs::JointTrajectory	getMoveitTransitionPlan (const std::string &group_name, const std::vector< double > &joints_start, const std::vector< double > &joints_stop, const std::vector< double > &initial_pose, moveit::core::RobotModelConstPtr model, const bool force_insert_reset=false)
PoseVector	interpolateCartesian (const Eigen::Affine3d &start, const Eigen::Affine3d &stop, double ds)
JointVector	interpolateJoint (const std::vector< double > &start, const std::vector< double > &stop, double dtheta)
trajectory_msgs::JointTrajectory	planFreeMove (descartes_core::RobotModel &model, const std::string &group_name, moveit::core::RobotModelConstPtr moveit_model, const std::vector< double > &start, const std::vector< double > &stop)
void	retractionPlanning (descartes_core::RobotModelPtr model, const std::vector< planning_scene::PlanningScenePtr > &planning_scenes_approach, const std::vector< planning_scene::PlanningScenePtr > &planning_scenes_depart, const std::vector< descartes_planner::ConstrainedSegment > &segs, std::vector< choreo_msgs::UnitProcessPlan > &plans)
bool	retractPath (const std::vector< double > &start_joint, double retract_dist, double TCP_speed, const std::vector< Eigen::Matrix3d > &eef_directions, descartes_core::RobotModelPtr &model, std::vector< std::vector< double >> &retract_jt_traj)
std::vector< descartes_planner::ConstrainedSegmentPickNPlace >	toDescartesConstrainedPath (const choreo_msgs::AssemblySequencePickNPlace &as_pnp, const std::vector< std::vector< planning_scene::PlanningScenePtr >> &planning_scene_subprocess, const double &linear_vel, const double &linear_disc)
std::vector< descartes_planner::ConstrainedSegment >	toDescartesConstrainedPath (const std::vector< choreo_msgs::ElementCandidatePoses > &task_sequence, const ConstrainedSegParameters &seg_params)
std::vector< descartes_planner::ConstrainedSegment >	toDescartesConstrainedPath (const std::vector< choreo_msgs::ElementCandidatePoses > &task_sequence, const double &linear_vel, const double &linear_disc, const double &angular_disc, const double &retract_dist)
trajectory_msgs::JointTrajectory	toROSTrajectory (const DescartesTraj &solution, const descartes_core::RobotModel &model)
trajectory_msgs::JointTrajectory	toROSTrajectory (const std::vector< std::vector< double >> &solution, const descartes_core::RobotModel &model)
void	transitionPlanning (std::vector< choreo_msgs::UnitProcessPlan > &plans, moveit::core::RobotModelConstPtr moveit_model, ros::ServiceClient &planning_scene_diff_client, const std::string &move_group_name, const std::vector< double > &start_state, std::vector< planning_scene::PlanningScenePtr > &planning_scenes)
void	transitionPlanningPickNPlace (std::vector< choreo_msgs::UnitProcessPlan > &plans, moveit::core::RobotModelConstPtr moveit_model, ros::ServiceClient &planning_scene_diff_client, const std::string &move_group_name, const std::vector< double > &start_state, const std::vector< std::vector< planning_scene::PlanningScenePtr >> &planning_scenes)

Variables
static const std::string	JOINT_TOPIC_NAME
static const std::string	JOINT_TOPIC_NAME
const double	PRINT_ANGLE_DISCRETIZATION

Typedef Documentation

typedef std::vector<descartes_core::TrajectoryPtPtr> choreo_process_planning::DescartesTraj

Definition at line 25 of file common_utils.h.

typedef std::vector<std::vector<double> > choreo_process_planning::JointVector

Definition at line 18 of file trajectory_utils.h.

typedef std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > choreo_process_planning::PoseVector

Definition at line 13 of file trajectory_utils.h.

Function Documentation

bool choreo_process_planning::addCollisionObject	(	ros::ServiceClient &	planning_scene,
		const moveit_msgs::CollisionObject &	c_obj
	)

Definition at line 242 of file common_utils.cpp.

moveit_msgs::AttachedCollisionObject choreo_process_planning::addFullEndEffectorCollisionObject

(

bool

is_add

)

Definition at line 274 of file common_utils.cpp.

void choreo_process_planning::adjustTrajectoryTiming	(	std::vector< choreo_msgs::UnitProcessPlan > &	plans,
		const std::vector< std::string > &	joint_names,
		const std::string	world_frame
	)

Definition at line 426 of file generate_motion_plan.cpp.

void choreo_process_planning::appendTCPPoseToPlans	(	const descartes_core::RobotModelPtr	model,
		const std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_shrinked,
		const std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_full,
		std::vector< choreo_msgs::UnitProcessPlan > &	plans
	)

Definition at line 473 of file generate_motion_plan.cpp.

void choreo_process_planning::appendTCPPoseToPlansPickNPlace	(	const descartes_core::RobotModelPtr	model,
		const std::vector< std::vector< planning_scene::PlanningScenePtr >> &	planning_scenes_transition,
		const std::vector< std::vector< planning_scene::PlanningScenePtr >> &	planning_scenes_subprocess,
		std::vector< choreo_msgs::UnitProcessPlan > &	plans
	)

Definition at line 517 of file generate_motion_plan.cpp.

void choreo_process_planning::appendTrajectoryHeaders	(	const trajectory_msgs::JointTrajectory &	orig_traj,
		trajectory_msgs::JointTrajectory &	traj,
		const double	sim_time_scale = 0.0
	)

Definition at line 213 of file common_utils.cpp.

bool choreo_process_planning::clearAllCollisionObjects

(

ros::ServiceClient &

planning_scene

)

Definition at line 321 of file common_utils.cpp.

void choreo_process_planning::CLTRRTforProcessROSTraj	(	descartes_core::RobotModelPtr	model,
		std::vector< descartes_planner::ConstrainedSegment > &	segs,
		const double	clt_rrt_unit_process_timeout,
		const double	clt_rrt_timeout,
		const std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_approach,
		const std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_depart,
		const std::vector< choreo_msgs::ElementCandidatePoses > &	task_seq,
		std::vector< choreo_msgs::UnitProcessPlan > &	plans,
		const std::string &	saved_graph_file_name,
		bool	use_saved_graph
	)

Definition at line 57 of file semi_constrained_cartesian_planning.cpp.

void choreo_process_planning::CLTRRTforProcessROSTraj	(	descartes_core::RobotModelPtr	model,
		const choreo_msgs::AssemblySequencePickNPlace &	as_pnp,
		const double	clt_rrt_unit_process_timeout,
		const double	clt_rrt_timeout,
		const double &	linear_vel,
		const double &	linear_disc,
		const std::vector< std::vector< planning_scene::PlanningScenePtr >> &	planning_scenes_subprocess,
		std::vector< choreo_msgs::UnitProcessPlan > &	plans,
		const std::string &	saved_graph_file_name,
		bool	use_saved_graph
	)

Definition at line 169 of file semi_constrained_cartesian_planning.cpp.

void choreo_process_planning::constructPlanningScene	(	const planning_scene::PlanningScenePtr	base_scene,
		const std::vector< moveit_msgs::CollisionObject > &	add_cos,
		const std::vector< moveit_msgs::CollisionObject > &	remove_cos,
		const std::vector< moveit_msgs::AttachedCollisionObject > &	attach_objs,
		const std::vector< moveit_msgs::AttachedCollisionObject > &	detach_objs,
		planning_scene::PlanningScenePtr	s
	)

Definition at line 54 of file construct_planning_scene.cpp.

planning_scene::PlanningScenePtr choreo_process_planning::constructPlanningScene	(	const planning_scene::PlanningScenePtr	base_scene,
		const std::vector< moveit_msgs::CollisionObject > &	add_cos,
		const std::vector< moveit_msgs::CollisionObject > &	remove_cos,
		const std::vector< moveit_msgs::AttachedCollisionObject > &	attach_objs,
		const std::vector< moveit_msgs::AttachedCollisionObject > &	detach_objs
	)

Definition at line 88 of file construct_planning_scene.cpp.

void choreo_process_planning::constructPlanningScenes	(	moveit::core::RobotModelConstPtr	moveit_model,
		const std::string &	world_frame,
		const choreo_msgs::AssemblySequencePickNPlace &	as_pnp,
		std::vector< std::vector< planning_scene::PlanningScenePtr >> &	planning_scenes_transition,
		std::vector< std::vector< planning_scene::PlanningScenePtr >> &	planning_scenes_subprocess
	)

Definition at line 99 of file construct_planning_scene.cpp.

void choreo_process_planning::constructPlanningScenes	(	moveit::core::RobotModelConstPtr	moveit_model,
		const std::vector< choreo_msgs::WireFrameCollisionObject > &	wf_collision_objs,
		std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_shrinked_approach,
		std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_shrinked_depart,
		std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_full
	)

Definition at line 367 of file construct_planning_scene.cpp.

choreo_process_planning::DescartesTraj choreo_process_planning::createJointPath	(	const std::vector< double > &	start,
		const std::vector< double > &	stop,
		double	dtheta = M_PI / 180.0
	)

Definition at line 373 of file common_utils.cpp.

Eigen::Affine3d choreo_process_planning::createNominalTransform	(	const geometry_msgs::Pose &	ref_pose,
		const geometry_msgs::Point &	pt
	)

Definition at line 55 of file common_utils.cpp.

Eigen::Affine3d choreo_process_planning::createNominalTransform	(	const geometry_msgs::Pose &	ref_pose,
		const double	z_adjust = 0.0
	)

Definition at line 76 of file common_utils.cpp.

Eigen::Affine3d choreo_process_planning::createNominalTransform	(	const Eigen::Affine3d &	ref_pose,
		const double	z_adjust = 0.0
	)

Definition at line 86 of file common_utils.cpp.

static std::vector<double> choreo_process_planning::extractJoints ( const descartes_core::RobotModel &  model, const descartes_core::TrajectoryPt &  pt  )

inlinestatic

Definition at line 58 of file common_utils.h.

void choreo_process_planning::fillTrajectoryHeaders	(	const std::vector< std::string > &	joints,
		trajectory_msgs::JointTrajectory &	traj,
		const std::string	world_frame
	)

Definition at line 204 of file common_utils.cpp.

double choreo_process_planning::freeSpaceCostFunction	(	const std::vector< double > &	source,
		const std::vector< double > &	target
	)

Definition at line 654 of file common_utils.cpp.

bool choreo_process_planning::generateMotionPlan	(	const std::string	world_frame,
		const bool	use_saved_graph,
		const std::string &	saved_graph_file_name,
		const double	clt_rrt_unit_process_timeout,
		const double	clt_rrt_timeout,
		const std::string &	move_group_name,
		const std::vector< double > &	start_state,
		const std::vector< choreo_msgs::ElementCandidatePoses > &	task_seq,
		const std::vector< choreo_msgs::WireFrameCollisionObject > &	wf_collision_objs,
		std::vector< descartes_planner::ConstrainedSegment > &	segs,
		descartes_core::RobotModelPtr	model,
		moveit::core::RobotModelConstPtr	moveit_model,
		ros::ServiceClient &	planning_scene_diff_client,
		std::vector< choreo_msgs::UnitProcessPlan > &	plans
	)

Definition at line 567 of file generate_motion_plan.cpp.

bool choreo_process_planning::generateMotionPlan	(	const std::string	world_frame,
		const bool	use_saved_graph,
		const std::string &	saved_graph_file_name,
		const double	clt_rrt_unit_process_timeout,
		const double	clt_rrt_timeout,
		const double &	linear_vel,
		const double &	linear_disc,
		const std::string &	move_group_name,
		const std::vector< double > &	start_state,
		const choreo_msgs::AssemblySequencePickNPlace &	as_pnp,
		descartes_core::RobotModelPtr	model,
		moveit::core::RobotModelConstPtr	moveit_model,
		ros::ServiceClient &	planning_scene_diff_client,
		std::vector< choreo_msgs::UnitProcessPlan > &	plans
	)

Definition at line 651 of file generate_motion_plan.cpp.

std::vector< double > choreo_process_planning::getCurrentJointState

(

const std::string &

topic

)

Definition at line 233 of file common_utils.cpp.

trajectory_msgs::JointTrajectory choreo_process_planning::getMoveitPlan	(	const std::string &	group_name,
		const std::vector< double > &	joints_start,
		const std::vector< double > &	joints_stop,
		moveit::core::RobotModelConstPtr	model
	)

Definition at line 385 of file common_utils.cpp.

trajectory_msgs::JointTrajectory choreo_process_planning::getMoveitTransitionPlan	(	const std::string &	group_name,
		const std::vector< double > &	joints_start,
		const std::vector< double > &	joints_stop,
		const std::vector< double > &	initial_pose,
		moveit::core::RobotModelConstPtr	model,
		const bool	force_insert_reset = false
	)

Definition at line 453 of file common_utils.cpp.

choreo_process_planning::PoseVector choreo_process_planning::interpolateCartesian	(	const Eigen::Affine3d &	start,
		const Eigen::Affine3d &	stop,
		double	ds
	)

Definition at line 48 of file trajectory_utils.cpp.

choreo_process_planning::JointVector choreo_process_planning::interpolateJoint	(	const std::vector< double > &	start,
		const std::vector< double > &	stop,
		double	dtheta
	)

Definition at line 79 of file trajectory_utils.cpp.

trajectory_msgs::JointTrajectory choreo_process_planning::planFreeMove	(	descartes_core::RobotModel &	model,
		const std::string &	group_name,
		moveit::core::RobotModelConstPtr	moveit_model,
		const std::vector< double > &	start,
		const std::vector< double > &	stop
	)

Definition at line 621 of file common_utils.cpp.

void choreo_process_planning::retractionPlanning	(	descartes_core::RobotModelPtr	model,
		const std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_approach,
		const std::vector< planning_scene::PlanningScenePtr > &	planning_scenes_depart,
		const std::vector< descartes_planner::ConstrainedSegment > &	segs,
		std::vector< choreo_msgs::UnitProcessPlan > &	plans
	)

Definition at line 41 of file generate_motion_plan.cpp.

bool choreo_process_planning::retractPath	(	const std::vector< double > &	start_joint,
		double	retract_dist,
		double	TCP_speed,
		const std::vector< Eigen::Matrix3d > &	eef_directions,
		descartes_core::RobotModelPtr &	model,
		std::vector< std::vector< double >> &	retract_jt_traj
	)

Definition at line 213 of file path_transitions.cpp.

std::vector< descartes_planner::ConstrainedSegmentPickNPlace > choreo_process_planning::toDescartesConstrainedPath	(	const choreo_msgs::AssemblySequencePickNPlace &	as_pnp,
		const std::vector< std::vector< planning_scene::PlanningScenePtr >> &	planning_scene_subprocess,
		const double &	linear_vel,
		const double &	linear_disc
	)

Definition at line 75 of file path_transitions.cpp.

std::vector< descartes_planner::ConstrainedSegment > choreo_process_planning::toDescartesConstrainedPath	(	const std::vector< choreo_msgs::ElementCandidatePoses > &	task_sequence,
		const ConstrainedSegParameters &	seg_params
	)

Definition at line 146 of file path_transitions.cpp.

std::vector< descartes_planner::ConstrainedSegment > choreo_process_planning::toDescartesConstrainedPath	(	const std::vector< choreo_msgs::ElementCandidatePoses > &	task_sequence,
		const double &	linear_vel,
		const double &	linear_disc,
		const double &	angular_disc,
		const double &	retract_dist
	)

Definition at line 200 of file path_transitions.cpp.

trajectory_msgs::JointTrajectory choreo_process_planning::toROSTrajectory	(	const DescartesTraj &	solution,
		const descartes_core::RobotModel &	model
	)

Definition at line 154 of file common_utils.cpp.

trajectory_msgs::JointTrajectory choreo_process_planning::toROSTrajectory	(	const std::vector< std::vector< double >> &	solution,
		const descartes_core::RobotModel &	model
	)

Definition at line 110 of file common_utils.cpp.

void choreo_process_planning::transitionPlanning	(	std::vector< choreo_msgs::UnitProcessPlan > &	plans,
		moveit::core::RobotModelConstPtr	moveit_model,
		ros::ServiceClient &	planning_scene_diff_client,
		const std::string &	move_group_name,
		const std::vector< double > &	start_state,
		std::vector< planning_scene::PlanningScenePtr > &	planning_scenes
	)

Definition at line 133 of file generate_motion_plan.cpp.

void choreo_process_planning::transitionPlanningPickNPlace	(	std::vector< choreo_msgs::UnitProcessPlan > &	plans,
		moveit::core::RobotModelConstPtr	moveit_model,
		ros::ServiceClient &	planning_scene_diff_client,
		const std::string &	move_group_name,
		const std::vector< double > &	start_state,
		const std::vector< std::vector< planning_scene::PlanningScenePtr >> &	planning_scenes
	)

Definition at line 270 of file generate_motion_plan.cpp.

Variable Documentation

const std::string choreo_process_planning::JOINT_TOPIC_NAME

static

Initial value:

=
    "joint_states"

Definition at line 24 of file move_to_target_pose_planning.cpp.

const std::string choreo_process_planning::JOINT_TOPIC_NAME

static

Initial value:

=
    "joint_states"

Definition at line 25 of file print_process_planning.cpp.

const double choreo_process_planning::PRINT_ANGLE_DISCRETIZATION

Initial value:

=
    M_PI / 12.0

Definition at line 22 of file print_process_planning.cpp.