From 5476ded9616c469f98bef2750777ccca7ac12206 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Fri, 21 Apr 2023 17:24:28 +0100 Subject: [PATCH] simplify mpc return type --- mpc_pybullet_demo/mpc_demo_pybullet.py | 14 +++----------- mpc_pybullet_demo/mpcpy/cvxpy_mpc.py | 2 +- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 03efa5d..6f86129 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -30,15 +30,12 @@ def get_state(robotId): 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( @@ -230,7 +227,6 @@ def run_sim(): 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]) @@ -267,19 +263,15 @@ def run_sim(): # 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( + # MPC step + acc, steer = 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]) + set_ctrl(car, state[2], acc, steer) if P.DT - elapsed > 0: time.sleep(P.DT - elapsed) diff --git a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py index a9c64f5..fe0b780 100755 --- a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py +++ b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py @@ -132,4 +132,4 @@ class MPC: prob = opt.Problem(opt.Minimize(cost), constr) solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False) - return x, u + return u[:, 0].value