robowflex::darts::PlanBuilder Class Reference

A helper class to setup common OMPL structures for planning. More...

#include <planning.h>

Inheritance diagram for robowflex::darts::PlanBuilder:

Public Member Functions
Setup and Initialization.
	PlanBuilder (WorldPtr world)
	Constructor. More...
void	initialize ()
	Initialize OMPL structures. Only call this once all constraints, groups, and other parameters have been set for the builder. More...
void	setup ()
	Calls setup routines on OMPL structures. Call after all setup is done and builder is initialized. More...
MoveIt Planning Messages
void	getWorkspaceBoundsFromMessage (const moveit_msgs::MotionPlanRequest &msg)
	Get workspace bounds from a planning request. More...
void	getGroupFromMessage (const std::string &robot_name, const moveit_msgs::MotionPlanRequest &msg)
	Set group for planning from message. More...
void	getStartFromMessage (const std::string &robot_name, const moveit_msgs::MotionPlanRequest &msg)
	Get the start state from the request message. More...
void	getPathConstraintsFromMessage (const std::string &robot_name, const moveit_msgs::MotionPlanRequest &msg)
	Get the set of path constraints from the request message. More...
ompl::base::GoalPtr	getGoalFromMessage (const std::string &robot_name, const moveit_msgs::MotionPlanRequest &msg)
	Get the goal constraints from the planning request message. More...
TSRPtr	fromPositionConstraint (const std::string &robot_name, const moveit_msgs::PositionConstraint &msg) const
	Get a TSR from an position constraint. More...
TSRPtr	fromOrientationConstraint (const std::string &robot_name, const moveit_msgs::OrientationConstraint &msg) const
	Get a TSR from an orientation constraint. More...
JointRegionGoalPtr	fromJointConstraints (const std::vector< moveit_msgs::JointConstraint > &msgs) const
	Get a joint region goal from a message. More...
ompl::base::GoalPtr	fromMessage (const std::string &robot_name, const moveit_msgs::MotionPlanRequest &msg)
	Use all of the planning request message to setup motion planning. More...
Other Settings
void	addGroup (const std::string &robot, const std::string &name, std::size_t cyclic=0)
	Add a robot's planning group to the controlled DoF in the state space. More...
void	addConstraint (const TSRPtr &tsr)
	Adds a TSR as a path constraint to the problem. More...
Start Configuration
void	setStartConfigurationFromWorld ()
	Set the start configuration from the current state of the world. More...
void	setStartConfiguration (const Eigen::Ref< const Eigen::VectorXd > &q)
	Set the start configuration from a configuration. More...
void	setStartConfiguration (const std::vector< double > &q)
	Set the start configuration from a configuration. More...
void	sampleStartConfiguration ()
	Sample a valid start configuration. More...
Goals
std::shared_ptr< ompl::base::GoalStates >	getGoalConfigurationFromWorld ()
	Set the goal configuration from the current state of the world. More...
std::shared_ptr< ompl::base::GoalStates >	getGoalConfiguration (const Eigen::Ref< const Eigen::VectorXd > &q)
	Set the goal configuration from a configuration. More...
std::shared_ptr< ompl::base::GoalStates >	getGoalConfiguration (const std::vector< double > &q)
	Set the goal configuration from a configuration. More...
TSRGoalPtr	getGoalTSR (const TSRPtr &tsr)
	Set a TSR as the goal for the planning problem (will create a TSRGoal) More...
TSRGoalPtr	getGoalTSR (const std::vector< TSRPtr > &tsrs)
	Set a set of TSRs as the goal for the planning problem (will create a TSRGoal) More...
std::shared_ptr< ompl::base::GoalStates >	sampleGoalConfiguration ()
	Sample a valid goal configuration. More...
void	setGoal (const ompl::base::GoalPtr &goal)
	Set the goal for the problem. More...
Other Functions
ompl::base::State *	sampleState () const
	Sample a valid state. More...
ompl::geometric::PathGeometric	getSolutionPath (bool simplify=true, bool interpolate=true) const
	Get the solution path from a successful plan. More...
Public Member Functions inherited from robowflex::darts::ConstraintExtractor
	ConstraintExtractor ()=default
	Constructor. More...
	ConstraintExtractor (const ompl::base::SpaceInformationPtr &si)
	Constructor. More...
