robowflex::MotionRequestBuilder Class Reference

A helper class to build motion planning requests for a robowflex::Planner. More...

#include <builder.h>

Inheritance diagram for robowflex::MotionRequestBuilder:

Public Member Functions
	MotionRequestBuilder (const RobotConstPtr &robot)
	Constructor. More...
	MotionRequestBuilder (const RobotConstPtr &robot, const std::string &group_name, const std::string &planner_config="")
	Constructor. Set planner config and group as well. More...
	MotionRequestBuilder (const PlannerConstPtr &planner, const std::string &group_name, const std::string &planner_config="")
	Constructor. More...
	MotionRequestBuilder (const MotionRequestBuilder &other)
	Copy Constructor. More...
MotionRequestBuilderPtr	clone () const
	Clone this request. More...
Configuring Builder
void	initialize ()
	Sets defaults. More...
void	setPlanningGroup (const std::string &group_name)
	Set the planning group to use for this request builder. More...
void	setPlanner (const PlannerConstPtr &planner)
	Set the Robowflex planner to use. More...
Starting Configurations
void	setStartConfiguration (const std::vector< double > &joints)
	Set the start configuration from a vector joints. All joints are assumed to be specified and in the default order. More...
void	setStartConfiguration (const robot_state::RobotState &state)
	Set the start configuration from a robot state. More...
void	setStartConfiguration (const robot_state::RobotStatePtr &state)
	Set the start configuration from a robot state. More...
void	useSceneStateAsStart (const SceneConstPtr &scene)
	Use the current scene state for the starting configuration. More...
bool	attachObjectToStart (ScenePtr scene, const std::string &object)
	Attach an object to the current request start state. Uses object from scene, and modifies the underlying scene state. This uses attachObject() on the scene, so note that the attached object will have to be re-added to the scene. More...
bool	attachObjectToStartConst (const SceneConstPtr &scene, const std::string &object)
	Attach an object to the current request start state. Uses object from scene, but does not modify the underlying scene. Be aware that attached objects can collide with themselves if not removed from the provided scene. More...
