Function stomp_moveit::costs::getCostFunctionFromStateValidator

• Defined in File cost_functions.hpp

Function Documentation

CostFn stomp_moveit::costs::getCostFunctionFromStateValidator(const StateValidatorFn &state_validator_fn, double interpolation_step_size)

Creates a cost function from a robot state validation function. This is used for computing smooth cost profiles for waypoint state conditions like collision checks and constraints. The validator function is applied for all states in the validated path while also considering interpolated states. If a waypoint or an interpolated state is invalid, a local penalty is being applied to the path. Penalty costs are being smoothed out using a Gaussian so that valid neighboring states (near collisions) are optimized as well. This implementation does not support cost thresholds, non-zero local costs will render the trajectory as invalid.

Parameters:

• state_validator_fn – The validator function that produces local costs for all waypoints

• interpolation_step_size – The L2 norm distance step used for interpolation (disabled when set to 0.0)

Returns:

Cost function that computes smooth costs for binary validity conditions