const StateSpace *	getSpace () const
	Gets the underlying state space from the space information. More...
void	setSpaceInformation (const ompl::base::SpaceInformationPtr &si)
	Set space information used for constraint extraction. More...
StateSpace::StateType *	toState (ompl::base::State *state) const
	Extract underlying state from a base state. More...
const StateSpace::StateType *	toStateConst (const ompl::base::State *state) const
	Extract underlying state from a base state. More...
StateSpace::StateType *	fromConstrainedState (ompl::base::State *state) const
	Access the underlying state from a constrained OMPL state. More...
const StateSpace::StateType *	fromConstrainedStateConst (const ompl::base::State *state) const
	Access the underlying state from a constrained OMPL state. More...
StateSpace::StateType *	fromUnconstrainedState (ompl::base::State *state) const
	Access the underlying state from an unconstrained OMPL state. More...
const StateSpace::StateType *	fromUnconstrainedStateConst (const ompl::base::State *state) const
	Access the underlying state from an unconstrained OMPL state. More...

Public Attributes
StateSpacePtr	rspace {nullptr}
	Underlying Robot State Space. More...
ompl::base::StateSpacePtr	space {nullptr}
	Actual OMPL State Space (might be constrained). More...
ompl::base::SpaceInformationPtr	rinfo {nullptr}
	Underlying Space Information. More...
ompl::base::SpaceInformationPtr	info {nullptr}
	Actual Space Information. More...
ompl::geometric::SimpleSetupPtr	ss {nullptr}
	Simple Setup. More...
WorldPtr	world
	World used for planning. More...
std::vector< TSRPtr >	path_constraints
	Path Constraints. More...
TSRConstraintPtr	constraint {nullptr}
	OMPL Constraint for Path Constraints. More...
Eigen::VectorXd	start
	Start configuration. More...
struct {
struct {
double   delta {0.2}
	Manifold delta. More...
double   lambda {5.0}
	Manifold lambda. More...
}   constraints
	Constrained planning options. More...
}	options
	Hyperparameter options. More...

Protected Member Functions
void	initializeConstrained ()
	Initialize when path constraints are present. More...
void	initializeUnconstrained ()
	Initialize when no path constraints are present. More...
void	setStateValidityChecker ()
	Set the state validity checker on the simple setup. More...
ompl::base::StateValidityCheckerFn	getSVCUnconstrained ()
	Get a state validity checker for the unconstrained space. More...
ompl::base::StateValidityCheckerFn	getSVCConstrained ()
	Get a state validity checker for the unconstrained space. More...

Protected Attributes
ompl::base::GoalPtr	goal_ {nullptr}
	Desired goal. More...

Detailed Description

A helper class to setup common OMPL structures for planning.

Definition at line 221 of file robowflex_dart/include/robowflex_dart/planning.h.

Constructor & Destructor Documentation

PlanBuilder()

PlanBuilder::PlanBuilder

(

WorldPtr

world

)

Constructor.

Parameters

[in]

world

World to use for planning.

PlanBuilder

Definition at line 243 of file robowflex_dart/src/planning.cpp.

                                       : rspace(std::make_shared<StateSpace>(world)), world(world)
{
}

Member Function Documentation

addConstraint()

void PlanBuilder::addConstraint

(

const TSRPtr &

tsr

)

Adds a TSR as a path constraint to the problem.

Parameters

[in]

tsr

Path constraint.

Definition at line 408 of file robowflex_dart/src/planning.cpp.

{
    path_constraints.emplace_back(tsr);
}

addGroup()

void PlanBuilder::addGroup	(	const std::string &	robot,
		const std::string &	name,
		std::size_t	cyclic = 0
	)

Add a robot's planning group to the controlled DoF in the state space.

Parameters

[in]	robot	Name of the robot.
[in]	name	Name of the group in the robot to add.
[in]	cyclic	If > 0, if the group contains cyclic joints (SO(2), SO(3)) these will be modeled as Rn.

Definition at line 403 of file robowflex_dart/src/planning.cpp.

{
    rspace->addGroup(skeleton, name, cyclic);
}

fromJointConstraints()