Path Constraints
void	addPathPoseConstraint (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
	Set a pose constraint on the path. Sets the position constraint from geometry at a pose pose, and the orientation constraint from orientation and XYZ Euler angle tolerances tolerances. More...
void	addPathPositionConstraint (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry)
	Set a position constraint on the path. Sets the position constraint from geometry at a pose pose. More...
void	addPathOrientationConstraint (const std::string &ee_name, const std::string &base_name, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
	Set an orientation constraint on the path. Sets the orientation constraint from orientation and XYZ Euler angle tolerances tolerances. More...
Miscellaneous Settings
bool	setConfig (const std::string &requested_config)
	Set the planning configuration to use for the motion planning request. Attempts to match requested_config with the planner configuration offered by planner_ that is the shortest configuration that contains requested_config as a substring. For example, specifying RRTConnect will match RRTConnectkConfigDefault, and specifying RRT will match RRTkConfigDefault and not RRTConnectkConfigDefault. More...
void	setAllowedPlanningTime (double allowed_planning_time)
	Set the allowed planning time in the request. More...
void	setNumPlanningAttempts (unsigned int num_planning_attempts)
	Set the number of planning attemps in the request. More...
void	setWorkspaceBounds (const moveit_msgs::WorkspaceParameters &wp)
	Sets workspace bounds of the planning request. More...
void	setWorkspaceBounds (const Eigen::Ref< const Eigen::VectorXd > &min, const Eigen::Ref< const Eigen::VectorXd > &max)
	Sets workspace bounds of the planning request. More...
bool	swapStartWithGoal ()
	Swap the start and goal configurations. This is only possible when a single joint goal is specified, otherwise an error is raised. More...
Getters
planning_interface::MotionPlanRequest &	getRequest ()
	Get a reference to the currently built motion planning request. More...
const planning_interface::MotionPlanRequest &	getRequestConst () const
	Get a const reference to the currently built motion planning request. More...
robot_state::RobotStatePtr	getStartConfiguration () const
	Get the start state of the request as a robot state. More...
robot_state::RobotStatePtr	getGoalConfiguration () const
	Get the goal state of the request as a robot state. More...
moveit_msgs::Constraints &	getPathConstraints ()
	Get a reference to the current path constraints on the motion planning request. More...
const RobotConstPtr &	getRobot () const
	Get the robot for this request. More...
const PlannerConstPtr &	getPlanner () const
	Get the planner for this request. More...
const std::string &	getPlanningGroup () const
	Get the planning group. More...
const std::string &	getPlannerConfig () const
	Get the planner config. More...
IO
bool	toYAMLFile (const std::string &file) const
	Serialize the motion planning request to a YAML file file. More...
bool	fromYAMLFile (const std::string &file)
	Load a planning request from a YAML file file. More...
Public Member Functions inherited from robowflex::ID
	ID ()
	Constructor. More...
const std::string &	getID () const
	Get the unique ID for this object. More...
std::size_t	getVersion () const
	Get the current version of this object. More...
Key	getKey () const
	Get this ID as a Key. More...
bool	operator== (const ID &b) const
	Compare with another ID object. More...
bool	operator== (const Key &b) const
	Compare with an ID Key. More...

Private Attributes
const RobotConstPtr	robot_
	The robot to build the request for. More...
PlannerConstPtr	planner_
	The planner to build the request for. More...
std::string	group_name_
	The group to plan for. More...
robot_model::JointModelGroup *	jmg_ {nullptr}
planning_interface::MotionPlanRequest	request_
	The build request. More...

Static Private Attributes
static const std::string	DEFAULT_CONFIG = "RRTConnectkConfigDefault"
	Default planner configuration to use. More...

Goals
using	ConfigurationValidityCallback = std::function< bool(const robot_state::RobotState &)>
	Callback function that returns true if configuration is valid for goal set, false otherwise. More...
void	addGoalConfiguration (const std::vector< double > &joints)
	Add a goal configuration from a vector joints. All joints are assumed to be specified and in the default order. More...
void	addGoalConfiguration (const robot_state::RobotStatePtr &state)
	Add a goal configuration from a robot state. More...
void	addGoalConfiguration (const robot_state::RobotState &state)
	Add a goal configuration from a robot state. More...
void	addGoalFromIKQuery (const Robot::IKQuery &query)
	Add an IK query as a goal pose. More...
void	addGoalPose (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, double tolerance=0.001)
	Add a goal pose for the end-effector ee_name. Generates a sphere with radius tolerance as well as orientation tolerances of tolerance from pose. More...
void	addGoalRegion (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
	Add a goal region for an end-effector ee_name. Sets the position constraint from geometry at a pose pose, and the orientation constraint from orientation and XYZ Euler angle tolerances tolerances. More...
void	addGoalRotaryTile (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances, const RobotPose &offset, const Eigen::Vector3d &axis, unsigned int n)
	Tiles some geometry around a pose in base_name for the end-effector ee_name. The geometry is placed at offset from pose, and n copies are placed evenly rotated about axis. The desired orientation is also rotated about the axis and set for each copy. More...
void	addCylinderSideGrasp (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &cylinder, double distance, double depth, unsigned int n)
	Adds a set of regions to grasp a cylinder from the side. This function assumes the X-axis of the end-effector frame ee_name points "forward" for grasping. More...
void	setGoalConfiguration (const std::vector< double > &joints)
	Set the goal configuration from a vector joints. All joints are assumed to be specified and in the default order. More...
void	setGoalConfiguration (const robot_state::RobotStatePtr &state)
	Set the goal configuration from a robot state. More...
void	setGoalConfiguration (const robot_state::RobotState &state)
	Set the goal configuration from a robot state. More...
void	setGoalFromIKQuery (const Robot::IKQuery &query)
	Set the goal pose from an IK query. More...
void	setGoalPose (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, double tolerance=0.001)
	Set a goal pose for the end-effector ee_name. Generates a sphere with radius tolerance as well as orientation tolerances of tolerance from pose. More...
void	setGoalRegion (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
	Set a goal region for an end-effector ee_name. Sets the position constraint from geometry at a pose pose, and the orientation constraint from orientation and XYZ Euler angle tolerances tolerances. More...
void	precomputeGoalConfigurations (std::size_t n_samples, const ScenePtr &scene, const ConfigurationValidityCallback &callback={})
	Override the goals of this motion request with precomputed goal configurations (from the specified regions). More...
void	clearGoals ()
	Clears all goals. More...

Additional Inherited Members
Public Types inherited from robowflex::ID
using	Key = std::pair< std::string, std::size_t >
	A snapshot of the state of an ID. Can be compared against another ID. More...
Static Public Member Functions inherited from robowflex::ID
static Key	getNullKey ()
	Get a null key for initialization. More...
Protected Member Functions inherited from robowflex::ID
void	incrementVersion ()
	Increment the version number of this object. More...

Detailed Description

A helper class to build motion planning requests for a robowflex::Planner.

Definition at line 33 of file builder.h.

Member Typedef Documentation

ConfigurationValidityCallback

using robowflex::MotionRequestBuilder::ConfigurationValidityCallback = std::function<bool(const robot_state::RobotState &)>

Callback function that returns true if configuration is valid for goal set, false otherwise.

Definition at line 265 of file builder.h.

Constructor & Destructor Documentation

MotionRequestBuilder() [1/4]

MotionRequestBuilder::MotionRequestBuilder

(

const RobotConstPtr &

robot

)

Constructor.

Parameters

[in]

robot

Robot to build planning problem for.

Definition at line 27 of file builder.cpp.

                                                                     : robot_(robot)
{
    initialize();
 
    robot_state::RobotState start_state(robot_->getModelConst());
    start_state.setToDefaultValues();
 
    moveit::core::robotStateToRobotStateMsg(start_state, request_.start_state);
}

MotionRequestBuilder() [2/4]

MotionRequestBuilder::MotionRequestBuilder	(	const RobotConstPtr &	robot,
		const std::string &	group_name,
		const std::string &	planner_config = ""
	)

Constructor. Set planner config and group as well.

Parameters

[in]	robot	Robot to build planning problem for.
[in]	group_name	The motion planning group to build the request for.
[in]	planner_config	Desired planning configuration to use.

Definition at line 37 of file builder.cpp.

  : MotionRequestBuilder(robot)
{
    setPlanningGroup(group_name);
 
    if (not planner_config.empty())
        setConfig(planner_config);
}

MotionRequestBuilder() [3/4]

MotionRequestBuilder::MotionRequestBuilder	(	const PlannerConstPtr &	planner,
		const std::string &	group_name,
		const std::string &	planner_config = ""
	)

Constructor.

Parameters

[in]	planner	The motion planner to build a request for.
[in]	group_name	The motion planning group to build the request for.
[in]	planner_config	Desired planning configuration to use.

Definition at line 47 of file builder.cpp.

  : MotionRequestBuilder(planner->getRobot())
{
    setPlanningGroup(group_name);
    setPlanner(planner);
 
    if (not planner_config.empty())
        setConfig(planner_config);
}

MotionRequestBuilder() [4/4]

MotionRequestBuilder::MotionRequestBuilder

(

const MotionRequestBuilder &

other

)

Copy Constructor.

Parameters

[in]

other

Request to copy.

Definition at line 58 of file builder.cpp.

  : MotionRequestBuilder(other.getRobot())
{
    request_ = other.getRequestConst();
 
    const auto &planner = other.getPlanner();
    if (planner)
        setPlanner(planner);
}

Member Function Documentation

addCylinderSideGrasp()

void MotionRequestBuilder::addCylinderSideGrasp	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		const GeometryConstPtr &	cylinder,
		double	distance,
		double	depth,
		unsigned int	n
	)

Adds a set of regions to grasp a cylinder from the side. This function assumes the X-axis of the end-effector frame ee_name points "forward" for grasping.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose.
[in]	pose	The pose of the frame to be rotated about.
[in]	cylinder	The cylinder to grasp.
[in]	distance	The distance from the cylinder to place the regions.
[in]	depth	The depth of boxes to create.
[in]	n	The number of regions to create.

Definition at line 355 of file builder.cpp.

{
    // Grasping region to tile
    auto box = Geometry::makeBox(depth, depth, cylinder->getDimensions()[1]);
    RobotPose offset(Eigen::Translation3d(cylinder->getDimensions()[0] + distance, 0, 0));
 
    Eigen::Quaterniond orientation = Eigen::AngleAxisd(-constants::pi, Eigen::Vector3d::UnitX())  //
                                     * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())             //
                                     * Eigen::AngleAxisd(constants::pi, Eigen::Vector3d::UnitZ());
 
    addGoalRotaryTile(ee_name, base_name,                          //
                      pose, box, orientation, {0.01, 0.01, 0.01},  //
                      offset, Eigen::Vector3d::UnitZ(), n);
}

addGoalConfiguration() [1/3]

void MotionRequestBuilder::addGoalConfiguration

(

const robot_state::RobotState &

state

)

Add a goal configuration from a robot state.

Parameters

[in]

state

The robot state to set ass reference. Usually from robowflex::Robot::getScratchState().

Definition at line 256 of file builder.cpp.

{
    if (not jmg_)
    {
        RBX_ERROR("No planning group set!");
        throw Exception(1, "No planning group set!");
    }
 
    incrementVersion();
    request_.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints(state, jmg_));
}

addGoalConfiguration() [2/3]

void MotionRequestBuilder::addGoalConfiguration

(

const robot_state::RobotStatePtr &

state

)

Add a goal configuration from a robot state.

Parameters

[in]

state

The robot state to set as pointer. Usually from robowflex::Robot::getScratchState().

Definition at line 251 of file builder.cpp.

{
    addGoalConfiguration(*state);
}

addGoalConfiguration() [3/3]

void MotionRequestBuilder::addGoalConfiguration

(

const std::vector< double > &

joints

)

Add a goal configuration from a vector joints. All joints are assumed to be specified and in the default order.

Parameters

[in]

joints

The values of the joints to set.

Definition at line 234 of file builder.cpp.

{
    if (not jmg_)
    {
        RBX_ERROR("No planning group set!");
        throw Exception(1, "No planning group set!");
    }
 
    incrementVersion();
 
    robot_state::RobotStatePtr state;
    state.reset(new robot_state::RobotState(robot_->getModelConst()));
    state->setJointGroupPositions(jmg_, joints);
 
    addGoalConfiguration(state);
}

addGoalFromIKQuery()

void MotionRequestBuilder::addGoalFromIKQuery

(

const Robot::IKQuery &

query

)

Add an IK query as a goal pose.

Parameters

[in]

query

IK query to construct goal from.

Definition at line 268 of file builder.cpp.

{
    if (not jmg_)
    {
        RBX_ERROR("No planning group set!");
        throw Exception(1, "No planning group set!");
    }
 
    if (group_name_ != query.group)
    {
        RBX_ERROR("Planning group in IK query `%1%` not the same as request `%2%`", query.group, group_name_);
        throw Exception(1, "Mismatched query groups!");
    }
 
    if (query.regions.size() > 1)
    {
        RBX_ERROR("Cannot set goal request from IK query with multiple targets!");
        throw Exception(1, "Tried to set goal from multi-target request!");
    }
 
    std::string tip_to_use = query.tips[0];
    if (tip_to_use.empty())
    {
        const auto &tips = robot_->getSolverTipFrames(group_name_);
        if (tips.empty() or tips.size() > 1)
        {
            RBX_ERROR("Unable to find tip frame for request.");
            throw Exception(1, "Unable to find tip frame for request.");
        }
 
        tip_to_use = tips[0];
    }
 
    const std::string &base = robot_->getSolverBaseFrame(group_name_);
    if (base.empty())
    {
        RBX_ERROR("Failed to get base frame for request.");
        throw Exception(1, "Unable to find base frame for request.");
    }
 
    addGoalRegion(tip_to_use, base, query.region_poses[0], query.regions[0], query.orientations[0],
                  query.tolerances[0]);
}

addGoalPose()

void MotionRequestBuilder::addGoalPose	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		double	tolerance = 0.001
	)

