robowflex::darts::StateSpace Class Reference

An OMPL state space for robowflex::darts::World. Can do motion planning for any of the robots or structures in the world. More...

#include <space.h>

Inheritance diagram for robowflex::darts::StateSpace:

Classes
class	StateSampler
	Sampler for robowflex::darts::StateSpace. More...
class	StateType
	State type for the robowflex::darts::StateSpace. More...

Public Member Functions
void	setMetricSpace (bool metric)
Constructor and Setup
	StateSpace (WorldPtr world)
	Constructor. More...
void	addGroup (const std::string &name, const std::string &group, std::size_t cyclic=0)
	Add a group to be planned for. More...
void	addGroupFromJoints (const std::string &group_name, const std::vector< dart::dynamics::Joint * > &joints, std::size_t cyclic=0)
	Add a group to be planned for. More...
World State
void	setWorldState (WorldPtr world, const ompl::base::State *state) const
	Set the state of a world from an OMPL state. More...
void	setWorldState (WorldPtr world, const Eigen::Ref< const Eigen::VectorXd > &x) const
	Set the state of a world from a configuration. More...
void	getWorldState (WorldPtr world, ompl::base::State *state) const
	Get the state of a world in an OMPL state. More...
void	getWorldState (WorldPtr world, Eigen::Ref< Eigen::VectorXd > x) const
	Get the state of a world in a configuration vector. More...
void	setWorldGroupState (WorldPtr world, const std::string &group_name, const Eigen::Ref< const Eigen::VectorXd > &x) const
	Set the state of a world from a configuration of a group. More...
void	getWorldGroupState (WorldPtr world, const std::string &group_name, Eigen::Ref< Eigen::VectorXd > x) const
	Get the group state of a world. More...
Getters and Setters
WorldPtr	getWorld ()
	Get underlying world. More...
const WorldPtr &	getWorldConst () const
	Get underlying world. More...
std::vector< std::pair< std::size_t, std::size_t > >	getIndices () const
	Get the set of indices that is being planned for. Pairs of skeleton index and joint index. More...
Eigen::VectorXd	getLowerBound () const
	Get a vector of the lower joint bounds for the space. More...
Eigen::VectorXd	getUpperBound () const
	Get a vector of the upper joint bounds for the space. More...
JointPtr	getJoint (std::size_t index) const
	Get a joint that is being planned for. More...
JointPtr	getJoint (const std::string &name) const
	Get a joint that is being planned for. More...
const std::vector< JointPtr > &	getJoints () const
	Get a vector of the joints being planned for. More...
void	getGroupState (const std::string &group, const ompl::base::State *state, Eigen::Ref< Eigen::VectorXd > v) const
	From a full state, get only the state of a group. More...
void	setGroupState (const std::string &group, ompl::base::State *state, const Eigen::Ref< const Eigen::VectorXd > &v) const
	Set a (sub)group state in a full state. More...
std::size_t	getGroupDimension (const std::string &group) const
	Get the dimension of joint variables for a subgroup. More...
std::vector< std::string >	getGroups () const
	Get the names of all groups managed by this space. More...
std::vector< std::string >	getGroupDofNames (const std::string &group_name) const
	Get the names of each of the dof controlled by a group. More...
OMPL StateSpace Methods
bool	isMetricSpace () const override
void	enforceBounds (ompl::base::State *state) const override
bool	satisfiesBounds (const ompl::base::State *state) const override
double	distance (const ompl::base::State *state1, const ompl::base::State *state2) const override
double	getMaximumExtent () const override
bool	equalStates (const ompl::base::State *state1, const ompl::base::State *state2) const override
void	interpolate (const ompl::base::State *from, const ompl::base::State *to, double t, ompl::base::State *state) const override
ompl::base::StateSamplerPtr	allocDefaultStateSampler () const override
ompl::base::State *	allocState () const override
void	freeState (ompl::base::State *state) const override

Public Attributes
friend	Joint
friend	RnJoint
friend	StateSampler

Protected Member Functions
void	addJoint (const std::string &group_name, const JointPtr &joint)
void	addJointToGroup (const std::string &group_name, const JointPtr &joint)