JointRegionGoalPtr PlanBuilder::fromJointConstraints

(

const std::vector< moveit_msgs::JointConstraint > &

msgs

)

const

Get a joint region goal from a message.

Parameters

[in]

msgs

Joint constraint messages to create region goal.

Returns

The joint region goal.

Definition at line 287 of file robowflex_dart/src/planning.cpp.

{
    std::size_t n = rspace->getDimension();
    Eigen::VectorXd lower(n);
    Eigen::VectorXd upper(n);
 
    for (const auto &msg : msgs)
    {
        const auto &joint = rspace->getJoint(msg.joint_name);
        joint->getSpaceVars(lower)[0] = msg.position - msg.tolerance_below;
        joint->getSpaceVars(upper)[0] = msg.position + msg.tolerance_above;
    }
 
    return std::make_shared<JointRegionGoal>(*this, lower, upper);
}

fromMessage()

ompl::base::GoalPtr PlanBuilder::fromMessage	(	const std::string &	robot_name,
		const moveit_msgs::MotionPlanRequest &	msg
	)

Use all of the planning request message to setup motion planning.

Parameters

[in]	robot_name	Robot name to use message on.
[in]	msg	Planning request message.

Definition at line 388 of file robowflex_dart/src/planning.cpp.

{
    getWorkspaceBoundsFromMessage(msg);
    getGroupFromMessage(robot_name, msg);
    getStartFromMessage(robot_name, msg);
    getPathConstraintsFromMessage(robot_name, msg);
    initialize();
 
    auto goal = getGoalFromMessage(robot_name, msg);
    setGoal(goal);
 
    return goal;
}

fromOrientationConstraint()

TSRPtr PlanBuilder::fromOrientationConstraint	(	const std::string &	robot_name,
		const moveit_msgs::OrientationConstraint &	msg
	)		const

Get a TSR from an orientation constraint.

Parameters

[in]	robot_name	Robot name to use message on.
[in]	msg	Orientation constraint.

Definition at line 346 of file robowflex_dart/src/planning.cpp.

{
    TSR::Specification spec;
    spec.setFrame(robot_name, msg.link_name);
    if (not msg.header.frame_id.empty() and msg.header.frame_id != "world")
        spec.setBase(robot_name, msg.header.frame_id);
 
    spec.setRotation(TF::quaternionMsgToEigen(msg.orientation));
    spec.setNoPosTolerance();
    spec.setXRotTolerance(msg.absolute_x_axis_tolerance);
    spec.setYRotTolerance(msg.absolute_y_axis_tolerance);
    spec.setZRotTolerance(msg.absolute_z_axis_tolerance);
 
    return std::make_shared<TSR>(world, spec);
}

fromPositionConstraint()

TSRPtr PlanBuilder::fromPositionConstraint	(	const std::string &	robot_name,
		const moveit_msgs::PositionConstraint &	msg
	)		const

Get a TSR from an position constraint.

Parameters

[in]	robot_name	Robot name to use message on.
[in]	msg	Position constraint.

Definition at line 303 of file robowflex_dart/src/planning.cpp.

{
    TSR::Specification spec;
    spec.setFrame(robot_name, msg.link_name);
    if (not msg.header.frame_id.empty() and msg.header.frame_id != "world")
        spec.setBase(robot_name, msg.header.frame_id);
 
    if (not msg.constraint_region.meshes.empty())
    {
        RBX_ERROR("Invalid Position Constraint");
        return nullptr;
    }
 
    if (msg.constraint_region.primitives.size() != 1)
    {
        RBX_ERROR("Invalid Position Constraint");
        return nullptr;
    }
 
    spec.setNoRotTolerance();
 
    // get pose
    spec.setPose(TF::poseMsgToEigen(msg.constraint_region.primitive_poses[0]));
    spec.setPosition(spec.getPosition() + TF::vectorMsgToEigen(msg.target_point_offset));
 
    auto &&primitive = msg.constraint_region.primitives[0];
    if (primitive.type == shape_msgs::SolidPrimitive::BOX)
    {
        spec.setXPosTolerance(primitive.dimensions[0]);
        spec.setYPosTolerance(primitive.dimensions[1]);
        spec.setZPosTolerance(primitive.dimensions[2]);
    }
    else if (primitive.type == shape_msgs::SolidPrimitive::SPHERE)
    {
        spec.setXPosTolerance(primitive.dimensions[0]);
        spec.setYPosTolerance(primitive.dimensions[0]);
        spec.setZPosTolerance(primitive.dimensions[0]);
    }
 
    return std::make_shared<TSR>(world, spec);
}