Add a goal pose for the end-effector ee_name. Generates a sphere with radius tolerance as well as orientation tolerances of tolerance from pose.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose.
[in]	pose	The pose of the end-effector in base_frame.
[in]	tolerance	The tolerance to put on the pose.

Definition at line 312 of file builder.cpp.

{
    auto copy = pose;
    Eigen::Quaterniond orientation(pose.rotation());
    copy.linear() = Eigen::Matrix3d::Identity();
    addGoalRegion(ee_name, base_name,                     //
                  copy, Geometry::makeSphere(tolerance),  //
                  orientation, {tolerance, tolerance, tolerance});
}

addGoalRegion()

void MotionRequestBuilder::addGoalRegion	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		const GeometryConstPtr &	geometry,
		const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	tolerances
	)

Add a goal region for an end-effector ee_name. Sets the position constraint from geometry at a pose pose, and the orientation constraint from orientation and XYZ Euler angle tolerances tolerances.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose and orientation.
[in]	pose	The pose of geometry in base_frame.
[in]	geometry	The geometry describing the position constraint.
[in]	orientation	The desired orientation.
[in]	tolerances	XYZ Euler angle tolerances about orientation.

Definition at line 323 of file builder.cpp.

{
    incrementVersion();
 
    moveit_msgs::Constraints constraints;
 
    constraints.position_constraints.push_back(TF::getPositionConstraint(ee_name, base_name, pose, geometry));
    constraints.orientation_constraints.push_back(
        TF::getOrientationConstraint(ee_name, base_name, orientation, tolerances));
 
    request_.goal_constraints.push_back(constraints);
}

