Merge pull request #44 from Simple-Robotics/topic/solve_typos

 Fix small typos

Author: edantec

bindings/expose-mpc.cpp

@@ -29,8 +29,6 @@ namespace simple_mpc
     {
       MPCSettings conf;
 
-      conf.ddpIteration = bp::extract<int>(settings["ddpIteration"]);
-
       conf.support_force = bp::extract<double>(settings["support_force"]);
 
       conf.TOL = bp::extract<double>(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;

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,

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,
         )
 

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,

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,

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,

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;

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;