Inheritance diagram for CustomTrajOptPlanner:

Classes
struct	CartCnt

Public Member Functions
	CustomTrajOptPlanner (const RobotPtr &robot, const std::string &group_name)

PlannerResult	plan (const SceneConstPtr &scene, const robot_state::RobotStatePtr &start_state)
	Plan a motion using custom terms specified by the user. More...

Public Member Functions inherited from robowflex::TrajOptPlanner
	TrajOptPlanner (const RobotPtr &robot, const std::string &group_name, const std::string &name="trajopt")
	Constructor. More...

bool	initialize (const std::string &manip)
	Initialize planner. The user must specify a chain group defined in the robot's srdf that contains all the links of the manipulator. More...

bool	initialize (const std::string &base_link, const std::string &tip_link)
	Initialize planner. All links between base_link and tip_link will be added to the manipulator. More...

void	setInitialTrajectory (const robot_trajectory::RobotTrajectoryPtr &init_trajectory)
	Set initial trajectory for TrajOpt and set init_type to GIVEN_TRAJ. More...

void	setInitialTrajectory (const trajopt::TrajArray &init_trajectory)
	Set initial trajectory for TrajOpt and set init_type to GIVEN_TRAJ. More...

void	setInitType (const trajopt::InitInfo::Type &init_type)
	Set type of initialization to use for the trajectory optimization. Current options are: STATIONARY JOINT_INTERPOLATED The user can also provide their own trajectory using setInitialTrajectory(). In such case, there is no need to call setInitType(). More...

const robot_trajectory::RobotTrajectoryPtr &	getTrajectory () const
	Get the trajectory that resulted in the last call to plan(). More...

const trajopt::TrajArray &	getTesseractTrajectory () const
	Get the trajectory that resulted in the last call to plan() in Tesseract format. More...

const std::vector< std::string > &	getEnvironmentLinks () const
	Get the link names in the tesseract KDL environment. More...

const std::vector< std::string > &	getManipulatorLinks () const
	Get the link names of the env manipulator. More...

const std::vector< std::string > &	getManipulatorJoints () const
	Get the joint names of the env manipulator. More...

double	getPlanningTime () const
	Get the time spent by the planner the last time it was called. More...

void	fixJoints (const std::vector< std::string > &joints)
	Constrain certain joints during optimization to their initial value. More...

planning_interface::MotionPlanResponse	plan (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) override
	Plan a motion given a request and a scene. More...

PlannerResult	plan (const SceneConstPtr &scene, const robot_state::RobotStatePtr &start_state, const robot_state::RobotStatePtr &goal_state)
	Plan a motion using a scene from start_state to a goal_state. More...

PlannerResult	plan (const SceneConstPtr &scene, const robot_state::RobotStatePtr &start_state, const RobotPose &goal_pose, const std::string &link)
	Plan a motion given a start_state, a goal_pose for a link and a scene. More...

PlannerResult	plan (const SceneConstPtr &scene, const std::unordered_map< std::string, double > &start_state, const RobotPose &goal_pose, const std::string &link)
	Plan a motion given a start_state, a goal_pose for a link and a scene. More...

PlannerResult	plan (const SceneConstPtr &scene, const RobotPose &start_pose, const std::string &start_link, const RobotPose &goal_pose, const std::string &goal_link)
	Plan a motion given a start_pose for start_link and a goal_pose for goal_link. More...

std::vector< std::string >	getPlannerConfigs () const override
	Get planner configurations offered by this planner. Any of the configurations returned can be set as the planner for a motion planning query sent to plan(). More...

void	setWriteFile (bool file_write_cb, const std::string &file_path="")
	Set whether a report file (for the optimization) should be written or not. More...

Public Member Functions inherited from robowflex::Planner
	Planner (const RobotPtr &robot, const std::string &name="")
	Constructor. Takes in a robot description and an optional namespace name. If name is specified, planner parameters are namespaced under the namespace of robot. More...

	Planner (Planner const &)=delete

void	operator= (Planner const &)=delete

virtual std::map< std::string, ProgressProperty >	getProgressProperties (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) const
	Retrieve the planner progress property map for this planner given a specific request. More...

const RobotPtr	getRobot () const
	Return the robot for this planner. More...

const std::string &	getName () const
	Get the name of the planner. More...

virtual void	preRun (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request)
	This function is called before benchmarking. More...

Public Attributes
std::vector< CartCnt >	constraints_ {}

Public Attributes inherited from robowflex::TrajOptPlanner
struct robowflex::TrajOptPlanner::Options	options