addGoalRotaryTile()

void MotionRequestBuilder::addGoalRotaryTile	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		const GeometryConstPtr &	geometry,
		const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	tolerances,
		const RobotPose &	offset,
		const Eigen::Vector3d &	axis,
		unsigned int	n
	)

Tiles some geometry around a pose in base_name for the end-effector ee_name. The geometry is placed at offset from pose, and n copies are placed evenly rotated about axis. The desired orientation is also rotated about the axis and set for each copy.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose and orientation.
[in]	pose	The pose of the frame to be rotated about.
[in]	geometry	The geometry describing the position constraint.
[in]	orientation	The desired orientation.
[in]	tolerances	XYZ Euler angle tolerances about orientation.
[in]	offset	Offset of the goal geometry from pose.
[in]	axis	Axis to rotation the goal geometry and orientation about in pose.
[in]	n	Number of rotations (evenly divided around the circle).

Definition at line 339 of file builder.cpp.

{
    for (double angle = 0; angle < constants::two_pi; angle += constants::two_pi / n)
    {
        Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axis));
        RobotPose new_pose = pose * rotation * offset;
        Eigen::Quaterniond new_orientation(rotation * orientation);
 
        addGoalRegion(ee_name, base_name, new_pose, geometry, new_orientation, tolerances);
    }
}

addPathOrientationConstraint()

void MotionRequestBuilder::addPathOrientationConstraint	(	const std::string &	ee_name,
		const std::string &	base_name,
		const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	tolerances
	)

Set an orientation constraint on the path. Sets the orientation constraint from orientation and XYZ Euler angle tolerances tolerances.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of orientation.
[in]	orientation	The desired orientation.
[in]	tolerances	XYZ Euler angle tolerances about orientation.

Definition at line 477 of file builder.cpp.

{
    incrementVersion();
    request_.path_constraints.orientation_constraints.push_back(
        TF::getOrientationConstraint(ee_name, base_name, orientation, tolerances));
}

addPathPoseConstraint()

