robowflex::darts::JointRegionGoal Class Reference

A joint space goal volume. More...

#include <planning.h>

Inheritance diagram for robowflex::darts::JointRegionGoal:

Public Member Functions
	JointRegionGoal (const PlanBuilder &builder, const Eigen::Ref< const Eigen::VectorXd > &state, double tolerance=0.)
	JointRegionGoal (const PlanBuilder &builder, const Eigen::Ref< const Eigen::VectorXd > &lower, const Eigen::Ref< const Eigen::VectorXd > &upper)
	~JointRegionGoal ()
void	sampleGoal (ompl::base::State *state) const override
unsigned int	maxSampleCount () const override
double	distanceGoal (const ompl::base::State *state) const override
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...

Private Attributes
ompl::RNG	rng_
ompl::base::State *	lower_
ompl::base::State *	upper_

Detailed Description

A joint space goal volume.

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

Constructor & Destructor Documentation

JointRegionGoal() [1/2]

JointRegionGoal::JointRegionGoal	(	const PlanBuilder &	builder,
		const Eigen::Ref< const Eigen::VectorXd > &	state,
		double	tolerance = 0.
	)

JointRegionGoal

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

  : JointRegionGoal(builder,                                                     //
                    state - Eigen::VectorXd::Constant(state.size(), tolerance),  //
                    state + Eigen::VectorXd::Constant(state.size(), tolerance))
{
}

JointRegionGoal() [2/2]

JointRegionGoal::JointRegionGoal	(	const PlanBuilder &	builder,
		const Eigen::Ref< const Eigen::VectorXd > &	lower,
		const Eigen::Ref< const Eigen::VectorXd > &	upper
	)

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

  : ompl::base::GoalSampleableRegion(builder.info)
  , ConstraintExtractor(builder.info)
  , lower_(si_->allocState())
  , upper_(si_->allocState())
{
    if (lower.size() != upper.size())
        throw std::runtime_error("Bound size mismatch!");
 
    if (builder.space->getDimension() != lower.size())
        throw std::runtime_error("Bound size mismatch!");
 
    toState(lower_)->data = lower;
    toState(upper_)->data = upper;
}

~JointRegionGoal()

JointRegionGoal::~JointRegionGoal

(

)

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

{
    si_->freeState(lower_);
    si_->freeState(upper_);
}

Member Function Documentation

distanceGoal()

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

override

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

{
    const auto &s = toStateConst(state)->data;
    const auto &l = toStateConst(lower_)->data;
    const auto &u = toStateConst(upper_)->data;
 
    const auto &ss = std::dynamic_pointer_cast<StateSpace>(si_->getStateSpace());
 
    double d = 0.;
    for (int i = 0; i < l.size(); ++i)
    {
        const auto &joint = ss->getJoint(i);
        const auto &sv = joint->getSpaceVarsConst(s);
        const auto &lv = joint->getSpaceVarsConst(l);
        const auto &uv = joint->getSpaceVarsConst(u);
 
        if (sv[0] < lv[0])
            d += joint->distance(sv, lv);
        if (sv[0] > uv[0])
            d += joint->distance(sv, uv);
    }
 
    return d;
}

maxSampleCount()

unsigned int JointRegionGoal::maxSampleCount ( ) const

override

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

{
    return std::numeric_limits<unsigned int>::max();
}

sampleGoal()

void JointRegionGoal::sampleGoal ( ompl::base::State *  state ) const

override

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

{
    auto &s = toState(state)->data;
    const auto &l = toStateConst(lower_)->data;
    const auto &u = toStateConst(upper_)->data;
 
    for (int i = 0; i < l.size(); ++i)
        s[i] = rng_.uniformReal(l[i], u[i]);
}

Member Data Documentation

lower_

ompl::base::State* robowflex::darts::JointRegionGoal::lower_

private

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

rng_

ompl::RNG robowflex::darts::JointRegionGoal::rng_

mutableprivate

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

upper_

ompl::base::State* robowflex::darts::JointRegionGoal::upper_

private

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