Protected Attributes
WorldPtr	world_
	World to use for planning. More...
std::set< dart::dynamics::Joint * >	jointset_
	Set of joints used in planning. More...
std::vector< std::size_t >	indices_
	Vector of indices for planning. More...
bool	metric_ {true}
	Is this space all Rn? More...
std::vector< JointPtr >	joints_
	Vector of all joints used in planning. More...
ompl::RNG	rng_
	Random number generator. More...
std::map< std::string, std::vector< JointPtr > >	group_joints_
	Joints belonging to a group. More...
std::map< std::string, std::size_t >	group_dimension_
	Dimension of the group. More...

Detailed Description

An OMPL state space for robowflex::darts::World. Can do motion planning for any of the robots or structures in the world.

Definition at line 30 of file space.h.

Constructor & Destructor Documentation

StateSpace()

StateSpace::StateSpace

(

WorldPtr

world

)

Constructor.

Parameters

[in]

world

World to plan for.

StateSpace::StateType

Definition at line 56 of file space.cpp.

                                     : ompl::base::RealVectorStateSpace(), world_(std::move(world))
{
}

Member Function Documentation

addGroup()

void StateSpace::addGroup	(	const std::string &	name,
		const std::string &	group,
		std::size_t	cyclic = 0
	)

Add a group to be planned for.

Parameters

[in]	name	Name of the robot that has the group.
[in]	group	Group to plan for.
[in]	cyclic	If >0, will flatten rotational groups (i.e., SO(2), SO(3)) into Rn spaces, where n is cyclic.

Definition at line 65 of file space.cpp.

{
    auto robot = world_->getRobot(name);
    if (not robot)
    {
        RBX_ERROR("Robot %1% does not exist in world.", name);
        throw std::runtime_error("Invalid robot");
    }
 
    auto joints = robot->getGroupJoints(group);
    addGroupFromJoints(group, joints, cyclic);
}

addGroupFromJoints()

void StateSpace::addGroupFromJoints	(	const std::string &	group_name,
		const std::vector< dart::dynamics::Joint * > &	joints,
		std::size_t	cyclic = 0
	)

Add a group to be planned for.

Parameters

[in]	group_name	Name of the new group.
[in]	joints	Joints to add to this group.
[in]	cyclic	If >0, will flatten rotational groups (i.e., SO(2), SO(3)) into Rn spaces, where n is cyclic.

Definition at line 78 of file space.cpp.