void MotionRequestBuilder::addPathPoseConstraint	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		const GeometryConstPtr &	geometry,
		const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	tolerances
	)

Set a pose constraint on the path. Sets the position constraint from geometry at a pose pose, and the orientation constraint from orientation and XYZ Euler angle tolerances tolerances.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose and orientation.
[in]	pose	The pose of geometry in base_frame.
[in]	geometry	The geometry describing the position constraint.
[in]	orientation	The desired orientation.
[in]	tolerances	XYZ Euler angle tolerances about orientation.

Definition at line 460 of file builder.cpp.

{
    addPathPositionConstraint(ee_name, base_name, pose, geometry);
    addPathOrientationConstraint(ee_name, base_name, orientation, tolerances);
}

addPathPositionConstraint()

void MotionRequestBuilder::addPathPositionConstraint	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		const GeometryConstPtr &	geometry
	)

Set a position constraint on the path. Sets the position constraint from geometry at a pose pose.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose.
[in]	pose	The pose of geometry in base_frame.
[in]	geometry	The geometry describing the position constraint.

Definition at line 469 of file builder.cpp.

{
    incrementVersion();
    request_.path_constraints.position_constraints.push_back(
        TF::getPositionConstraint(ee_name, base_name, pose, geometry));
}

attachObjectToStart()

bool MotionRequestBuilder::attachObjectToStart	(	ScenePtr	scene,
		const std::string &	object
	)

Attach an object to the current request start state. Uses object from scene, and modifies the underlying scene state. This uses attachObject() on the scene, so note that the attached object will have to be re-added to the scene.

Parameters

[in]	scene	Scene to use objects from. Will be modified.
[in]	object	Object to attach.

Returns

True on success, false on failure.

Definition at line 217 of file builder.cpp.

{
    // Attach object to current start configuration.
    const auto &start = getStartConfiguration();
    if (not scene->attachObject(*start, object))
        return false;
 
    useSceneStateAsStart(scene);
    return true;
}

attachObjectToStartConst()

bool MotionRequestBuilder::attachObjectToStartConst	(	const SceneConstPtr &	scene,
		const std::string &	object
	)

Attach an object to the current request start state. Uses object from scene, but does not modify the underlying scene. Be aware that attached objects can collide with themselves if not removed from the provided scene.

Parameters

[in]	scene	Scene to use objects from.
[in]	object	Object to attach.

Returns

True on success, false on failure.

Definition at line 228 of file builder.cpp.

{
    auto copy = scene->deepCopy();
    return attachObjectToStart(copy, object);
}

clearGoals()

void MotionRequestBuilder::clearGoals

(

)

Clears all goals.

Definition at line 442 of file builder.cpp.

{
    incrementVersion();
    request_.goal_constraints.clear();
}

clone()

MotionRequestBuilderPtr MotionRequestBuilder::clone

(

)

const

Clone this request.

Returns

A copy of this request.

Definition at line 68 of file builder.cpp.

{
    return std::make_shared<MotionRequestBuilder>(*this);
}

fromYAMLFile()

bool MotionRequestBuilder::fromYAMLFile

(

const std::string &

file

)

Load a planning request from a YAML file file.

Parameters

[in]

file

The name of the file to load the request from.

Returns

True on success, false on failure.

Definition at line 548 of file builder.cpp.

{
    incrementVersion();
 
    bool success = IO::fromYAMLFile(request_, file);
    setPlanningGroup(request_.group_name);
    return success;
}

getGoalConfiguration()

robot_state::RobotStatePtr MotionRequestBuilder::getGoalConfiguration

(

)

const

Get the goal state of the request as a robot state.

Returns

The goal state.

Definition at line 509 of file builder.cpp.

{
    auto goal_state = robot_->allocState();
 
    if (request_.goal_constraints.size() != 1)
    {
        RBX_ERROR("Ambiguous goal, %lu goal goal_constraints exist, returning default goal",
                  request_.goal_constraints.size());
        return goal_state;
    }
 
    if (request_.goal_constraints[0].joint_constraints.empty())
    {
        RBX_ERROR("No joint constraints specified, returning default goal");
        return goal_state;
    }
 
    std::map<std::string, double> variable_map;
    for (const auto &joint : request_.goal_constraints[0].joint_constraints)
        variable_map[joint.joint_name] = joint.position;
 
    // Start state includes attached objects and values for the non-group links.
    moveit::core::robotStateMsgToRobotState(request_.start_state, *goal_state);
    goal_state->setVariablePositions(variable_map);
    goal_state->update(true);
 
    return goal_state;
}

getPathConstraints()

moveit_msgs::Constraints & MotionRequestBuilder::getPathConstraints

(

)

Get a reference to the current path constraints on the motion planning request.

Returns

The motion planning request.

Definition at line 487 of file builder.cpp.

{
    incrementVersion();
    return request_.path_constraints;
}

getPlanner()

const PlannerConstPtr & MotionRequestBuilder::getPlanner

(

)

const

Get the planner for this request.

Returns

The planner. Will be null if not set.

