distance
Description
estimates the cost of propagating from one state to another. The
h
= distance(mobileProp
,q1
,q2
)DistanceEstimator
property of the state propagator defines the
distance metric for approximating cost.
Examples
Create Path Representing Control-Based Kinematic Trajectory
Create a state propagator and specify the distance metric for estimating propagation cost.
propagator = mobileRobotPropagator(DistanceEstimator="dubins");
Create a Dubins state space.
dubinsSpace = stateSpaceDubins([0 25; 0 25; -pi pi]);
Update the state space of the state propagator using the created state space.
propagator.StateSpace = dubinsSpace; setup(propagator)
Create a navPath
object based on multiple waypoints in a Dubins space.
path = navPath(dubinsSpace); waypoints = [8 10 pi/2; 7 14 pi/4; 10 17 pi/2; 10 10 -pi]; append(path,waypoints)
Interpolate that path so that it contains exactly 250 points.
numStates = 250; interpolate(path,numStates)
Extract the sequence of motions from the path.
q1 = path.States(1:end-1,:); % Initial states q2 = path.States(2:end,:); % Final states
Estimate the cost of propagating to target state.
cost = distance(propagator,q1,q2);
Generate a series of control commands and number of steps to move from the current state q1
with control command u
toward the target state q2
.
u = zeros(size(q1,1),propagator.NumControlOutput); steps = zeros(size(q1,1),1); for i = 1:size(q1,1) [u(i+1,:),steps(i)] = sampleControl(propagator,q1(i,:),u(i,:),q2(i,:)); end
Create a control-based path object with the specified state propagator and a sequence of specified states, controls, targets, and durations.
states = path.States; controls = u(2:end,:); targets = q2; durations = steps*propagator.ControlStepSize; path2 = navPathControl(propagator,states,controls,targets,durations);
Visualize the results.
figure grid on axis equal hold on plot(path2.States(:,1),path2.States(:,2),".b") plot(waypoints(:,1),waypoints(:,2),"*r","MarkerSize",10)
Input Arguments
mobileProp
— Mobile robot state propagator
mobileRobotPropagator
object
Mobile robot state propagator, specified as a mobileRobotPropagator
object.
q1
— Initial states
n-by-s matrix
Initial states, specified as an n-by-s matrix. n is the number of states and s is the size of the state vector.
q2
— Final states
n-by-s matrix
Final states, specified as an n-by-s matrix. n is the number of states and s is the size of the state vector.
Output Arguments
Version History
Introduced in R2021b
See Also
Objects
Functions
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)