def ctrl_traj(x, y, th, ctrl_prev, x_d, y_d, xd_d, yd_d, xdd_d, ydd_d, x_g,
y_g, th_g):
# (x,y,th): current state
# ctrl_prev: previous control input (V,om)
# (x_d, y_d): desired position
# (xd_d, yd_d): desired velocity
# (xdd_d, ydd_d): desired acceleration
# (x_g,y_g,th_g): desired final state
# Timestep
dt = 0.005
# Gains
kpx = 1
kpy = 1
kdx = 0.5
kdy = 0.5
# Cutoffs
d_eps = 0.5 # Switch to pose controller
V_eps = 1e-5 # Reset to nominal velocity
# Define control inputs (V,om) - without saturation constraints
# Switch to pose controller once "close" enough, i.e., when
# the robot is within 0.5m of the goal xy position.
if np.sqrt((x_g - x)**2 + (y_g - y)**2) <= d_eps:
return ctrl_pose(x, y, th, x_g, y_g, th_g)
# Assume current velocity is that commanded in previous timestep
V_prev = ctrl_prev[0]
x_dot = V_prev * np.cos(th)
y_dot = V_prev * np.sin(th)
# Virtual control law
u1 = xdd_d + kpx * (x_d - x) + kdx * (xd_d - x_dot)
u2 = ydd_d + kpy * (y_d - y) + kdy * (yd_d - y_dot)
# Recover actual control inputs
V_dot = u1 * np.cos(th) + u2 * np.sin(th)
V = V_prev + dt * V_dot # Euler step
# Reset to desired velocity if V = 0
if np.abs(V) <= V_eps:
V = np.sqrt(xd_d**2 + yd_d**2)
print "Resetting to nominal velocity: ", V
om = (-u1 * np.sin(th) + u2 * np.cos(th)) / V
# Apply saturation limits
V = np.sign(V) * min(0.5, np.abs(V))
om = np.sign(om) * min(1, np.abs(om))
return np.array([V, om])
def ctrl_traj(x, y, th, ctrl_prev, x_d, y_d, xd_d, yd_d, xdd_d, ydd_d, x_g,
y_g, th_g):
# (x,y,th): current state
# ctrl_prev: previous control input (V,om)
# (x_d, y_d): desired position
# (xd_d, yd_d): desired velocity
# (xdd_d, ydd_d): desired acceleration
# (x_g,y_g,th_g): desired final state
# Timestep
dt = 0.005
# Gains
kpx = 1.0
kpy = 1.0
kdx = 2.0
kdy = 2.0
# Distance form current pos to the target
dist = np.sqrt((x_g - x)**2 + (y_g - y)**2)
# Define control inputs (V,om) - without saturation constraints
# Switch to pose controller once "close" enough, i.e., when
# the robot is within 0.5m of the goal xy position.
if dist <= 0.5:
V, om = ctrl_pose(x, y, th, x_g, y_g, th_g)
else:
if abs(ctrl_prev[0]) <= 1e-5:
V_prev = np.sign(ctrl_prev[0]) * 1e-5
else:
V_prev = ctrl_prev[0]
x_dot = V_prev * np.cos(th)
y_dot = V_prev * np.sin(th)
# virtual control inputs
u = np.array([
xdd_d + kpx * (x_d - x) + kdx * (xd_d - x_dot),
ydd_d + kpy * (y_d - y) + kdy * (yd_d - y_dot)
])
J = np.array([[np.cos(th), -V_prev * np.sin(th)],
[np.sin(th), V_prev * np.cos(th)]])
a, om = np.linalg.solve(J, u)
V = V_prev + a * dt
# Apply saturation limits
V = np.sign(V) * min(0.5, np.abs(V))
om = np.sign(om) * min(1, np.abs(om))
return np.array([V, om])
def ctrl_traj(x, y, th, ctrl_prev, x_d, y_d, xd_d, yd_d, xdd_d, ydd_d, x_g,
y_g, th_g):
# (x,y,th): current state
# ctrl_prev: previous control input (V,om)
# (x_d, y_d): desired position
# (xd_d, yd_d): desired velocity
# (xdd_d, ydd_d): desired acceleration
# (x_g,y_g,th_g): desired final state
# Timestep
dt = 0.005
# Gains
kpx = 0.9
kpy = 0.9
kdx = 0.7
kdy = 0.7
# Define control inputs (V,om) - without saturation constraints
# Switch to pose controller once "close" enough, i.e., when
# the robot is within 0.5m of the goal xy position.
if (math.sqrt((x - x_d)**2 + (y - y_d)**2) > 0.0):
# Use virtual control law
V_prev = ctrl_prev[0]
om_prev = ctrl_prev[1]
xd = V_prev * math.cos(th)
yd = V_prev * math.sin(th)
u1 = xdd_d + kpx * (x_d - x) + kdx * (xd_d - xd)
u2 = ydd_d + kpy * (y_d - y) + kdy * (yd_d - yd)
Vd = u1 * math.cos(th) + u2 * math.sin(th)
V = V_prev + Vd * dt
if V <= 0:
V = math.sqrt(xd_d**2 + yd_d**2) # reset to nominal velocity
om = (u2 * math.cos(th) - u1 * math.sin(th)) / V
else:
# Use pose controller from problem 3
ctrl = ctrl_pose(x, y, th, x_g, y_g, th_g)
V = ctrl[0]
om = ctrl[1]
# Apply saturation limits
V = np.sign(V) * min(0.5, np.abs(V))
om = np.sign(om) * min(1, np.abs(om))
return np.array([V, om])
#timestep
dt = 0.005
N = int (t_end/dt)
# Set up simulation
time = dt * np.array(range(N+1))
state = np.zeros((N+1,3))
state[0,:] = np.array([[x_0, y_0, th_0]])
x = state[0,:]
ctrl = np.zeros((N,2))
for i in range(N):
ctrl_fbck = ctrl_pose(x[0], x[1], x[2], x_g, y_g, th_g)
ctrl[i,0] = ctrl_fbck[0]
ctrl[i,1] = ctrl_fbck[1]
d_state = odeint(car_dyn, x, np.array([time[i], time[i+1]]), args=(ctrl[i,:], [0,0]) )
x = d_state[1,:]
state[i+1,:] = x
# Plots
maybe_makedirs('plots')
plt.figure(figsize=(15,5))
plt.subplot(1,3,1)
plt.plot(state[:,0],state[:,1],linewidth=1)
plt.title('Trajectory')
plt.quiver(state[0:-1:200,0],state[0:-1:200,1],np.cos(state[0:-1:200,2]), np.sin(state[0:-1:200,2]))
plt.grid('on')
def ctrl_traj(x, y, th, ctrl_prev, x_d, y_d, xd_d, yd_d, xdd_d, ydd_d, x_g,
y_g, th_g):
'''
This function computes the closed-loop control law.
Inputs:
(x,y,th): current state
ctrl_prev: previous control input (V,om)
(x_d, y_d): desired position
(xd_d, yd_d): desired velocity
(xdd_d, ydd_d): desired acceleration
(x_g,y_g,th_g): desired final state
Outputs:
(V, om): a numpy array np.array([V, om]) containing the desired control inputs
'''
# Timestep
dt = 0.005
########## Code starts here ##########
kpx = 1
kpy = 1
kdx = 2
kdy = 2
dist_from_goal = np.sqrt((x_g - x)**2 + (y_g - y)**2)
if (dist_from_goal < 0.5):
# switch control laws because we're close to the goal
[V, om] = ctrl_pose(x, y, th, x_g, y_g, th_g)
else:
# Set up and solve for a, w
V_prev = ctrl_prev[0]
w_prev = ctrl_prev[1]
xdot = V_prev * np.cos(th)
ydot = V_prev * np.sin(th)
u1 = xdd_d + kpx * (x_d - x) + kdx * (xd_d - xdot)
u2 = ydd_d + kpy * (y_d - y) + kdy * (yd_d - ydot)
J = np.array([[np.cos(th), -V_prev * np.sin(th)],
[np.sin(th), V_prev * np.cos(th)]])
u = np.array([u1, u2])
aw = linalg.solve(J, u)
a = aw[0]
om = aw[1]
# Integrate to obtain V
V = V_prev + a * dt
#reset V if it becomes 0 to avoid singularity
if (abs(V) < 0.01):
V = np.sqrt((xd_d - xdot)**2 + (yd_d - xdot)**2)
#Apply saturation limits
V = np.sign(V) * min(0.5, abs(V))
om = np.sin(om) * min(1.0, abs(om))
########## Code ends here ##########
return np.array([V, om])
def ctrl_traj(x, y, th, ctrl_prev, x_d, y_d, xd_d, yd_d, xdd_d, ydd_d, x_g,
y_g, th_g):
'''
This function computes the closed-loop control law.
Inputs:
(x,y,th): current state
ctrl_prev: previous control input (V,om)
(x_d, y_d): desired position
(xd_d, yd_d): desired velocity
(xdd_d, ydd_d): desired acceleration
(x_g,y_g,th_g): desired final state
Outputs:
(V, om): a numpy array np.array([V, om]) containing the desired control inputs
'''
# Timestep
dt = 0.005
########## Code starts here ##########
# Let's set gains
kpx = 1.5
kpy = 1.0
kdx = 2.5
kdy = 2.0
rho = np.sqrt(
(x_g - x)**2 +
(y_g - y)**2) # Distance between current position and goal position
# If the robot is sufficiently close (rho <= 0.5); switch to pose stabilisation controller from Problem 3
if rho <= 0.5:
V, om = ctrl_pose(x, y, th, x_g, y_g, th_g)
else: # Dealing with singularity
if abs(ctrl_prev[0]) <= 1e-6:
V_ = np.sign(ctrl_prev[0]) * 1e-6
else:
V_ = ctrl_prev[0]
xd = V_ * np.cos(th)
yd = V_ * np.sin(th)
#Adding control inputs
# As shown in the equation in Problem #2
u = np.array([
xdd_d + kpx * (x_d - x) + kdx * (xd_d - xd),
ydd_d + kpy * (y_d - y) + kdy * (yd_d - yd)
])
J = np.array([[np.cos(th), -V_ * np.sin(th)],
[np.sin(th), V_ * np.cos(th)]])
a, om = np.linalg.solve(J, u)
V = V_prev + a * dt
# Keeping V < 0.5 and om < 1
V_max = 0.5
om_max = 1
om = np.sign(om) * min(om_max, np.abs(om))
V = np.sign(V) * min(V_max, np.abs(V))
########## Code ends here ##########
return np.array([V, om])