sampleControl

Generate control command and duration

Since R2021b

Syntax

[u,steps] = sampleControl(mobileProp,q0,u0,qTgt)

Description

[u,steps] = sampleControl(mobileProp,q0,u0,qTgt) generates a series of control commands and number of steps to move from the current state q0 with control command u0 toward the target state qTgt.

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.

q0 — Initial state

s-element vector

Initial state of the system, specified as an s-element vector. s is the number of state variables in the state space.

u0 — Initial control on the initial state

c-element vector

Initial control input, specified as an c-element vector. c is the number of control inputs.

qTgt — Target state

s-element vector

Target state of the system, specified as an s-element vector. s is the number of state variables in the state space.

Output Arguments

u — Control inputs for propagating states

n-by-c matrix

Control inputs for propagating states, returned as an c-element vector. c is the number of control inputs.

steps — Number of steps from each state and control input to next

n-element vector of positive integers

Number of steps from each state and control input to next, returned as an n-element vector of positive integers.

Version History

Introduced in R2021b