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

simulation code cleanup

master
mcarfagno 2020-04-08 14:01:42 +01:00
parent f40c5a0387
commit 5190e3d4ef
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
mpc_demo/mpc_demo_pybullet.py
View File

@ -32,6 +32,8 @@ def set_ctrl(robotId,v,w):
p.setJointMotorControl2(robotId,1,p.VELOCITY_CONTROL,targetVelocity=rightWheelVelocity,force=1000) p.setJointMotorControl2(robotId,1,p.VELOCITY_CONTROL,targetVelocity=rightWheelVelocity,force=1000)
def plot(path,x_history,y_history): def plot(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")
@ -42,20 +44,20 @@ def plot(path,x_history,y_history):
plt.legend() plt.legend()
plt.show() plt.show()
def do_sim(): def run_sim():
"""
"""
p.connect(p.GUI) p.connect(p.GUI)
start_offset = [0,2,0] start_offset = [0,2,0]
start_orientation = p.getQuaternionFromEuler([0,0,0]) start_orientation = p.getQuaternionFromEuler([0,0,0])
turtle = p.loadURDF("turtlebot.urdf",start_offset, start_orientation) turtle = p.loadURDF("turtlebot.urdf",start_offset, start_orientation)
plane = p.loadURDF("plane.urdf") plane = p.loadURDF("plane.urdf")
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
p.setGravity(0,0,-10) p.setGravity(0,0,-10)
state = get_state(turtle) # MPC time step
# MPC step
dt = 0.25 dt = 0.25
# starting guess output # starting guess output
@ -73,6 +75,7 @@ def do_sim():
y_history=[] y_history=[]
while (1): while (1):
state = get_state(turtle) state = get_state(turtle)
x_history.append(state[0]) x_history.append(state[0])
y_history.append(state[1]) y_history.append(state[1])
@ -99,4 +102,4 @@ def do_sim():
time.sleep(dt-elapsed) time.sleep(dt-elapsed)
if __name__ == '__main__': if __name__ == '__main__':
do_sim() run_sim()