{
    group_joints_.emplace(group_name, std::vector<JointPtr>{});
    group_dimension_.emplace(group_name, 0);
 
    for (auto *joint : joints)
    {
        if (jointset_.find(joint) != jointset_.end())
        {
            addJointToGroup(group_name, getJoint(joint->getName()));
            continue;
        }
 
        jointset_.emplace(joint);
 
        const auto &type = joint->getType();
        if (type == "RevoluteJoint")
        {
            auto *revolute = static_cast<dart::dynamics::RevoluteJoint *>(joint);
 
            if (revolute->isCyclic(0))
            {
                if (cyclic)
                {
                    double low = -constants::pi * cyclic;
                    double high = constants::pi * cyclic;
                    addJoint(group_name, std::make_shared<RnJoint>(this, revolute, low, high));
                }
                else
                    addJoint(group_name, std::make_shared<SO2Joint>(this, revolute));
            }
            else
            {
                auto *dof = joint->getDof(0);
                auto limits = dof->getPositionLimits();
                addJoint(group_name, std::make_shared<RnJoint>(this, revolute, limits.first, limits.second));
            }
        }
        else if (type == "PrismaticJoint")
        {
            auto *prismatic = static_cast<dart::dynamics::PrismaticJoint *>(joint);
            auto *dof = joint->getDof(0);
            auto limits = dof->getPositionLimits();
            addJoint(group_name, std::make_shared<RnJoint>(this, prismatic, limits.first, limits.second));
        }
        else if (type == "PlanarJoint")
        {
            // Assume XY plane for now
            auto *planar = static_cast<dart::dynamics::PlanarJoint *>(joint);
            if (cyclic)
            {
                Eigen::Vector3d low;
                Eigen::Vector3d high;
 
                // XY limits
                for (std::size_t i = 0; i < 2; ++i)
                {
                    low[i] = world_->getWorkspaceLowConst()[i];
                    high[i] = world_->getWorkspaceHighConst()[i];
                }
 
                low[2] = -constants::pi * cyclic;
                high[2] = constants::pi * cyclic;
 
                addJoint(group_name, std::make_shared<RnJoint>(this, planar, 3, 0, low, high));
            }
            else
            {
                Eigen::Vector2d low;
                Eigen::Vector2d high;
 
                for (std::size_t i = 0; i < 2; ++i)
                {
                    low[i] = world_->getWorkspaceLowConst()[i];
                    high[i] = world_->getWorkspaceHighConst()[i];
                }
 
                addJoint(group_name, std::make_shared<RnJoint>(this, planar, 2, 0, low, high));
                addJoint(group_name, std::make_shared<SO2Joint>(this, planar, 2));
            }
        }
        else if (type == "FreeJoint")
        {
            auto *free = static_cast<dart::dynamics::FreeJoint *>(joint);
 
            if (cyclic)
            {
                double alow = -constants::pi * cyclic;
                double ahigh = constants::pi * cyclic;
 
                Eigen::Vector6d low;
                Eigen::Vector6d high;
 
                for (std::size_t i = 0; i < 3; ++i)
                {
                    low[i] = alow;
                    high[i] = ahigh;
                    low[3 + i] =
                        std::max({free->getPositionLowerLimit(3 + i), world_->getWorkspaceLowConst()[i]});
                    high[3 + i] =
                        std::min({free->getPositionUpperLimit(3 + i), world_->getWorkspaceHighConst()[i]});
                }
                auto j = std::make_shared<RnJoint>(this, free, 6, 0, low, high);
                addJoint(group_name, j);
            }
            else
            {
                Eigen::Vector3d low;
                Eigen::Vector3d high;
 
                for (std::size_t i = 0; i < 3; ++i)
                {
                    low[i] =
                        std::max({free->getPositionLowerLimit(3 + i), world_->getWorkspaceLowConst()[i]});
                    high[i] =
                        std::min({free->getPositionUpperLimit(3 + i), world_->getWorkspaceHighConst()[i]});
                }
 
                addJoint(group_name, std::make_shared<RnJoint>(this, free, 3, 3, low, high));
                addJoint(group_name, std::make_shared<SO3Joint>(this, free));
            }
        }
        else
            RBX_WARN("Unknown joint type %1%, skipping.", type);
    }
 
    registerDefaultProjection(
        std::make_shared<ompl::base::RealVectorRandomLinearProjectionEvaluator>(this, 2));
}

addJoint()

void StateSpace::addJoint ( const std::string &  group_name, const JointPtr &  joint  )

protected

Definition at line 209 of file space.cpp.

{
    joints_.emplace_back(joint);
    addJointToGroup(group_name, joint);
 
    if (not std::dynamic_pointer_cast<RnJoint>(joint))
        metric_ = false;
}

addJointToGroup()

void StateSpace::addJointToGroup ( const std::string &  group_name, const JointPtr &  joint  )

protected

Definition at line 218 of file space.cpp.

{
    group_joints_[group_name].emplace_back(joint);
    group_dimension_[group_name] += joint->getDimension();
}

allocDefaultStateSampler()

ompl::base::StateSamplerPtr StateSpace::allocDefaultStateSampler ( ) const

override

Definition at line 357 of file space.cpp.

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

allocState()

ompl::base::State * StateSpace::allocState ( ) const

override

Definition at line 362 of file space.cpp.

{
    return new StateType(dimension_);
}

distance()

double StateSpace::distance ( const ompl::base::State *  state1, const ompl::base::State *  state2  ) const

override

Definition at line 309 of file space.cpp.

{
    const auto &as1 = state1->as<StateType>();
    const auto &as2 = state2->as<StateType>();
 
    double d = 0;
    for (const auto &joint : joints_)
    {
        const auto &v1 = joint->getSpaceVarsConst(as1->data);
        const auto &v2 = joint->getSpaceVarsConst(as2->data);
 
        d += joint->distance(v1, v2);
    }
 
    return d;
}

