Todo List

Module Active Robots

This is an artifact of developing robot groups. It should be replaced by using a proper subgroup where each robot within is active.

Global Actuator::ComputeOutput (const Control::Signal &_s) const

Generalize this so that the robot's starting point can be taken into account. Currently we assume that the generated force is independent of starting state.

Global AddHoles (tetgenio *const _freeModel, const NefPolyhedron &_freespace, const Environment *const _env, const bool _debug)

There seems to be an intermittent problem with adding holes for obstacles that touch the environment boundary. Sometimes it causes bad decompositions where the free space gets eaten away, and sometimes not. Not adding the hole sometimes causes tetgen to not remove the tetras within the obstacle. We need to characterize the cases where it fails and where it works.

Global BasicExtender::Expand (const Cfg &_start, const Cfg &_end, Cfg &_newCfg, double _delta, LPOutput &_lp, CDInfo &_cdInfo, double _posRes, double _oriRes)

No, we don't have to set _newCfg to _end, and it would be better if we didn't use this assumption. We are doing this so that we can use extenders as local planners in multi-tree RRT methods. To fix and remove the assumption, we need to homogenize our extenders and local planners into a single class with methods for both uses (i.e. 'Extend' and 'LocalPlan').

Global BasicPRM::Initialize () override

Move input map parsing to the MPSolution somehow?

Global BasicRRTStrategy::SelectDispersedTarget (const VID _v)

This functionality can probably be moved into a dispersed extender, which we could call several times here.

Class CCTracker< RoadmapType >

Add support for non-tree directed graphs. We need to use predecessor information to do this, which we now have in the roadmap graph.

The stat class tracking adds a lot of complication. Remove it once we're sure the performance is fast enough to forgo tracking.

Class CDInfo

Generalize this object to store collisions with obstacles, boundaries, and other robots in a uniform way. It should contain a vector of 'Collision' structures which describe the object type, indexes, and distance for each detected collision.

Global Cfg::operator< (const Cfg &_cfg) const

The function currently seems to check each dof in order, returning T/F immediately unless exactly equal; is this intended?

Global Chain::Decompose (const MultiBody *const _mb)

Expand to support joints with more than one forward or backward connection. For now we only support chain robots.

Class CollisionDetectionValidity

Remove the 'GetCDMethod' function after re-implementing ObstacleClearanceValidity as a subtype of CollisionDetectionValidity.

Global CollisionDetectionValidity::IsInBoundaryCollision (CDInfo &_cdInfo, const Cfg &_cfg)

This also doesn't work, we can't distinguish between no collision, self collision, and boundary collision since we use -1 for all three.

Global CollisionDetectionValidity::IsInInterRobotCollision (CDInfo &_cdInfo, Robot *const _robot, const std::vector< Robot * > &_robots, const std::string &_caller)

Arg, more ambiguous data. See above.

Global CollisionDetectionValidity::IsInSelfCollision (CDInfo &_cdInfo, const MultiBody *const _multibody, const std::string &_caller)

-1 is the default value here, so this doesn't really work. We need to expand our CDInfo object to handle this properly.

Global CollisionDetectionValidity::IsInsideObstacle (const Point3d &_p, std::vector< size_t > *_obstIdxs)

Implement a bounding box check (per multibody and body) before calling m_cdMethod.

Global CollisionDetectionValidity::IsInsideObstacle (const Point3d &_p) override

Implement a bounding box check (per multibody and body) before calling m_cdMethod.

Global CompositeState< GraphType >::GetRobot (const size_t _index) const

Remove this after we are very sure things are working.

Global ComputeReachableDistanceOfSingleLink (const size_t _dimension, const Connection *_joint, const Connection *_parent, const Chain *_chain)

We can probably support some non-actuated joints here, but need to verify with the theory.

Global CSpaceConstraint::CSpaceConstraint (Robot *const _r, XMLNode &_node, Environment *_env=nullptr)

Verify that this works with constraints of lower dimension than the robot's cspace (for partial constraint), or decide that we will not support this and throw an error if requested.

Global DistanceMetricMethod::EdgeWeight (const RoadmapType *const _r, const typename RoadmapType::CEI &_edge) noexcept

We need to make this work for group roadmaps. Probably it needs to take the roadmap as a templated parameter and not be virtual (there is really only one way to do this).

Global DistanceMetricMethod::ScaleCfg (double _length, Cfg &_c, const Cfg &_o)