Definition at line 562 of file builder.cpp.

{
    return planner_;
}

getPlannerConfig()

const std::string & MotionRequestBuilder::getPlannerConfig

(

)

const

Get the planner config.

Returns

The current planner config used.

Definition at line 572 of file builder.cpp.

{
    return request_.planner_id;
}

getPlanningGroup()

const std::string & MotionRequestBuilder::getPlanningGroup

(

)

const

Get the planning group.

Returns

The current planning group used.

Definition at line 567 of file builder.cpp.

{
    return group_name_;
}

getRequest()

planning_interface::MotionPlanRequest & MotionRequestBuilder::getRequest

(

)

Get a reference to the currently built motion planning request.

Returns

The motion planning request.

Definition at line 493 of file builder.cpp.

{
    incrementVersion();
    return request_;
}

getRequestConst()

const planning_interface::MotionPlanRequest & MotionRequestBuilder::getRequestConst

(

)

const

Get a const reference to the currently built motion planning request.

Returns

The motion planning request.

Definition at line 538 of file builder.cpp.

{
    return request_;
}

getRobot()

const RobotConstPtr & MotionRequestBuilder::getRobot

(

)

const

Get the robot for this request.

Returns

The robot.

Definition at line 557 of file builder.cpp.

{
    return robot_;
}

getStartConfiguration()

robot_state::RobotStatePtr MotionRequestBuilder::getStartConfiguration

(

)

const

Get the start state of the request as a robot state.

Returns

The start state.

Definition at line 499 of file builder.cpp.

{
    auto start_state = robot_->allocState();
 
    moveit::core::robotStateMsgToRobotState(request_.start_state, *start_state);
    start_state->update(true);
 
    return start_state;
}

initialize()

void MotionRequestBuilder::initialize

(

)

Sets defaults.

Definition at line 73 of file builder.cpp.

{
    setConfig(DEFAULT_CONFIG);
 
    setWorkspaceBounds(Eigen::Vector3d::Constant(-constants::default_workspace_bound),
                       Eigen::Vector3d::Constant(constants::default_workspace_bound));
    request_.allowed_planning_time = constants::default_allowed_planning_time;
}

precomputeGoalConfigurations()

void MotionRequestBuilder::precomputeGoalConfigurations	(	std::size_t	n_samples,
		const ScenePtr &	scene,
		const ConfigurationValidityCallback &	callback = {}
	)

Override the goals of this motion request with precomputed goal configurations (from the specified regions).

That is, rather than a set of sampleable goal regions, the request will have n_samples goal configurations, all sampled from the prior goal regions.

Parameters

[in]	n_samples	Number of samples to precompute.
[in]	scene	Scene to collision check against.
[in]	callback	If provided, will only keep samples that are valid according to callback.

Definition at line 412 of file builder.cpp.

{
    // Allocate samplers for each region
    constraint_samplers::ConstraintSamplerManager manager;
    std::vector<constraint_samplers::ConstraintSamplerPtr> samplers;
    for (const auto &goal : request_.goal_constraints)
    {
        samplers.emplace_back(manager.selectSampler(scene->getSceneConst(), group_name_, goal));
        samplers.back()->setGroupStateValidityCallback(scene->getGSVCF(false));
    }
 
    clearGoals();
 
    // Clone start
    robot_state::RobotState state = *robot_->getScratchStateConst();
 
    // Sample n_samples to add to new request
    std::size_t n = n_samples;
    while (n)
    {
        auto sampler = RNG::uniformSample(samplers);
        if (sampler->sample(state) and (not callback or callback(state)))
        {
            addGoalConfiguration(state);
            n--;
        }
    }
}

setAllowedPlanningTime()

void MotionRequestBuilder::setAllowedPlanningTime

(

double

allowed_planning_time

)

Set the allowed planning time in the request.

Parameters

[in]

allowed_planning_time

The allowed planning time.

Definition at line 448 of file builder.cpp.

{
    incrementVersion();
    request_.allowed_planning_time = allowed_planning_time;
}

setConfig()

bool MotionRequestBuilder::setConfig

(

const std::string &

requested_config

)

Set the planning configuration to use for the motion planning request. Attempts to match requested_config with the planner configuration offered by planner_ that is the shortest configuration that contains requested_config as a substring. For example, specifying RRTConnect will match RRTConnectkConfigDefault, and specifying RRT will match RRTkConfigDefault and not RRTConnectkConfigDefault.

Parameters

[in]

requested_config

The planner config to find and use.

Returns

True if the requested_config is found, false otherwise.

Definition at line 114 of file builder.cpp.

{
    if (not planner_)
    {
        RBX_INFO("No planner set! Using requested config `%s`", requested_config);
        request_.planner_id = requested_config;
        return true;
    }
 
    const auto &configs = planner_->getPlannerConfigs();
 
    std::vector<std::reference_wrapper<const std::string>> matches;
    for (const auto &config : configs)
    {
        if (config.find(requested_config) != std::string::npos)
            matches.emplace_back(config);
    }
 
    if (matches.empty())
        return false;
 
    const auto &found =
        std::min_element(matches.begin(), matches.end(),
                         [](const std::string &a, const std::string &b) { return a.size() < b.size(); });
 
    incrementVersion();
 
    request_.planner_id = *found;
    RBX_INFO("Requested Config: `%s`: Using planning config `%s`", requested_config, request_.planner_id);
    return true;
}

