diff --git a/bindings/expose-mpc.cpp b/bindings/expose-mpc.cpp index 92271d4e..65bcba54 100644 --- a/bindings/expose-mpc.cpp +++ b/bindings/expose-mpc.cpp @@ -29,8 +29,6 @@ namespace simple_mpc { MPCSettings conf; - conf.ddpIteration = bp::extract(settings["ddpIteration"]); - conf.support_force = bp::extract(settings["support_force"]); conf.TOL = bp::extract(settings["TOL"]); @@ -50,7 +48,6 @@ namespace simple_mpc { MPCSettings & conf = self.settings_; bp::dict settings; - settings["ddpIteration"] = conf.ddpIteration; settings["support_force"] = conf.support_force; settings["TOL"] = conf.TOL; settings["mu_init"] = conf.mu_init; diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index b76ca227..c754cea6 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -84,7 +84,6 @@ T_ss = 30 N_simu = int(0.01 / 0.001) mpc_conf = dict( - ddpIteration=1, support_force=-model_handler.getMass() * gravity[2], TOL=1e-4, mu_init=1e-8, diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py index 6a34b636..1016791d 100644 --- a/examples/go2_kinodynamics.py +++ b/examples/go2_kinodynamics.py @@ -31,7 +31,7 @@ fref = np.zeros(force_size) fref[2] = -model_handler.getMass() / nk * gravity[2] u0 = np.concatenate((fref, fref, fref, fref, np.zeros(model_handler.getModel().nv - 6))) - +dt = 0.01 w_basepos = [0, 0, 100, 10, 10, 0] w_legpos = [1, 1, 1] @@ -60,7 +60,7 @@ w_centder = np.diag(np.concatenate((w_centder_lin, w_centder_ang))) problem_conf = dict( - timestep=0.01, + timestep=dt, w_x=w_x, w_u=w_u, w_cent=w_cent, @@ -85,7 +85,6 @@ T_ss = 30 mpc_conf = dict( - ddpIteration=1, support_force=-model_handler.getMass() * gravity[2], TOL=1e-4, mu_init=1e-8, @@ -94,7 +93,7 @@ swing_apex=0.15, T_fly=T_ss, T_contact=T_ds, - timestep=0.01, + timestep=dt, ) mpc = MPC(mpc_conf, dynproblem) @@ -149,7 +148,7 @@ qp = IDSolver(id_conf, model_handler.getModel()) """ Interpolation """ -interpolator = Interpolator(nq + nv, nv, nu, nf, 0.01) +interpolator = Interpolator(model_handler.getModel()) """ Initialize simulation""" device = BulletRobot( @@ -252,7 +251,10 @@ for j in range(N_simu): # time.sleep(0.01) - interpolator.interpolate(j / float(N_simu), xss, uss, ddqs, forces) + delay = j / float(N_simu) * dt + + acc_interp = interpolator.interpolateLinear(delay, dt, ddqs) + force_interp = interpolator.interpolateLinear(delay, dt, forces) q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) @@ -263,9 +265,9 @@ mpc.getDataHandler().getData(), contact_states, x_measured[nq:], - interpolator.a_interpolated, + acc_interp, np.zeros(12), - interpolator.forces_interpolated, + force_interp, mpc.getDataHandler().getData().M, ) diff --git a/examples/talos_centroidal.py b/examples/talos_centroidal.py index 444a7278..12580c42 100644 --- a/examples/talos_centroidal.py +++ b/examples/talos_centroidal.py @@ -64,7 +64,6 @@ T_ss = 80 mpc_conf = dict( - ddpIteration=1, support_force=-model_handler.getMass() * gravity[2], TOL=1e-4, mu_init=1e-8, diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py index f8456fa4..88ee7227 100644 --- a/examples/talos_fulldynamics.py +++ b/examples/talos_fulldynamics.py @@ -92,7 +92,6 @@ T_ds = 20 totalSteps = 1 mpc_conf = dict( - ddpIteration=1, support_force=-model_handler.getMass() * gravity[2], TOL=1e-4, mu_init=1e-8, diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py index 32ffcd57..59ee378e 100644 --- a/examples/talos_kinodynamics.py +++ b/examples/talos_kinodynamics.py @@ -100,7 +100,6 @@ T_ds = 20 T_ss = 80 mpc_conf = dict( - ddpIteration=1, support_force=-model_handler.getMass() * gravity[2], TOL=1e-4, mu_init=1e-8, diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index 0e1a78c9..03a5e6de 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -37,7 +37,6 @@ namespace simple_mpc double mu_init = 1e-8; std::size_t max_iters = 1; std::size_t num_threads = 2; - int ddpIteration = 1; // Timings int T_fly = 80; diff --git a/tests/mpc.cpp b/tests/mpc.cpp index ff8af6ac..4143fc1e 100644 --- a/tests/mpc.cpp +++ b/tests/mpc.cpp @@ -25,7 +25,7 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics) fdproblem.createProblem(model_handler.getReferenceState(), T, 6, -settings.gravity[2], true); MPCSettings mpc_settings; - mpc_settings.ddpIteration = 1; + mpc_settings.max_iters = 1; mpc_settings.support_force = -model_handler.getMass() * settings.gravity[2]; @@ -108,7 +108,7 @@ BOOST_AUTO_TEST_CASE(mpc_kinodynamics) kinoproblem.createProblem(model_handler.getReferenceState(), T, 6, -settings.gravity[2], true); MPCSettings mpc_settings; - mpc_settings.ddpIteration = 1; + mpc_settings.max_iters = 1; mpc_settings.support_force = support_force; @@ -186,7 +186,7 @@ BOOST_AUTO_TEST_CASE(mpc_centroidal) centproblem.createProblem(data_handler.getCentroidalState(), T, 6, -settings.gravity[2], false); MPCSettings mpc_settings; - mpc_settings.ddpIteration = 1; + mpc_settings.max_iters = 1; mpc_settings.support_force = support_force;