getGoalConfiguration() [1/2]

std::shared_ptr< ompl::base::GoalStates > PlanBuilder::getGoalConfiguration

(

const Eigen::Ref< const Eigen::VectorXd > &

q

)

Set the goal configuration from a configuration.

Parameters

[in]

q

Configuration.

Definition at line 447 of file robowflex_dart/src/planning.cpp.

{
    if (not space)
        throw std::runtime_error("Builder must be initialized before creating goal!");
 
    ompl::base::ScopedState<> goal_state(space);
    toState(goal_state.get())->data = q;
 
    auto goal = std::make_shared<ompl::base::GoalStates>(info);
    goal->addState(goal_state);
 
    return goal;
}

getGoalConfiguration() [2/2]

std::shared_ptr< ompl::base::GoalStates > PlanBuilder::getGoalConfiguration

(

const std::vector< double > &

q

)

Set the goal configuration from a configuration.

Parameters

[in]

q

Configuration.

Definition at line 461 of file robowflex_dart/src/planning.cpp.

{
    return getGoalConfiguration(Eigen::Map<const Eigen::VectorXd>(q.data(), rspace->getDimension()));
}

getGoalConfigurationFromWorld()

std::shared_ptr< ompl::base::GoalStates > PlanBuilder::getGoalConfigurationFromWorld

(

)

Set the goal configuration from the current state of the world.

Definition at line 439 of file robowflex_dart/src/planning.cpp.

{
    Eigen::VectorXd goal = Eigen::VectorXd::Zero(rspace->getDimension());
    rspace->getWorldState(world, goal);
    return getGoalConfiguration(goal);
}

getGoalFromMessage()

ompl::base::GoalPtr PlanBuilder::getGoalFromMessage	(	const std::string &	robot_name,
		const moveit_msgs::MotionPlanRequest &	msg
	)

Get the goal constraints from the planning request message.

Parameters

[in]	robot_name	Robot name to use message on.
[in]	msg	Planning request message.

Definition at line 372 of file robowflex_dart/src/planning.cpp.

{
    // TODO get other goals as well
    std::vector<TSRPtr> tsrs;
    for (const auto &constraint : msg.goal_constraints[0].position_constraints)
        tsrs.emplace_back(fromPositionConstraint(robot_name, constraint));
    for (const auto &constraint : msg.goal_constraints[0].orientation_constraints)
        tsrs.emplace_back(fromOrientationConstraint(robot_name, constraint));
 
    if (tsrs.empty())
        return fromJointConstraints(msg.goal_constraints[0].joint_constraints);
 
    return getGoalTSR(tsrs);
}

getGoalTSR() [1/2]

TSRGoalPtr PlanBuilder::getGoalTSR

(

const std::vector< TSRPtr > &

tsrs

)

Set a set of TSRs as the goal for the planning problem (will create a TSRGoal)

Parameters

[in]

tsrs

TSRs for the goal.

Definition at line 471 of file robowflex_dart/src/planning.cpp.

{
    if (not info)
        throw std::runtime_error("Builder must be initialized before creating goal!");
 
    return std::make_shared<TSRGoal>(*this, tsrs);
}

getGoalTSR() [2/2]

TSRGoalPtr PlanBuilder::getGoalTSR

(

const TSRPtr &

tsr

)

Set a TSR as the goal for the planning problem (will create a TSRGoal)

Parameters

[in]

tsr

TSR for the goal.

Definition at line 466 of file robowflex_dart/src/planning.cpp.

{
    return getGoalTSR(std::vector<TSRPtr>{tsr});
}

getGroupFromMessage()

void PlanBuilder::getGroupFromMessage	(	const std::string &	robot_name,
		const moveit_msgs::MotionPlanRequest &	msg
	)

Set group for planning from message.

Parameters

[in]	robot_name	Robot name to use message on.
[in]	msg	Planning request message.