setGoalConfiguration() [1/3]

void MotionRequestBuilder::setGoalConfiguration

(

const robot_state::RobotState &

state

)

Set the goal configuration from a robot state.

Parameters

[in]

state

The robot state to set as reference. Usually from robowflex::Robot::getScratchState().

Definition at line 384 of file builder.cpp.

{
    clearGoals();
    addGoalConfiguration(state);
}

setGoalConfiguration() [2/3]

void MotionRequestBuilder::setGoalConfiguration

(

const robot_state::RobotStatePtr &

state

)

Set the goal configuration from a robot state.

Parameters

[in]

state

The robot state to set as pointer. Usually from robowflex::Robot::getScratchState().

Definition at line 378 of file builder.cpp.

{
    clearGoals();
    addGoalConfiguration(state);
}

setGoalConfiguration() [3/3]

void MotionRequestBuilder::setGoalConfiguration

(

const std::vector< double > &

joints

)

Set the goal configuration from a vector joints. All joints are assumed to be specified and in the default order.

Parameters

[in]

joints

The values of the joints to set.

Definition at line 372 of file builder.cpp.

{
    clearGoals();
    addGoalConfiguration(joints);
}

setGoalFromIKQuery()

void MotionRequestBuilder::setGoalFromIKQuery

(

const Robot::IKQuery &

query

)

Set the goal pose from an IK query.

Parameters

[in]

query

IK query to construct goal from.

Definition at line 390 of file builder.cpp.

{
    clearGoals();
    addGoalFromIKQuery(query);
}

setGoalPose()

void MotionRequestBuilder::setGoalPose	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		double	tolerance = 0.001
	)

Set a goal pose for the end-effector ee_name. Generates a sphere with radius tolerance as well as orientation tolerances of tolerance from pose.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose.
[in]	pose	The pose of the end-effector in base_frame.
[in]	tolerance	The tolerance to put on the pose.

Definition at line 396 of file builder.cpp.

{
    clearGoals();
    addGoalPose(ee_name, base_name, pose, tolerance);
}

setGoalRegion()

void MotionRequestBuilder::setGoalRegion	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		const GeometryConstPtr &	geometry,
		const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	tolerances
	)

Set a goal region for an end-effector ee_name. Sets the position constraint from geometry at a pose pose, and the orientation constraint from orientation and XYZ Euler angle tolerances tolerances.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The name of the frame of reference of pose and orientation.
[in]	pose	The pose of geometry in base_frame.
[in]	geometry	The geometry describing the position constraint.
[in]	orientation	The desired orientation.
[in]	tolerances	XYZ Euler angle tolerances about orientation.

Definition at line 403 of file builder.cpp.

{
    clearGoals();
    addGoalRegion(ee_name, base_name, pose, geometry, orientation, tolerances);
}

setNumPlanningAttempts()

void MotionRequestBuilder::setNumPlanningAttempts

(

unsigned int

num_planning_attempts

)

Set the number of planning attemps in the request.

Parameters

[in]

num_planning_attempts

The required time for planning attempts.

Definition at line 454 of file builder.cpp.

{
    incrementVersion();
    request_.num_planning_attempts = num_planning_attempts;
}

setPlanner()

void MotionRequestBuilder::setPlanner

(

const PlannerConstPtr &

planner

)

Set the Robowflex planner to use.

Parameters

[in]

planner

Robowflex planner to build request for.

Definition at line 82 of file builder.cpp.

{
    const auto &rname = robot_->getName();
    const auto &pname = planner->getRobot()->getName();
 
    if (rname != pname)
    {
        RBX_ERROR("Conflicting robots `%s` and `%s` in request builder!", rname, pname);
        throw Exception(1, "Invalid planner!");
    }
 
    planner_ = planner;
}

setPlanningGroup()

void MotionRequestBuilder::setPlanningGroup

(

const std::string &

group_name

)

Set the planning group to use for this request builder.

Parameters

[in]

group_name

Name of planning group.

Definition at line 96 of file builder.cpp.

{
    const auto &model = robot_->getModelConst();
 
    if (model->hasJointModelGroup(group_name))
    {
        group_name_ = group_name;
        jmg_ = robot_->getModelConst()->getJointModelGroup(group_name_);
 
        request_.group_name = group_name_;
    }
    else
    {
        RBX_ERROR("Joint group `%s` does not exist in robot!", group_name);
        throw Exception(1, "Invalid joint group name!");
    }
}

setStartConfiguration() [1/3]

void MotionRequestBuilder::setStartConfiguration

(

const robot_state::RobotState &

state

)

Set the start configuration from a robot state.

Parameters

[in]