enforceBounds()

void StateSpace::enforceBounds ( ompl::base::State *  state ) const

override

Definition at line 286 of file space.cpp.

{
    const auto &as = state->as<StateType>();
    for (const auto &joint : joints_)
    {
        auto v = joint->getSpaceVars(as->data);
        joint->enforceBounds(v);
    }
}

equalStates()

bool StateSpace::equalStates ( const ompl::base::State *  state1, const ompl::base::State *  state2  ) const

override

Definition at line 335 of file space.cpp.

{
    return distance(state1, state2) <= 1e-8;
}

freeState()

void StateSpace::freeState ( ompl::base::State *  state ) const

override

Definition at line 367 of file space.cpp.

{
    auto *as = state->as<StateType>();
    delete as;
}

getGroupDimension()

std::size_t StateSpace::getGroupDimension

(

const std::string &

group

)

const

Get the dimension of joint variables for a subgroup.

Returns

The group's dimension.

Definition at line 446 of file space.cpp.

{
    return group_dimension_.find(group)->second;
}

getGroupDofNames()

std::vector< std::string > StateSpace::getGroupDofNames

(

const std::string &

group_name

)

const

Get the names of each of the dof controlled by a group.

Returns

The names of all the controlled dof for a group.

Definition at line 460 of file space.cpp.

{
    std::vector<std::string> dofs;
 
    const auto &joints = group_joints_.find(group_name)->second;
    for (const auto &joint : joints)
    {
        const auto &jdofs = joint->getDofs();
        dofs.insert(dofs.end(), jdofs.begin(), jdofs.end());
    }
 
    return dofs;
}

getGroups()

std::vector< std::string > StateSpace::getGroups

(

)

const

Get the names of all groups managed by this space.

Returns

Names of all the groups.

Definition at line 451 of file space.cpp.

{
    std::vector<std::string> groups;
    for (const auto &pair : group_joints_)
        groups.emplace_back(pair.first);
 
    return groups;
}

getGroupState()

void StateSpace::getGroupState	(	const std::string &	group,
		const ompl::base::State *	state,
		Eigen::Ref< Eigen::VectorXd >	v
	)		const

From a full state, get only the state of a group.

Parameters

[in]	group	The group to get the state for.
[in]	state	The state to get the group state from.
[out]	v	The group state to set.

Definition at line 416 of file space.cpp.

{
    const auto &joints = group_joints_.find(group)->second;
    const auto &as = state->as<StateType>();
 
    std::size_t index = 0;
    for (const auto &joint : joints)
    {
        std::size_t n = joint->getDimension();
        v.segment(index, n) = joint->getSpaceVarsConst(as->data);
        index += n;
    }
}

getIndices()

std::vector< std::pair< std::size_t, std::size_t > > StateSpace::getIndices

(

)

const

Get the set of indices that is being planned for. Pairs of skeleton index and joint index.

Returns

Vector of indices.

Definition at line 383 of file space.cpp.

{
    std::vector<std::pair<std::size_t, std::size_t>> indices;
    for (const auto &joint : joints_)
    {
        auto skelx = joint->getSkeletonIndex();
        auto add = joint->getIndices();
        for (const auto &index : add)
            indices.emplace_back(skelx, index);
    }
 
    return indices;
}

getJoint() [1/2]

JointPtr StateSpace::getJoint

(

const std::string &

name

)

const

Get a joint that is being planned for.

Returns

The joint.

Definition at line 402 of file space.cpp.

{
    auto it = std::find_if(joints_.begin(), joints_.end(),
                           [&](const JointPtr &j) { return j->getJoint(world_)->getName() == name; });
    if (it != joints_.end())
        return *it;
    return nullptr;
}

getJoint() [2/2]

JointPtr StateSpace::getJoint

(

std::size_t

index

)

const

Get a joint that is being planned for.

Returns

The joint.

Definition at line 397 of file space.cpp.

{
    return joints_[index];
}

getJoints()

const std::vector< JointPtr > & StateSpace::getJoints

(

)

const

Get a vector of the joints being planned for.

Returns

The vector of joints being planned for.

Definition at line 411 of file space.cpp.

