12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

formatted scripts with black

master
mcarfagno 2022-07-21 12:23:08 +01:00
parent 7fd594bd6c
commit 7bcd72cc66
2 changed files with 208 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -109,9 +109,7 @@ class MPCSim:
# optimization loop # optimization loop
# start=time.time() # start=time.time()
# dynamycs w.r.t robot frame # dynamycs w.r.t robot frame
curr_state = np.array( curr_state = np.array([0, 0, self.state[2], 0])
[0, 0, self.state[2], 0]
)
# State Matrices # State Matrices
A, B, C = mpcpy.get_linear_model_matrices(curr_state, self.action) A, B, C = mpcpy.get_linear_model_matrices(curr_state, self.action)
# Get Reference_traj -> inputs are in worldframe # Get Reference_traj -> inputs are in worldframe
@ -139,6 +137,7 @@ class MPCSim:
self.predict([self.action[0], self.action[1]]) self.predict([self.action[0], self.action[1]])
self.preview(x_mpc.value) self.preview(x_mpc.value)
self.plot_sim() self.plot_sim()
def predict(self, u): def predict(self, u):
def kinematics_model(x, t, u): def kinematics_model(x, t, u):
dxdt = x[2] * np.cos(x[3]) dxdt = x[2] * np.cos(x[3])

316
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

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