Definition at line 260 of file robowflex_dart/src/planning.cpp.

{
    bool cyclic = false;
    if (not msg.path_constraints.position_constraints.empty() or
        not msg.path_constraints.orientation_constraints.empty())
        cyclic = true;
 
    addGroup(robot_name, msg.group_name, cyclic ? 2 : 0);
}

getPathConstraintsFromMessage()

void PlanBuilder::getPathConstraintsFromMessage	(	const std::string &	robot_name,
		const moveit_msgs::MotionPlanRequest &	msg
	)

Get the set of path constraints from the request message.

Parameters

[in]	robot_name	Robot name to use message on.
[in]	msg	Planning request message.

Definition at line 363 of file robowflex_dart/src/planning.cpp.

{
    for (const auto &constraint : msg.path_constraints.position_constraints)
        addConstraint(fromPositionConstraint(robot_name, constraint));
    for (const auto &constraint : msg.path_constraints.orientation_constraints)
        addConstraint(fromOrientationConstraint(robot_name, constraint));
}

getSolutionPath()

ompl::geometric::PathGeometric PlanBuilder::getSolutionPath	(	bool	simplify = true,
		bool	interpolate = true
	)		const

Get the solution path from a successful plan.

Parameters

[in]	simplify	Simplify the solution.
[in]	interpolate	Interpolate the solution.

Definition at line 623 of file robowflex_dart/src/planning.cpp.

{
    if (simplify)
        ss->simplifySolution();
 
    auto path = ss->getSolutionPath();
    if (interpolate)
        path.interpolate();
 
    return path;
}

getStartFromMessage()

void PlanBuilder::getStartFromMessage	(	const std::string &	robot_name,
		const moveit_msgs::MotionPlanRequest &	msg
	)

Get the start state from the request message.

Parameters

[in]	robot_name	Robot name to use message on.
[in]	msg	Planning request message.

Definition at line 271 of file robowflex_dart/src/planning.cpp.

{
    auto robot = world->getRobot(robot_name);
    for (std::size_t i = 0; i < msg.start_state.joint_state.name.size(); ++i)
    {
        std::string name = msg.start_state.joint_state.name[i];
        double value = msg.start_state.joint_state.position[i];
 
        robot->setJoint(name, value);
    }
 
    setStartConfigurationFromWorld();
}

getSVCConstrained()

ompl::base::StateValidityCheckerFn PlanBuilder::getSVCConstrained ( )

protected

Get a state validity checker for the unconstrained space.

Returns

The state validity checker.

Definition at line 610 of file robowflex_dart/src/planning.cpp.

{
    return [&](const ompl::base::State *state) -> bool {
        const auto &as = fromConstrainedStateConst(state);
 
        world->lock();
        rspace->setWorldState(world, as);
        bool r = not world->inCollision();
        world->unlock();
        return r;
    };
}

getSVCUnconstrained()

ompl::base::StateValidityCheckerFn PlanBuilder::getSVCUnconstrained ( )

protected

Get a state validity checker for the unconstrained space.

Returns

The state validity checker.

Definition at line 597 of file robowflex_dart/src/planning.cpp.

{
    return [&](const ompl::base::State *state) -> bool {
        const auto &as = fromUnconstrainedStateConst(state);
 
        world->lock();
        rspace->setWorldState(world, as);
        bool r = not world->inCollision();
        world->unlock();
        return r;
    };
}

getWorkspaceBoundsFromMessage()

void PlanBuilder::getWorkspaceBoundsFromMessage

(

const moveit_msgs::MotionPlanRequest &

msg

)

Get workspace bounds from a planning request.

Parameters

[in]

msg

Planning request message.

Definition at line 247 of file robowflex_dart/src/planning.cpp.

{
    const auto &mic = msg.workspace_parameters.min_corner;
    world->getWorkspaceLow()[0] = mic.x;
    world->getWorkspaceLow()[1] = mic.y;
    world->getWorkspaceLow()[2] = mic.z;
 
    const auto &mac = msg.workspace_parameters.max_corner;
    world->getWorkspaceHigh()[0] = mac.x;
    world->getWorkspaceHigh()[1] = mac.y;
    world->getWorkspaceHigh()[2] = mac.z;
}

initialize()