Private Member Functions
void	addCartTerms (std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const

Additional Inherited Members
Public Types inherited from robowflex::TrajOptPlanner
typedef std::pair< bool, bool >	PlannerResult
	Planner result: first->converged, second->collision_free. More...

Public Types inherited from robowflex::Planner
using	ProgressProperty = std::function< std::string()>
	A function that returns the value of a planner property over the course of a run. More...

Protected Member Functions inherited from robowflex::TrajOptPlanner
void	problemConstructionInfo (std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Create a TrajOpt problem construction info object with default values. More...

void	addVelocityCost (std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add velocity cost to the trajectory optimization. More...

void	addCollisionAvoidance (std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add collision avoidance cost to the trajectory optimization for all waypoints. More...

void	addStartState (const MotionRequestBuilderPtr &request, std::shared_ptr< trajopt::ProblemConstructionInfo > pci)
	Add a (joint) configuration constraint in the first waypoint taken from request. More...

void	addStartState (const robot_state::RobotStatePtr &start_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci)
	Add a (joint) configuration constraint start_state in the first waypoint. More...

void	addStartState (const std::unordered_map< std::string, double > &start_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci)
	Add a (joint) configuration constraint start_state in the first waypoint. More...

void	addStartPose (const RobotPose &start_pose, const std::string &link, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a cartesian pose constraint start_pose for link in the first waypoint. More...

void	addGoalState (const MotionRequestBuilderPtr &request, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a (joint) configuration constraint in the last waypoint taken from request. More...

void	addGoalState (const robot_state::RobotStatePtr &goal_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a (joint) configuration constraint goal_state in the last waypoint. More...

void	addGoalState (const std::vector< double > goal_state, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a (joint) configuration constraint goal_state in the last waypoint. More...

void	addGoalPose (const RobotPose &goal_pose, const std::string &link, std::shared_ptr< trajopt::ProblemConstructionInfo > pci) const
	Add a cartesian pose constraint goal_pose for link in the last waypoint. More...

PlannerResult	solve (const SceneConstPtr &scene, const std::shared_ptr< trajopt::ProblemConstructionInfo > &pci)
	Solve SQP optimization problem. More...

sco::BasicTrustRegionSQPParameters	getTrustRegionSQPParameters () const
	Get parameters of the SQP. More...

Protected Attributes inherited from robowflex::TrajOptPlanner
robot_trajectory::RobotTrajectoryPtr	trajectory_

trajopt::TrajArray	tesseract_trajectory_

tesseract::tesseract_ros::KDLEnvPtr	env_
	KDL environment. More...

std::string	group_
	Name of group to plan for. More...

std::string	manip_
	Name of manipulator chain to check for collisions. More...

bool	cont_cc_ {true}
	Use continuous collision checking. More...

std::shared_ptr< std::ofstream >	stream_ptr_
	Debug file stream. More...

std::string	file_path_
	Path of debug file. More...

bool	file_write_cb_ {false}
	Whether to write a debug file or not. More...

trajopt::InitInfo::Type	init_type_ {trajopt::InitInfo::Type::STATIONARY}

trajopt::TrajArray	initial_trajectory_
	Initial trajectory (if any). More...

double	time_ {0.0}
	Time taken by the optimizer the last time it was called. More...

std::vector< int >	fixed_joints_

robot_state::RobotStatePtr	ref_state_
	Reference state to build moveit trajectory waypoints. More...

Protected Attributes inherited from robowflex::Planner
RobotPtr	robot_
	The robot to plan for. More...

IO::Handler	handler_
	The parameter handler for the planner. More...

const std::string	name_
	Namespace for the planner. More...

Detailed Description

Definition at line 22 of file ur5_custom_planning.cpp.

Constructor & Destructor Documentation

◆ CustomTrajOptPlanner()

CustomTrajOptPlanner::CustomTrajOptPlanner	(	const RobotPtr &	robot,
		const std::string &	group_name
	)

inline

Definition at line 34 of file ur5_custom_planning.cpp.

       : TrajOptPlanner(robot, group_name, "custom_trajopt")
     {
     }

Member Function Documentation

◆ addCartTerms()

void CustomTrajOptPlanner::addCartTerms ( std::shared_ptr< trajopt::ProblemConstructionInfo > pci ) const

inlineprivate

Definition at line 66 of file ur5_custom_planning.cpp.

     {
         for (const auto &term : constraints_)
         {
             Eigen::Quaterniond rotation(term.pose.linear());
             auto pose_constraint = std::make_shared<trajopt::CartPoseTermInfo>();
             pose_constraint->term_type = trajopt::TT_CNT;
             pose_constraint->link = term.link;
             pose_constraint->timestep = term.timestep;
             pose_constraint->xyz = term.pose.translation();
             pose_constraint->wxyz = Eigen::Vector4d(rotation.w(), rotation.x(), rotation.y(), rotation.z());
             pose_constraint->pos_coeffs = Eigen::Vector3d::Constant(term.pos_coeffs);
             pose_constraint->rot_coeffs = Eigen::Vector3d::Constant(term.rot_coeffs);
             pose_constraint->name = "pose_cnt_link_" + term.link + std::to_string(term.timestep);
  
             pci->cnt_infos.push_back(pose_constraint);
         }
     }

◆ plan()

PlannerResult CustomTrajOptPlanner::plan	(	const SceneConstPtr &	scene,
		const robot_state::RobotStatePtr &	start_state
	)

inlinevirtual

Plan a motion using custom terms specified by the user.

Parameters

[in]	scene	A planning scene to compute the plan in.
[in]	start_state	Start state for the robot.

Returns: Planner result with convergence and collision status.

Reimplemented from robowflex::TrajOptPlanner.

Definition at line 39 of file ur5_custom_planning.cpp.

     {
         // This is required by any TrajOptPlanner::plan() method.
         ref_state_ = std::make_shared<robot_state::RobotState>(*start_state);
  
         // Transform robowflex scene to tesseract environment.
         hypercube::sceneToTesseractEnv(scene, env_);
  
         // Fill in the problem construction info and initialization.
         auto pci = std::make_shared<trajopt::ProblemConstructionInfo>(env_);
         problemConstructionInfo(pci);
  
         // Add velocity cost.
         addVelocityCost(pci);
  
         // Add start state.
         addStartState(start_state, pci);
  
         // Add cartesian terms in constraints_.
         addCartTerms(pci);
  
         return solve(scene, pci);
     }

Member Data Documentation

◆ constraints_

std::vector<CartCnt> CustomTrajOptPlanner::constraints_ {}

Definition at line 63 of file ur5_custom_planning.cpp.

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

ur5_custom_planning.cpp

Classes

Public Member Functions

Public Attributes

Private Member Functions

Additional Inherited Members