This is a very expensive way to scale a configuration. We should probably remove it and require derived classes to implement a more efficient function (this is the best we can do for a base class that does not know anything about the properties of the metric space).

Global Environment::ComputeResolution (const std::vector< std::unique_ptr< Robot >> &_robots)

Add an automatic computation of the orientation resolution here. This should be done so that rotating the robot base by one orientation resolution makes a point on the bounding sphere move by one position resolution. If possible we should also separate the resolutions for joint angles and base orientations since this gets really, really small with manipulators having several links (probably we'll need to manage this in the local planner to make maximum use of the available resolution).

Global Environment::Read (std::string _filename)

Add support for dynamic obstacles

Global Environment::ReadXML (XMLNode &_node)

Add support for dynamic obstacles

Class ExtenderMethod

Local planners and Extenders represent the same concepts and should be merged into a single class with both an Extend and LocalPlan function. This will help simplify several other objects within PMPL as well, such as bi-directional RRT.

Global GMSPolyhedron::COMAdjust

This has been a major source of bugs for a long time and should be removed entirely. All polyhedrons should be bounding-box centered without exception.

Class GridOverlay

Generalize to support 2d grids as well as 3d.

Global GridOverlay::LocateCells (const GMSPolyhedron &_polyhedron, const mathtool::Transformation &_transformation={}, const CellSet _type=CellSet::Closure) const

Can make the !_interior version more efficient by computing bbx cells for each facet.

Can make both versions more efficient by avoiding re-building the _polyhedron's BBX each time. Add min/max pts and transform them with the poly, use here.

Global GroupCfg< GraphType >::ApplyTransformationForRobots (const Formation &_robotList, const mathtool::Transformation &_transform, const mathtool::Transformation &_relativeTransform=mathtool::Transformation())

Generalize this to handle robots with more than one body.

Global GroupCfg< GraphType >::RotateFormationAboutLeader (const Formation &_robotList, const mathtool::Orientation &_rotation, const bool _debug=false)

We can probably compute this without having to configure the models (which cost a lot of transformations).

Class GroupLPOutput

Destroy this object and have LPs/Extenders work directly with a GroupLocalPlan.

Global GroupLPOutput::SetIndividualEdges (const std::vector< size_t > &_formation)

We need to preserve the intermediates.

This is not a correct edge for each individual robot - they will not all have the same weight. This needs to be tracked separately.

Global GroupPath::Length () const

This will be an error if we allow self-edges.

Global GroupPath::TimeSteps () const

This will be an error if we allow self-edges.

Class GroupPRM

Create option for decoupled planning.

Global GroupPRM::Connect (const std::vector< VID > &_vids)

Setup CC tracker for groups to fix this.

Class GroupQuery

There is a lot of copy-pasted code from QueryMethod because that method calls several other objects which are not ready for groups yet. We should be able to merge these two after a bit more flush-out of the group support. I have marked it final to remind us that we should not extend this as it will be merged.

Class GroupStrategyMethod

Incorporate path constraints when generating the start and goal.

Global GroupStrategyMethod::GenerateGoals (const std::string &_samplerLabel) override

For now we will only support one task per robot. Generalize this to support more task arrangements.

Global GroupStrategyMethod::GenerateStart (const std::string &_samplerLabel) override

For now we will only support one task per robot. Generalize this to support more task arrangements.

Global GroupStrategyMethod::GetGoalBoundaryMaps () const noexcept

For now we will only support one task per robot. Generalize this to support more task arrangements.

Global GroupStrategyMethod::GetStartBoundaryMap () const noexcept

For now we will only support one task per robot. Generalize this to support more task arrangements.

Class GroupTask

Move disassembly-specific items to a derived class specifically for disassembly tasks.

Global GroupTask::GetEndEffectorRobot ()

This uses magic strings in the XML files, let's find a better way.

Global GroupTask::GetManipulatorRobot ()

This uses magic strings in the XML files, let's find a better way.

Global GroupTask::GetStartConstraintCenter (GroupCfgType &_center) const noexcept

This is a hack that was needed to move the disassembly work forward before we had flushed out the tasks/groups. To be removed at the earliest opportunity.

Module Internal State

Remove m_k and m_radius - these don't apply to all NFs so it doesn't make sense to have them here. Fix design errors in SRT method which require this.

Class KdTreeNF

We should be able to use a Search_traits_adaptor to avoid recopying the points and instead put VIDs into the kd tree. However my attempt to do that resulted in bogus runtime behavior within CGAL's Kd_tree::build (dimension is computed incorrectly resulting in a bad array size). Figure out how to finagle this so that we can avoid recopying Cfgs.

This is limited to euclidean distance at present, but it should be possible to use any minkowski or mahalonobis distance with the appropriate distance adapter. It can not support arbitrary distances despite the claims in CGAL's documentation because the implementation requires finding the nearest and furthest point to an AABB; this is not tractible unless the geometrically nearest/farthest points (i.e. measured with euclidean) are also the nearest points for our metric.

Class LocalPlannerMethod

All local planners need to use a distance metric to set their edge weights properly; currently many of them are simply using the number of intermediate steps as a weight.

Local planners and Extenders represent the same concepts and should be merged into a single class with both an Extend and LocalPlan function. This will help simplify several other objects within PMPL as well, such as bi-directional RRT.

Class LPOutput

Destroy this object and have LPs/Extenders work directly with a LocalPlan object, which should replace the DefaultWeight class as our roadmap edge.

Global LPSweptDistance::Distance (const Cfg &_c1, const Cfg &_c2) override

Verify this, I believe m_path does include _c1 and _c2 for some lps. We need to make sure the usage is consistent across all of them.

Global MapEvaluatorMethod::m_activeRobots

This is an artifact of developing robot groups. It should be replaced by using a proper subgroup where each robot within is active.

Class MinkowskiDistance

Remove m_r2, which isn't part of the Minkowski difference and doesn't have any valid use.

Move the normalization option up to the base class.

Separate the computation of orientation and joint distances.

Global MinkowskiDistance::ScaleCfg (double _length, Cfg &_c, const Cfg &_o) override

This implementation is very poor. Scaling should be a constant-time operation - complexity should not depend on the length of the input vector.

Global MPProblem::ParseChild (XMLNode &_node)

We currently assume that the environment is parsed first. Need to make sure this always happens regardless of the XML file ordering.

Add check that tasks have unique labels

Global MPProblem::Print (std::ostream &_os) const

Print robot and task information.

Class MPSolutionType

Currently this object can represent a solution for each single robot and several robot group. It can almost support multiple of each - it just needs an interface for adding more robots/groups to the container.

Class MPStrategyMethod

Incorporate path constraints when generating the start and goal.

Global MPStrategyMethod::ClearRoadmap ()

This uses the STAPL graph 'clear' function, which doesn't activate any roadmap hooks. Methods which use hooks may have stale data after clearing the map. To fix we'll need to replace with our own function in RoadmapGraph.

Class MPTask

Support the use of path constraints in the library code, which currently doesn't care about this.

We should remove the capability here and redefine it as a constraint (since the constraints should fully define the task). The fact that a capability is or is not required should generally be discerned from the problem, while a capability constraint would more likely represent a deliberate restriction that is not strictly required (perhaps to express an operational preference). We will also eventually encounter cases where multiple capabilities are required to satsify a task.

Global NSphericalShell::Sample () const

This is likely close but not correct, need to verify.

Class ObstacleBasedSampler

Implement GetCenterOfMass function in the ChooseCenterOfMass class

Class ObstacleClearanceValidity

Re-implement this as a derived class of CollisionDetectionValidity and remove the 'GetCDMethod' function.

Global Path::Length () const

This will be an error if we allow self-edges.

Global PQPSolid::IsInsideObstacle (const mathtool::Vector3d &_point, const GMSPolyhedron &_polyhedron, const mathtool::Transformation &_transformation) override

This can still fail. I.e., what if we collide with two entering and one leaving triangle, as if we shot straight past the tip of a tetrahedron with the seem exactly aligned with the starting point? We need to check all colocated points, not just a pair. On detection of a scrape (colocated and different type) we need to erase all of the colocated collisions.

Class QueryMethod

Implement A* by accepting a distance metric label for the heuristic function, and expanding the SSSP functions to accept a binary functor heuristic function.

Class Rapid

There is no reason we can't support IsInsideObstacle with RAPID. This isn't really a part of PQP either - it's auxilliary code we created with a ray-shooting test. We can do the same thing here with an added 'RapidSolid' class, or by toggling the IsInsideObstacle check with a flag both here and in PQP.

Class RMSDDistance

Properly document. This looks like a very complex way to assess RMS distance between body translations.

Class Robot

Come up with a nice way to support both real and emulated hardware.

Global Robot::InitializePlanningSpaces ()

Set up a way to specify velocity limits for each joint.

Class SamplerMethod

The present layout of this class is confusing since the base method is actually generating uniform random samples which are filtered by the derived class. This does not work for many derived classes as evidenced by their overriding of Sample rather than Sampler: we should rework the design so that derived classes generate and filter their own samples. The base class can then implement its functions in terms of the generate and filter helpers.

The present implementation of Sample temporarily saves invalid configurations whether we will use them or not. Avoiding this extraneous retention will likely have good performance benefits in sampling-intensive applications such as proteins and manipulators where the success rate is very low.

Class ScaledEuclideanDistance

Replace with WeightedEuclidean, which full encompases this class and offers more functionality.

Separate the computation of orientation and joint distances.

Class std::hash< std::pair< T, U > >

Double-check that we get similar performance with differing types.

Global StraightLine::IsConnected (const GroupCfgType &_c1, const GroupCfgType &_c2, GroupCfgType &_col, GroupLPOutput *_lpOutput, double _positionRes, double _orientationRes, bool _checkCollision=true, bool _savePath=false, const Formation &_robotIndexes=Formation()) override

This can likely be optimized. For one, only one Configure call should be necessary here. Also a lot of the group Cfgs here could be made individual if using the leader, then using Configure on that.

Global StraightLine::IsConnectedEqualVelocities (const GroupCfgType &_c1, const GroupCfgType &_c2, GroupCfgType &_col, GroupLPOutput *_lpOutput, double _positionRes, double _orientationRes, bool _checkCollision, bool _savePath, const Formation &_robotIndexes)

This can likely be optimized. For one, only one Configure call should be necessary here. Also a lot of the group Cfgs here could be made individual if using the leader, then using Configure on that.

Global StraightLine::IsConnectedSLBinary (const Cfg &_c1, const Cfg &_c2, Cfg &_col, LPOutput *_lpOutput, int &_cdCounter, double _positionRes, double _orientationRes, bool _checkCollision=true, bool _savePath=false)

Why are we running a local planner on an interaction edge? That shouldn't be necessary since we need neither intermediates nor CD information in that case?

Global StraightLine::IsConnectedSLSequential (const Cfg &_c1, const Cfg &_c2, Cfg &_col, LPOutput *_lpOutput, int &_cdCounter, double _positionRes, double _orientationRes, bool _checkCollision=true, bool _savePath=false)

Why are we running a local planner on an interaction edge? That shouldn't be necessary since we need neither intermediates nor CD information in that case?

Global Syclop::ComputeFreeVolumes ()

Remove dependence on magic XML values.

Class TogglePRMStrategy

This can probably derive from BasicPRM to reduce duplication of common code.

Global TopologicalMap::ComputeApproximateMinimumInnerDistances (const WorkspaceRegion *const _source, const double _radius)

Refactor this and SSSP code to unify implementations. I think it should be feasible to make an adaptor class to give our grid (which represents an implicit graph) an API that is compatible with the STAPL (explicit) graph.

Global TopologicalMap::Initialize () override

We could probably do this lazily if we use something like a segment tree to find the regions which intersect a cell.

Move this to the decomposition class, which currently cannot do this job because it does not know its own label.

Global TwoVariableDijkstraSSSP (GraphType *const _g, const std::vector< typename GraphType::vertex_descriptor > &_starts, std::unordered_set< size_t > _goals, const double _startTime, const double _minEndTime, const double _lastConstraint, const double _lastGoalConstraint, SSSPPathWeightFunction< GraphType > &_weight)

If we want to add waiting, it should be added as a self edge here

Class WeightedEuclideanDistance

This doesn't work properly for linked robots. An additional weighting is required for the joint space, which is currently wrapped up with orientation.

Global WorkspaceDecomposition::AddTetrahedralRegion (const int _pts[4])

The boundary currently double-stores the points. We would like to have a non-mutable boundary that holds only references to the points.

Global WorkspaceDecomposition::FindNeighborhood (const std::vector< size_t > &_roots, const double _threshold) const

Remove the const-cast after STAPL fixes its API.

Global WorkspaceDecomposition::FindPath (const size_t _source, const size_t _target) const

Remove the const-cast after STAPL fixes its API.

Global WorkspaceRegion::FindSharedFacets (const WorkspaceRegion &_wr) const noexcept

Eliminate this extraneous copy. Perhaps check if = reverse within facet class.