void PlanBuilder::initialize

(

)

Initialize OMPL structures. Only call this once all constraints, groups, and other parameters have been set for the builder.

Definition at line 501 of file robowflex_dart/src/planning.cpp.

{
    if (path_constraints.empty())
        initializeUnconstrained();
    else
        initializeConstrained();
 
    setSpaceInformation(info);
}

initializeConstrained()

void PlanBuilder::initializeConstrained ( )

protected

Initialize when path constraints are present.

Definition at line 523 of file robowflex_dart/src/planning.cpp.

{
    world->clearIKModules();
    rspace->setMetricSpace(false);
 
    rinfo = std::make_shared<ompl::base::SpaceInformation>(rspace);
    rinfo->setStateValidityChecker(getSVCUnconstrained());
 
    constraint = std::make_shared<TSRConstraint>(rspace, path_constraints);
 
    auto pss = std::make_shared<ompl::base::ProjectedStateSpace>(rspace, constraint);
    space = pss;
    info = std::make_shared<ompl::base::ConstrainedSpaceInformation>(pss);
 
    ss = std::make_shared<ompl::geometric::SimpleSetup>(info);
    setStateValidityChecker();
 
    pss->setDelta(options.constraints.delta);
    pss->setLambda(options.constraints.lambda);
}

initializeUnconstrained()

void PlanBuilder::initializeUnconstrained ( )

protected

Initialize when no path constraints are present.

Definition at line 544 of file robowflex_dart/src/planning.cpp.

{
    space = rspace;
    ss = std::make_shared<ompl::geometric::SimpleSetup>(space);
    setStateValidityChecker();
 
    rinfo = info = ss->getSpaceInformation();
}

sampleGoalConfiguration()

std::shared_ptr< ompl::base::GoalStates > PlanBuilder::sampleGoalConfiguration

(

)

Sample a valid goal configuration.

Definition at line 479 of file robowflex_dart/src/planning.cpp.

{
    if (space)
    {
        auto *state = sampleState();
        auto goal = getGoalConfiguration(toState(state)->data);
        space->freeState(state);
 
        return goal;
    }
 
    return nullptr;
}

sampleStartConfiguration()

void PlanBuilder::sampleStartConfiguration

(

)

Sample a valid start configuration.

Definition at line 429 of file robowflex_dart/src/planning.cpp.

{
    if (space)
    {
        auto *state = sampleState();
        setStartConfiguration(toState(state)->data);
        space->freeState(state);
    }
}

sampleState()

ompl::base::State * PlanBuilder::sampleState

(

)

const

Sample a valid state.

Returns

a Valid state.

Definition at line 553 of file robowflex_dart/src/planning.cpp.

{
    if (space)
    {
        auto sampler = space->allocStateSampler();
        auto *state = space->allocState();
 
        bool valid = false;
        bool constrained = false;
        do
        {
            sampler->sampleUniform(state);
            space->enforceBounds(state);
 
            valid = info->isValid(state);
            if (constraint)
                constrained = constraint->isSatisfied(state);
            else
                constrained = true;
        } while (not valid or not constrained);
 
        return state;
    }
 
    return nullptr;
}

setGoal()

void PlanBuilder::setGoal

(

const ompl::base::GoalPtr &

goal

)

Set the goal for the problem.

Parameters

[in]

goal

Goal to use.

Definition at line 493 of file robowflex_dart/src/planning.cpp.

{
    goal_ = goal;
 
    if (ss)
        ss->setGoal(goal_);
}

setStartConfiguration() [1/2]

void PlanBuilder::setStartConfiguration

(

const Eigen::Ref< const Eigen::VectorXd > &

q

)

Set the start configuration from a configuration.

Parameters

[in]

q

Configuration.

Definition at line 419 of file robowflex_dart/src/planning.cpp.

{
    start = q;
}

setStartConfiguration() [2/2]

void PlanBuilder::setStartConfiguration

(

const std::vector< double > &

q

)

Set the start configuration from a configuration.

Parameters

[in]

q

Configuration.

Definition at line 424 of file robowflex_dart/src/planning.cpp.

{
    setStartConfiguration(Eigen::Map<const Eigen::VectorXd>(q.data(), rspace->getDimension()));
}