state

The robot state to set. Usually from robowflex::Robot::getScratchState().

Definition at line 201 of file builder.cpp.

{
    incrementVersion();
    moveit::core::robotStateToRobotStateMsg(state, request_.start_state);
}

setStartConfiguration() [2/3]

void MotionRequestBuilder::setStartConfiguration

(

const robot_state::RobotStatePtr &

state

)

Set the start configuration from a robot state.

Parameters

[in]

state

The robot state to set. Usually from robowflex::Robot::getScratchState().

Definition at line 207 of file builder.cpp.

{
    setStartConfiguration(*state);
}

setStartConfiguration() [3/3]

void MotionRequestBuilder::setStartConfiguration

(

const std::vector< double > &

joints

)

Set the start configuration from a vector joints. All joints are assumed to be specified and in the default order.

Parameters

[in]

joints

The values of the joints to set.

Definition at line 189 of file builder.cpp.

{
    if (not jmg_)
    {
        RBX_ERROR("No planning group set!");
        throw Exception(1, "No planning group set!");
    }
 
    incrementVersion();
    robot_->setStateMsgGroupState(request_.start_state, request_.group_name, joints);
}

setWorkspaceBounds() [1/2]

void MotionRequestBuilder::setWorkspaceBounds	(	const Eigen::Ref< const Eigen::VectorXd > &	min,
		const Eigen::Ref< const Eigen::VectorXd > &	max
	)

Sets workspace bounds of the planning request.

Parameters

[in]	min	XYZ vector of minimum workspace bounds.
[in]	max	XYZ vector of maximum workspace bounds.

Definition at line 152 of file builder.cpp.

{
    moveit_msgs::WorkspaceParameters wp;
    wp.min_corner.x = min[0];
    wp.min_corner.y = min[1];
    wp.min_corner.z = min[2];
    wp.max_corner.x = max[0];
    wp.max_corner.y = max[1];
    wp.max_corner.z = max[2];
 
    setWorkspaceBounds(wp);
}

setWorkspaceBounds() [2/2]

void MotionRequestBuilder::setWorkspaceBounds

(

const moveit_msgs::WorkspaceParameters &

wp

)

Sets workspace bounds of the planning request.

Parameters

[in]

wp

The workspace parameters to use.

Definition at line 146 of file builder.cpp.

{
    incrementVersion();
    request_.workspace_parameters = wp;
}

swapStartWithGoal()

bool MotionRequestBuilder::swapStartWithGoal

(

)

Swap the start and goal configurations. This is only possible when a single joint goal is specified, otherwise an error is raised.

Returns

True upon success, False otherwise.

Definition at line 166 of file builder.cpp.

{
    if (request_.goal_constraints.size() != 1)
    {
        RBX_ERROR("Multiple goal constraints exist, cannot swap start with goal");
        return false;
    }
 
    if (request_.goal_constraints[0].joint_constraints.empty())
    {
        RBX_ERROR("No joint goal is specified, cannot swap start with goal");
        return false;
    }
 
    const auto &start = getStartConfiguration();
    const auto &goal = getGoalConfiguration();
    clearGoals();
 
    setStartConfiguration(goal);
    setGoalConfiguration(start);
    return true;
}

toYAMLFile()

bool MotionRequestBuilder::toYAMLFile

(

const std::string &

file

)

const

Serialize the motion planning request to a YAML file file.

Parameters

[in]

file

The name of the file to serialize the request to.

Returns

True on success, false on failure.

Definition at line 543 of file builder.cpp.

{
    return IO::YAMLToFile(IO::toNode(request_), file);
}

useSceneStateAsStart()

void MotionRequestBuilder::useSceneStateAsStart

(

const SceneConstPtr &

scene

)

Use the current scene state for the starting configuration.

Parameters

[in]

scene

Scene to use state from.

Definition at line 212 of file builder.cpp.

{
    setStartConfiguration(scene->getCurrentStateConst());
}

Member Data Documentation

DEFAULT_CONFIG

const std::string MotionRequestBuilder::DEFAULT_CONFIG = "RRTConnectkConfigDefault"

staticprivate

Default planner configuration to use.

Definition at line 446 of file builder.h.

group_name_

std::string robowflex::MotionRequestBuilder::group_name_

private

The group to plan for.

Definition at line 440 of file builder.h.

jmg_

robot_model::JointModelGroup* robowflex::MotionRequestBuilder::jmg_ {nullptr}

private

The joint model group of the robot (from group_name_)

Definition at line 441 of file builder.h.

planner_

PlannerConstPtr robowflex::MotionRequestBuilder::planner_

private

The planner to build the request for.

Definition at line 439 of file builder.h.

request_

planning_interface::MotionPlanRequest robowflex::MotionRequestBuilder::request_

private

The build request.

Definition at line 444 of file builder.h.

robot_

const RobotConstPtr robowflex::MotionRequestBuilder::robot_

private

The robot to build the request for.

Definition at line 437 of file builder.h.

---

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

• builder.h
• builder.cpp