diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index d0b9321..7d42b56 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -109,9 +109,7 @@ class MPCSim: # optimization loop # start=time.time() # dynamycs w.r.t robot frame - curr_state = np.array( - [0, 0, self.state[2], 0] - ) + curr_state = np.array([0, 0, self.state[2], 0]) # State Matrices A, B, C = mpcpy.get_linear_model_matrices(curr_state, self.action) # Get Reference_traj -> inputs are in worldframe @@ -139,6 +137,7 @@ class MPCSim: self.predict([self.action[0], self.action[1]]) self.preview(x_mpc.value) self.plot_sim() + def predict(self, u): def kinematics_model(x, t, u): dxdt = x[2] * np.cos(x[3]) diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 894592e..a66a08c 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -13,181 +13,277 @@ import time import pybullet as p import time + def get_state(robotId): - """ - """ + """ """ robPos, robOrn = p.getBasePositionAndOrientation(robotId) - linVel,angVel = p.getBaseVelocity(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]]) + 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./21 - steering = [0,2] - wheels = [8,15] +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) + 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) + 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) + p.setJointMotorControl2( + robotId, steer, 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.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.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.resetDebugVisualizerCamera( + cameraDistance=1.0, + cameraYaw=-90, + cameraPitch=-45, + cameraTargetPosition=[-0.1, -0.0, 0.65], + ) p.resetSimulation() - p.setGravity(0,0,-10) + p.setGravity(0, 0, -10) useRealTimeSim = 1 - p.setTimeStep(1./120.) - p.setRealTimeSimulation(useRealTimeSim) # either this + 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) + # 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)): - #print("joint[",wheel,"]=", p.getJointInfo(car,wheel)) - p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) - p.getJointInfo(car,wheel) + # 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, + 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, + 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, + 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, + 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]) - 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,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) + 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) + 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 + 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 - + 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=[] + 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("Press Enter to continue...") - - while (1): + + 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]) + # 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: + 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) + 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 + # 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 + # 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() - - #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(), + 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 + + 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]) + set_ctrl(car, state[2], action[0], action[1]) - if P.DT-elapsed>0: - time.sleep(P.DT-elapsed) + if P.DT - elapsed > 0: + time.sleep(P.DT - elapsed) -if __name__ == '__main__': + +if __name__ == "__main__": run_sim()