setStartConfigurationFromWorld()

void PlanBuilder::setStartConfigurationFromWorld

(

)

Set the start configuration from the current state of the world.

Definition at line 413 of file robowflex_dart/src/planning.cpp.

{
    start = Eigen::VectorXd::Zero(rspace->getDimension());
    rspace->getWorldState(world, start);
}

setStateValidityChecker()

void PlanBuilder::setStateValidityChecker ( )

protected

Set the state validity checker on the simple setup.

Definition at line 580 of file robowflex_dart/src/planning.cpp.

{
    if (ss)
    {
        // ss->setStateValidityChecker(std::make_shared<WorldValidityChecker>(info, 1));
        ss->setStateValidityChecker([&](const ompl::base::State *state) -> bool {
            const auto &as = toStateConst(state);
 
            world->lock();
            rspace->setWorldState(world, as);
            bool r = not world->inCollision();
            world->unlock();
            return r;
        });
    }
}

setup()

void PlanBuilder::setup

(

)

Calls setup routines on OMPL structures. Call after all setup is done and builder is initialized.

Definition at line 511 of file robowflex_dart/src/planning.cpp.

{
    ompl::base::ScopedState<> start_state(space);
    toState(start_state.get())->data = start;
    ss->setStartState(start_state);
    if (not goal_)
        throw std::runtime_error("No goal setup in PlanBuilder!");
    ss->setGoal(goal_);
 
    ss->setup();
}

Member Data Documentation

constraint

TSRConstraintPtr robowflex::darts::PlanBuilder::constraint {nullptr}

OMPL Constraint for Path Constraints.

Definition at line 415 of file robowflex_dart/include/robowflex_dart/planning.h.

struct { ... } robowflex::darts::PlanBuilder::constraints

Constrained planning options.

delta

double robowflex::darts::PlanBuilder::delta {0.2}

Manifold delta.

Definition at line 427 of file robowflex_dart/include/robowflex_dart/planning.h.

goal_

ompl::base::GoalPtr robowflex::darts::PlanBuilder::goal_ {nullptr}

protected

Desired goal.

Definition at line 433 of file robowflex_dart/include/robowflex_dart/planning.h.

info

ompl::base::SpaceInformationPtr robowflex::darts::PlanBuilder::info {nullptr}

Actual Space Information.

Definition at line 410 of file robowflex_dart/include/robowflex_dart/planning.h.

lambda

double robowflex::darts::PlanBuilder::lambda {5.0}

Manifold lambda.

Definition at line 428 of file robowflex_dart/include/robowflex_dart/planning.h.

struct { ... } robowflex::darts::PlanBuilder::options

Hyperparameter options.

path_constraints

std::vector<TSRPtr> robowflex::darts::PlanBuilder::path_constraints

Path Constraints.

Definition at line 414 of file robowflex_dart/include/robowflex_dart/planning.h.

rinfo

ompl::base::SpaceInformationPtr robowflex::darts::PlanBuilder::rinfo {nullptr}

Underlying Space Information.

Definition at line 409 of file robowflex_dart/include/robowflex_dart/planning.h.

rspace

StateSpacePtr robowflex::darts::PlanBuilder::rspace {nullptr}

Underlying Robot State Space.

Definition at line 407 of file robowflex_dart/include/robowflex_dart/planning.h.

space

ompl::base::StateSpacePtr robowflex::darts::PlanBuilder::space {nullptr}

Actual OMPL State Space (might be constrained).

Definition at line 408 of file robowflex_dart/include/robowflex_dart/planning.h.

ss

ompl::geometric::SimpleSetupPtr robowflex::darts::PlanBuilder::ss {nullptr}

Simple Setup.

Definition at line 411 of file robowflex_dart/include/robowflex_dart/planning.h.

start

Eigen::VectorXd robowflex::darts::PlanBuilder::start

Start configuration.

Definition at line 417 of file robowflex_dart/include/robowflex_dart/planning.h.

world

WorldPtr robowflex::darts::PlanBuilder::world

World used for planning.

Definition at line 412 of file robowflex_dart/include/robowflex_dart/planning.h.

---

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

• robowflex_dart/include/robowflex_dart/planning.h
• robowflex_dart/src/planning.cpp