{
    return joints_;
}

getLowerBound()

Eigen::VectorXd StateSpace::getLowerBound

(

)

const

Get a vector of the lower joint bounds for the space.

Returns

The lower bounds of the space.

Definition at line 474 of file space.cpp.

{
    const auto &bounds = getBounds();
    return Eigen::Map<const Eigen::VectorXd>(bounds.low.data(), bounds.low.size());
}

getMaximumExtent()

double StateSpace::getMaximumExtent ( ) const

override

Definition at line 326 of file space.cpp.

{
    double d = 0;
    for (const auto &joint : joints_)
        d += joint->getMaximumExtent();
 
    return d;
}

getUpperBound()

Eigen::VectorXd StateSpace::getUpperBound

(

)

const

Get a vector of the upper joint bounds for the space.

Returns

The upper bounds of the space.

Definition at line 480 of file space.cpp.

{
    const auto &bounds = getBounds();
    return Eigen::Map<const Eigen::VectorXd>(bounds.high.data(), bounds.high.size());
}

getWorld()

WorldPtr StateSpace::getWorld

(

)

Get underlying world.

Returns

The world for the space.

Definition at line 373 of file space.cpp.

{
    return world_;
}

getWorldConst()

const WorldPtr & StateSpace::getWorldConst

(

)

const

Get underlying world.

Returns

The world for the space.

Definition at line 378 of file space.cpp.

{
    return world_;
}

getWorldGroupState()

void StateSpace::getWorldGroupState	(	WorldPtr	world,
		const std::string &	group_name,
		Eigen::Ref< Eigen::VectorXd >	x
	)		const

Get the group state of a world.

Parameters

[in]	world	World to get state of.
[in]	group_name	Name of group to get.
[out]	x	Configuration to get.

Definition at line 272 of file space.cpp.

{
    const auto &joints = group_joints_.find(group_name)->second;
 
    std::size_t index = 0;
    for (const auto &joint : joints)
    {
        std::size_t n = joint->getDimension();
        joint->getJointState(world, x.segment(index, n));
        index += n;
    }
}

getWorldState() [1/2]

void StateSpace::getWorldState	(	WorldPtr	world,
		Eigen::Ref< Eigen::VectorXd >	x
	)		const

Get the state of a world in a configuration vector.

Parameters

[in]	world	World to get state of.
[out]	x	Configuration to fill.

Definition at line 247 of file space.cpp.

{
    for (const auto &joint : joints_)
    {
        const auto &v = joint->getSpaceVars(x);
        joint->getJointState(world, v);
    }
}

getWorldState() [2/2]

void StateSpace::getWorldState	(	WorldPtr	world,
		ompl::base::State *	state
	)		const

Get the state of a world in an OMPL state.

Parameters

[in]	world	World to get state of.
[out]	state	State to fill.

Definition at line 241 of file space.cpp.

{
    auto *as = state->as<StateType>();
    getWorldState(std::move(world), as->data);
}

interpolate()

void StateSpace::interpolate ( const ompl::base::State *  from, const ompl::base::State *  to, double  t, ompl::base::State *  state  ) const

override

Definition at line 340 of file space.cpp.

{
    const auto &af = from->as<StateType>();
    const auto &at = to->as<StateType>();
    auto *as = state->as<StateType>();
 
    for (const auto &joint : joints_)
    {
        const auto &vf = joint->getSpaceVarsConst(af->data);
        const auto &vt = joint->getSpaceVarsConst(at->data);
        auto vs = joint->getSpaceVars(as->data);
 
        joint->interpolate(vf, vt, t, vs);
    }
}

isMetricSpace()

bool StateSpace::isMetricSpace ( ) const

override

Definition at line 60 of file space.cpp.

{
    return metric_;
}

satisfiesBounds()

bool StateSpace::satisfiesBounds ( const ompl::base::State *  state ) const

override

Definition at line 296 of file space.cpp.

{
    const auto &as = state->as<StateType>();
    for (const auto &joint : joints_)
    {
        const auto &v = joint->getSpaceVarsConst(as->data);
        if (not joint->satisfiesBounds(v))
            return false;
    }
 
    return true;
}

