290 lines
7.2 KiB
Python
290 lines
7.2 KiB
Python
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
from matplotlib import animation
|
|
|
|
from mpcpy.utils import compute_path_from_wp
|
|
import mpcpy
|
|
|
|
P = mpcpy.Params()
|
|
|
|
import sys
|
|
import time
|
|
|
|
import pybullet as p
|
|
import time
|
|
|
|
|
|
def get_state(robotId):
|
|
""" """
|
|
robPos, robOrn = p.getBasePositionAndOrientation(robotId)
|
|
linVel, angVel = p.getBaseVelocity(robotId)
|
|
|
|
return np.array(
|
|
[
|
|
robPos[0],
|
|
robPos[1],
|
|
np.sqrt(linVel[0] ** 2 + linVel[1] ** 2),
|
|
p.getEulerFromQuaternion(robOrn)[2],
|
|
]
|
|
)
|
|
|
|
|
|
def set_ctrl(robotId, currVel, acceleration, steeringAngle):
|
|
|
|
gearRatio = 1.0 / 21
|
|
steering = [0, 2]
|
|
wheels = [8, 15]
|
|
maxForce = 50
|
|
|
|
targetVelocity = currVel + acceleration * P.DT
|
|
# targetVelocity=lastVel
|
|
# print(targetVelocity)
|
|
|
|
for wheel in wheels:
|
|
p.setJointMotorControl2(
|
|
robotId,
|
|
wheel,
|
|
p.VELOCITY_CONTROL,
|
|
targetVelocity=targetVelocity / gearRatio,
|
|
force=maxForce,
|
|
)
|
|
|
|
for steer in steering:
|
|
p.setJointMotorControl2(
|
|
robotId, steer, p.POSITION_CONTROL, targetPosition=steeringAngle
|
|
)
|
|
|
|
|
|
def plot_results(path, x_history, y_history):
|
|
""" """
|
|
plt.style.use("ggplot")
|
|
plt.figure()
|
|
plt.title("MPC Tracking Results")
|
|
|
|
plt.plot(
|
|
path[0, :], path[1, :], c="tab:orange", marker=".", label="reference track"
|
|
)
|
|
plt.plot(
|
|
x_history,
|
|
y_history,
|
|
c="tab:blue",
|
|
marker=".",
|
|
alpha=0.5,
|
|
label="vehicle trajectory",
|
|
)
|
|
plt.axis("equal")
|
|
plt.legend()
|
|
plt.show()
|
|
|
|
|
|
def run_sim():
|
|
""" """
|
|
p.connect(p.GUI)
|
|
p.resetDebugVisualizerCamera(
|
|
cameraDistance=1.0,
|
|
cameraYaw=-90,
|
|
cameraPitch=-45,
|
|
cameraTargetPosition=[-0.1, -0.0, 0.65],
|
|
)
|
|
|
|
p.resetSimulation()
|
|
|
|
p.setGravity(0, 0, -10)
|
|
useRealTimeSim = 1
|
|
|
|
p.setTimeStep(1.0 / 120.0)
|
|
p.setRealTimeSimulation(useRealTimeSim) # either this
|
|
|
|
plane = p.loadURDF("racecar/plane.urdf")
|
|
# track = p.loadSDF("racecar/f10_racecar/meshes/barca_track.sdf", globalScaling=1)
|
|
|
|
car = p.loadURDF("racecar/f10_racecar/racecar_differential.urdf", [0, 0.3, 0.3])
|
|
for wheel in range(p.getNumJoints(car)):
|
|
# print("joint[",wheel,"]=", p.getJointInfo(car,wheel))
|
|
p.setJointMotorControl2(
|
|
car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0
|
|
)
|
|
p.getJointInfo(car, wheel)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
9,
|
|
car,
|
|
11,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
10,
|
|
car,
|
|
13,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
9,
|
|
car,
|
|
13,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
16,
|
|
car,
|
|
18,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
16,
|
|
car,
|
|
19,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
17,
|
|
car,
|
|
19,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
1,
|
|
car,
|
|
18,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
|
|
c = p.createConstraint(
|
|
car,
|
|
3,
|
|
car,
|
|
19,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
|
|
|
|
# Interpolated Path to follow given waypoints
|
|
path = compute_path_from_wp(
|
|
[0, 3, 4, 6, 10, 11, 12, 6, 1, 0],
|
|
[0, 0, 2, 4, 3, 3, -1, -6, -2, -2],
|
|
P.path_tick,
|
|
)
|
|
|
|
for x_, y_ in zip(path[0, :], path[1, :]):
|
|
p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1])
|
|
|
|
# starting guess
|
|
action = np.zeros(P.M)
|
|
action[0] = P.MAX_ACC / 2 # a
|
|
action[1] = 0.0 # delta
|
|
|
|
# Cost Matrices
|
|
Q = np.diag([20, 20, 10, 20]) # state error cost
|
|
Qf = np.diag([30, 30, 30, 30]) # state final error cost
|
|
R = np.diag([10, 10]) # input cost
|
|
R_ = np.diag([10, 10]) # input rate of change cost
|
|
|
|
mpc = mpcpy.MPC(P.N, P.M, Q, R)
|
|
x_history = []
|
|
y_history = []
|
|
|
|
time.sleep(0.5)
|
|
input("\033[92m Press Enter to continue... \033[0m")
|
|
|
|
while 1:
|
|
|
|
state = get_state(car)
|
|
x_history.append(state[0])
|
|
y_history.append(state[1])
|
|
|
|
# track path in bullet
|
|
p.addUserDebugLine(
|
|
[state[0], state[1], 0], [state[0], state[1], 0.5], [1, 0, 0]
|
|
)
|
|
|
|
if np.sqrt((state[0] - path[0, -1]) ** 2 + (state[1] - path[1, -1]) ** 2) < 0.2:
|
|
print("Success! Goal Reached")
|
|
set_ctrl(car, 0, 0, 0)
|
|
plot_results(path, x_history, y_history)
|
|
input("Press Enter to continue...")
|
|
p.disconnect()
|
|
return
|
|
|
|
# for MPC car ref frame is used
|
|
state[0:2] = 0.0
|
|
state[3] = 0.0
|
|
|
|
# add 1 timestep delay to input
|
|
state[0] = state[0] + state[2] * np.cos(state[3]) * P.DT
|
|
state[1] = state[1] + state[2] * np.sin(state[3]) * P.DT
|
|
state[2] = state[2] + action[0] * P.DT
|
|
state[3] = state[3] + action[0] * np.tan(action[1]) / P.L * P.DT
|
|
|
|
# optimization loop
|
|
start = time.time()
|
|
|
|
# State Matrices
|
|
A, B, C = mpcpy.get_linear_model_matrices(state, action)
|
|
|
|
# Get Reference_traj -> inputs are in worldframe
|
|
target, _ = mpcpy.get_ref_trajectory(get_state(car), path, 1.0)
|
|
|
|
x_mpc, u_mpc = mpc.optimize_linearized_model(
|
|
A, B, C, state, target, time_horizon=P.T, verbose=False
|
|
)
|
|
|
|
# action = np.vstack((np.array(u_mpc.value[0,:]).flatten(),
|
|
# (np.array(u_mpc.value[1,:]).flatten())))
|
|
|
|
action[:] = [u_mpc.value[0, 1], u_mpc.value[1, 1]]
|
|
|
|
elapsed = time.time() - start
|
|
print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
|
|
|
|
set_ctrl(car, state[2], action[0], action[1])
|
|
|
|
if P.DT - elapsed > 0:
|
|
time.sleep(P.DT - elapsed)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
run_sim()
|