Package org.robwork.sdurw_pathplanning
Class PlannerUtil
- java.lang.Object
-
- org.robwork.sdurw_pathplanning.PlannerUtil
-
public class PlannerUtil extends java.lang.Object
PlannerUtil provides various utilities useful in path planning
-
-
Nested Class Summary
Nested Classes Modifier and Type Class Description static class
PlannerUtil.EstimateType
Description of the different estimation type possible in the
estimateMotionWeights(EsitmateType, size_t) method
-
Constructor Summary
Constructors Constructor Description PlannerUtil(long cPtr, boolean cMemoryOwn)
-
Method Summary
All Methods Static Methods Instance Methods Concrete Methods Modifier and Type Method Description static Q
clampPosition(PairQ bounds, Q q)
Clamps values to be within bounds
The method treats all joints individually and clamps them to be
within the position bounds of the configuration space box.
static Q
clampPosition(Device device, Q q)
Clamps values to be within bounds
The method treats all joints individually and clamps them to be within the
position bounds of the device
void
delete()
static Q
estimateMotionWeights(Device device, FrameCPtr frame, State initialState, PlannerUtil.EstimateType type, long samples)
Estimate the distance traveled by the frame, when moving the joints
The estimate is based on sampling samples random configuration and
using the Jacobian to estimate the distance traveled when moving the
joints.static long
getCPtr(PlannerUtil obj)
static boolean
inCollision(PlannerConstraint constraint, Q q)
Collision checking for a configuration.
static boolean
inCollision(PlannerConstraint constraint, Q start, Q end)
Collision checking for a segment.
static boolean
inCollision(PlannerConstraint constraint, Q start, Q end, boolean checkStart)
Collision checking for a segment.
static boolean
inCollision(PlannerConstraint constraint, Q start, Q end, boolean checkStart, boolean checkEnd)
Collision checking for a segment.
static boolean
inCollision(PlannerConstraint constraint, VectorQ path)
Collision checking for a path of configurations.
Each configuration and edge of path is checked using the
configuration and edge constraints of constraint.
constraint [in] Constraint for path.
path [in] Sequence of configurations.static MetricQPtr
normalizingInfinityMetric(PairQ bounds)
Weighted infinity metric that maps the maps the longest vector
in the configuration space to a given length.
static MetricQPtr
normalizingInfinityMetric(PairQ bounds, double length)
Weighted infinity metric that maps the maps the longest vector
in the configuration space to a given length.
static MetricQPtr
timeMetric(Q speed)
Metric for the distance in time between a pair of
configurations.
The metric implements a simple scaled infinity-norm metric: The
metric assumes that the joints move synchronously with the joint
velocities given by speed.static MetricQPtr
timeMetric(Device device)
Metric for the distance in time between a pair of
configurations.
The metric implements a simple scaled infinity-norm metric: The
metric assumes that the joints move synchronously with the maximum
joint velocities given by device.
-
-
-
Method Detail
-
getCPtr
public static long getCPtr(PlannerUtil obj)
-
delete
public void delete()
-
inCollision
public static boolean inCollision(PlannerConstraint constraint, VectorQ path)
Collision checking for a path of configurations.
Each configuration and edge of path is checked using the
configuration and edge constraints of constraint.
constraint [in] Constraint for path.
path [in] Sequence of configurations.- Returns:
- true iff path is in collision.
-
inCollision
public static boolean inCollision(PlannerConstraint constraint, Q start, Q end, boolean checkStart, boolean checkEnd)
Collision checking for a segment.
- Parameters:
start
- [in] Start of segmentend
- [in] End of segmentconstraint
- [in] Constraint for segmentcheckStart
- [in] Check start configuration for collision.checkEnd
- [in] Check end configuration for collision.
-
inCollision
public static boolean inCollision(PlannerConstraint constraint, Q start, Q end, boolean checkStart)
Collision checking for a segment.
- Parameters:
start
- [in] Start of segmentend
- [in] End of segmentconstraint
- [in] Constraint for segmentcheckStart
- [in] Check start configuration for collision.
-
inCollision
public static boolean inCollision(PlannerConstraint constraint, Q start, Q end)
Collision checking for a segment.
- Parameters:
start
- [in] Start of segmentend
- [in] End of segmentconstraint
- [in] Constraint for segment
-
inCollision
public static boolean inCollision(PlannerConstraint constraint, Q q)
Collision checking for a configuration.
- Parameters:
constraint
- [in] Collision checking constraint.q
- [in] Configuration to check for collisions.
-
normalizingInfinityMetric
public static MetricQPtr normalizingInfinityMetric(PairQ bounds, double length)
Weighted infinity metric that maps the maps the longest vector
in the configuration space to a given length.
- Parameters:
bounds
- [in] Lower and upper corner of the configuration space.
length
- [in] The wanted distance between lower and upper corner.
- Returns:
- Metric for which the distance from lower to upper corner
equals length.
-
normalizingInfinityMetric
public static MetricQPtr normalizingInfinityMetric(PairQ bounds)
Weighted infinity metric that maps the maps the longest vector
in the configuration space to a given length.
- Parameters:
bounds
- [in] Lower and upper corner of the configuration space.
- Returns:
- Metric for which the distance from lower to upper corner
equals length.
-
timeMetric
public static MetricQPtr timeMetric(Device device)
Metric for the distance in time between a pair of
configurations.
The metric implements a simple scaled infinity-norm metric: The
metric assumes that the joints move synchronously with the maximum
joint velocities given by device.
-
timeMetric
public static MetricQPtr timeMetric(Q speed)
Metric for the distance in time between a pair of
configurations.
The metric implements a simple scaled infinity-norm metric: The
metric assumes that the joints move synchronously with the joint
velocities given by speed.
-
estimateMotionWeights
public static Q estimateMotionWeights(Device device, FrameCPtr frame, State initialState, PlannerUtil.EstimateType type, long samples)
Estimate the distance traveled by the frame, when moving the joints
The estimate is based on sampling samples random configuration and
using the Jacobian to estimate the distance traveled when moving the
joints. This estimate can be used in the WeightedEuclideanMetric.
- Parameters:
device
- [in] Device to estimate weights forframe
- [in] Frame to calculate weights for. If null the device
end frame will be usedinitialState
- [in] Initial state to use in estimationtype
- [in] The estimation typesamples
- [in] The number of samples to use (default 1000)- Returns:
- Weights representing the distance
-
clampPosition
public static Q clampPosition(Device device, Q q)
Clamps values to be within bounds
The method treats all joints individually and clamps them to be within the
position bounds of the device
- Parameters:
device
- [in] The device for the configurations.q
- [in] Configuration to clamp- Returns:
- The clamped configuration
-
clampPosition
public static Q clampPosition(PairQ bounds, Q q)
Clamps values to be within bounds
The method treats all joints individually and clamps them to be
within the position bounds of the configuration space box.
- Parameters:
bounds
- [in] The bounds of the configuration spaceq
- [in] Configuration to clamp- Returns:
- The clamped configuration
-
-