robowflex::darts::TSRGoal Class Reference

A sampleable goal region for OMPL for a set of TSRs. Samples goals in a separate thread using a clone of the world. More...

#include <planning.h>

Inheritance diagram for robowflex::darts::TSRGoal:

Public Member Functions
bool	sample (const ompl::base::GoalLazySamples *gls, ompl::base::State *state)
	Sampling routine. Generates IK samples from the TSR goal. More...
double	distanceGoal (const ompl::base::State *state) const override
	Distance to the goal. More...
TSRSetPtr	getTSRSet ()
	Get the TSR set of this goal region. More...
Constructors.
	TSRGoal (const ompl::base::SpaceInformationPtr &si, const WorldPtr &world, const std::vector< TSRPtr > &tsrs)
	Constructor. More...
	TSRGoal (const ompl::base::SpaceInformationPtr &si, const WorldPtr &world, const TSRPtr tsr)
	Constructor. More...
	TSRGoal (const PlanBuilder &builder, TSRPtr tsr)
	Constructor. More...
	TSRGoal (const PlanBuilder &builder, const std::vector< TSRPtr > &tsrs)
	Constructor. More...
	~TSRGoal ()
	Destructor. Stops sampling thread. 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
struct {
bool   use_gradient {false}
	Use gradient-based TSR solver. More...
std::size_t   max_samples {10}
	Maximum samples. More...
}	options
	Public options. More...

Private Attributes
WorldPtr	world_
	World used. More...
TSRSetPtr	tsr_
	TSR set to sample from. More...
ompl::base::StateSamplerPtr	sampler_
	State sampler. More...

Detailed Description

A sampleable goal region for OMPL for a set of TSRs. Samples goals in a separate thread using a clone of the world.

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

Constructor & Destructor Documentation

TSRGoal() [1/4]

TSRGoal::TSRGoal	(	const ompl::base::SpaceInformationPtr &	si,
		const WorldPtr &	world,
		const std::vector< TSRPtr > &	tsrs
	)

Constructor.

Parameters

[in]	si	Space information.
[in]	world	World to use.
[in]	tsrs	TSRs to sample.

TSRGoal

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

  : ompl::base::GoalLazySamples(
        si, std::bind(&TSRGoal::sample, this, std::placeholders::_1, std::placeholders::_2), false, 1e-3)
  , ConstraintExtractor(si)
  , world_(world->clone())
  , tsr_(std::make_shared<TSRSet>(world_, tsrs))
  // Have to allocate our own sampler from scratch since the constrained sampler might use the underlying
  // world used by the planner (e.g., in project)
  , sampler_(std::make_shared<StateSpace::StateSampler>(getSpace()))
{
    tsr_->useWorldIndices(getSpace()->getIndices());
    tsr_->setWorldIndices(getSpace()->getIndices());
 
    tsr_->setWorldLowerLimits(getSpace()->getLowerBound());
    tsr_->setWorldUpperLimits(getSpace()->getUpperBound());
 
    tsr_->initialize();
}

TSRGoal() [2/4]

TSRGoal::TSRGoal	(	const ompl::base::SpaceInformationPtr &	si,
		const WorldPtr &	world,
		const TSRPtr	tsr
	)

Constructor.

Parameters

[in]	si	Space information.
[in]	world	World to use.
[in]	tsr	TSR to sample.

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

  : TSRGoal(si, world, std::vector<TSRPtr>{tsr})
{
}

TSRGoal() [3/4]

TSRGoal::TSRGoal	(	const PlanBuilder &	builder,
		TSRPtr	tsr
	)

Constructor.

Parameters

[in]	builder	Plan builder to use as base for goal.
[in]	tsr	TSR to sample.

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

  : TSRGoal(builder, std::vector<TSRPtr>{std::move(tsr)})
{
}

TSRGoal() [4/4]

TSRGoal::TSRGoal	(	const PlanBuilder &	builder,
		const std::vector< TSRPtr > &	tsrs
	)

Constructor.

Parameters

[in]	builder	Plan builder to use as base for goal.
[in]	tsrs	TSRs to sample.

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

  : TSRGoal(builder.info, builder.world, [&] {
      std::vector<TSRPtr> temp = builder.path_constraints;
      temp.insert(temp.end(), tsrs.begin(), tsrs.end());
      return temp;
  }())
{
}

~TSRGoal()

TSRGoal::~TSRGoal

(

)

Destructor. Stops sampling thread.

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

{
    stopSampling();
}

Member Function Documentation

distanceGoal()

double TSRGoal::distanceGoal ( const ompl::base::State *  state ) const

override

Distance to the goal.

Parameters

[in]

state

State to measure.

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

{
    auto &&as = toStateConst(state);
    auto &&x = Eigen::Map<const Eigen::VectorXd>(as->values, si_->getStateDimension());
    return tsr_->distanceWorldState(x);
}

getTSRSet()

TSRSetPtr TSRGoal::getTSRSet

(

)

Get the TSR set of this goal region.

Returns

The TSR set.

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

{
    return tsr_;
}

sample()

bool TSRGoal::sample	(	const ompl::base::GoalLazySamples *	gls,
		ompl::base::State *	state
	)

Sampling routine. Generates IK samples from the TSR goal.

Parameters

[in]	gls	This class.
[in]	state	State to sample.

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

{
    if (getStateCount() >= options.max_samples)
        return false;
 
    bool success = false;
    while (not terminateSamplingThread_ and not success)
    {
        auto &&as = toState(state);
        sampler_->sampleUniform(as);
 
        auto &&x = Eigen::Map<Eigen::VectorXd>(as->values, si_->getStateDimension());
 
        if (tsr_->numTSRs() == 1 or not options.use_gradient)
            success = tsr_->solveWorldState(x);
 
        else if (options.use_gradient)
            success = tsr_->solveGradientWorldState(x);
 
        success &= si_->satisfiesBounds(state);
        si_->enforceBounds(state);
    }
 
    return true;
}

Member Data Documentation

max_samples

std::size_t robowflex::darts::TSRGoal::max_samples {10}

Maximum samples.

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

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

Public options.

sampler_

ompl::base::StateSamplerPtr robowflex::darts::TSRGoal::sampler_

private

State sampler.

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

tsr_

TSRSetPtr robowflex::darts::TSRGoal::tsr_

private

TSR set to sample from.

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

use_gradient

bool robowflex::darts::TSRGoal::use_gradient {false}

Use gradient-based TSR solver.

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

world_

WorldPtr robowflex::darts::TSRGoal::world_

private

World used.

Definition at line 184 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