setGroupState()

void StateSpace::setGroupState	(	const std::string &	group,
		ompl::base::State *	state,
		const Eigen::Ref< const Eigen::VectorXd > &	v
	)		const

Set a (sub)group state in a full state.

Parameters

[in]	group	The group to set the state for.
[out]	state	The state to set the group state in.
[in]	v	The group state to use.

Definition at line 431 of file space.cpp.

{
    const auto &joints = group_joints_.find(group)->second;
    auto *as = state->as<StateType>();
 
    std::size_t index = 0;
    for (const auto &joint : joints)
    {
        std::size_t n = joint->getDimension();
        joint->getSpaceVars(as->data) = v.segment(index, n);
        index += n;
    }
}

setMetricSpace()

void StateSpace::setMetricSpace

(

bool

metric

)

Definition at line 486 of file space.cpp.

{
    metric_ = metric;
}

setWorldGroupState()

void StateSpace::setWorldGroupState	(	WorldPtr	world,
		const std::string &	group_name,
		const Eigen::Ref< const Eigen::VectorXd > &	x
	)		const

Set the state of a world from a configuration of a group.

Parameters

[out]	world	World to set state of.
[in]	group_name	Name of group to set.
[in]	x	Configuration to set.

Definition at line 256 of file space.cpp.

{
    const auto &joints = group_joints_.find(group_name)->second;
 
    std::size_t index = 0;
    for (const auto &joint : joints)
    {
        std::size_t n = joint->getDimension();
        joint->setJointState(world, x.segment(index, n));
        index += n;
    }
 
    world->forceUpdate();
}

setWorldState() [1/2]

void StateSpace::setWorldState	(	WorldPtr	world,
		const Eigen::Ref< const Eigen::VectorXd > &	x
	)		const

Set the state of a world from a configuration.

Parameters

[out]	world	World to set state of.
[in]	x	Configuration to set world.

Definition at line 230 of file space.cpp.

{
    for (const auto &joint : joints_)
    {
        const auto &v = joint->getSpaceVarsConst(x);
        joint->setJointState(world, v);
    }
 
    world->forceUpdate();
}

setWorldState() [2/2]

void StateSpace::setWorldState	(	WorldPtr	world,
		const ompl::base::State *	state
	)		const

Set the state of a world from an OMPL state.

Parameters

[out]	world	World to set state of.
[in]	state	State to set world.

Definition at line 224 of file space.cpp.

{
    const auto &as = state->as<StateType>();
    setWorldState(std::move(world), as->data);
}

Member Data Documentation

group_dimension_

std::map<std::string, std::size_t> robowflex::darts::StateSpace::group_dimension_

protected

Dimension of the group.

Definition at line 269 of file space.h.

group_joints_

std::map<std::string, std::vector<JointPtr> > robowflex::darts::StateSpace::group_joints_

protected

Joints belonging to a group.

Definition at line 268 of file space.h.

indices_

std::vector<std::size_t> robowflex::darts::StateSpace::indices_

protected

Vector of indices for planning.

Definition at line 260 of file space.h.

Joint

friend robowflex::darts::StateSpace::Joint

Definition at line 80 of file space.h.

joints_

std::vector<JointPtr> robowflex::darts::StateSpace::joints_

protected

Vector of all joints used in planning.

Definition at line 264 of file space.h.

jointset_

std::set<dart::dynamics::Joint *> robowflex::darts::StateSpace::jointset_

protected

Set of joints used in planning.

Definition at line 259 of file space.h.

metric_

bool robowflex::darts::StateSpace::metric_ {true}

protected

Is this space all Rn?

Definition at line 262 of file space.h.

rng_

ompl::RNG robowflex::darts::StateSpace::rng_

protected

Random number generator.

Definition at line 266 of file space.h.

RnJoint

friend robowflex::darts::StateSpace::RnJoint

Definition at line 81 of file space.h.

StateSampler

friend robowflex::darts::StateSpace::StateSampler

Definition at line 82 of file space.h.

world_

WorldPtr robowflex::darts::StateSpace::world_

protected

World to use for planning.

Definition at line 257 of file space.h.

---

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

• space.h
• space.cpp