Update kinodynamics examples

Author: earlaud

examples/go2_kinodynamics.py

@@ -34,7 +34,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
+dt_mpc = 0.01
 
 w_basepos = [0, 0, 100, 10, 10, 0]
 w_legpos = [1, 1, 1]
@@ -63,7 +63,7 @@
 w_centder = np.diag(np.concatenate((w_centder_lin, w_centder_ang)))
 
 problem_conf = dict(
-    timestep=dt,
+    timestep=dt_mpc,
     w_x=w_x,
     w_u=w_u,
     w_cent=w_cent,
@@ -97,7 +97,7 @@
     swing_apex=0.15,
     T_fly=T_ss,
     T_contact=T_ds,
-    timestep=dt,
+    timestep=dt_mpc,
 )
 
 mpc = MPC(mpc_conf, dynproblem)
@@ -134,6 +134,8 @@
 mpc.generateCycleHorizon(contact_phases)
 
 """ Interpolation """
+N_simu = 10 # Number of substep the simulation does between two MPC computation
+dt_simu = dt_mpc/N_simu
 interpolator = Interpolator(model_handler.getModel())
 
 """ Inverse Dynamics """
@@ -146,15 +148,15 @@
 kino_ID_settings.w_contact_force = 1.
 kino_ID_settings.w_contact_motion = 1.
 
-kino_ID = KinodynamicsID(model_handler, dt, kino_ID_settings)
+kino_ID = KinodynamicsID(model_handler, dt_simu, kino_ID_settings)
 
 
 """ Initialize simulation"""
 device = BulletRobot(
     model_handler.getModel().names,
     erd.getModelPath(URDF_SUBPATH),
     URDF_SUBPATH,
-    1e-3,
+    dt_simu,
     model_handler.getModel(),
     model_handler.getReferenceState()[:3],
 )
@@ -191,7 +193,6 @@
 solve_time = []
 L_measured = []
 
-N_simu = 10
 v = np.zeros(6)
 v[0] = 0.2
 mpc.velocity_base = v
@@ -249,12 +250,12 @@
     )
 
     for sub_step in range(N_simu):
-        t = (step * N_simu + sub_step) * dt
+        t = step * dt_mpc + sub_step * dt_simu
 
-        delay = sub_step / float(N_simu) * dt
-        xs_interp = interpolator.interpolateLinear(delay, dt, xss)
-        acc_interp = interpolator.interpolateLinear(delay, dt, ddqs)
-        force_interp = interpolator.interpolateLinear(delay, dt, forces).reshape((4,3))
+        delay = sub_step / float(N_simu) * dt_mpc
+        xs_interp = interpolator.interpolateLinear(delay, dt_mpc, xss)
+        acc_interp = interpolator.interpolateLinear(delay, dt_mpc, ddqs)
+        force_interp = interpolator.interpolateLinear(delay, dt_mpc, forces).reshape((4,3))
 
         q_interp = xs_interp[:mpc.getModelHandler().getModel().nq]
         v_interp = xs_interp[mpc.getModelHandler().getModel().nq:]

examples/talos_kinodynamics.py

@@ -4,7 +4,7 @@
 from bullet_robot import BulletRobot
 import time
 from utils import loadTalos
-from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC
+from simple_mpc import RobotModelHandler, RobotDataHandler, Interpolator, KinodynamicsOCP, MPC, KinodynamicsID, KinodynamicsIDSettings
 
 # RobotWrapper
 URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf"
@@ -73,8 +73,10 @@
 w_centder_ang = np.ones(3) * 0.1
 w_centder = np.diag(np.concatenate((w_centder_lin, w_centder_ang)))
 
+dt_mpc = 0.01
+
 problem_conf = dict(
-    timestep=0.01,
+    timestep=dt_mpc,
     w_x=w_x,
     w_u=w_u,
     w_cent=w_cent,
@@ -136,12 +138,30 @@
 
 mpc.generateCycleHorizon(contact_phases)
 
+""" Interpolation """
+N_simu = 10 # Number of substep the simulation does between two MPC computation
+dt_simu = dt_mpc/N_simu
+interpolator = Interpolator(model_handler.getModel())
+
+""" Inverse Dynamics """
+kino_ID_settings = KinodynamicsIDSettings()
+kino_ID_settings.kp_base = 7.
+kino_ID_settings.kp_posture = 10.
+kino_ID_settings.kp_contact = 10.
+kino_ID_settings.w_base = 100.
+kino_ID_settings.w_posture = 1.
+kino_ID_settings.w_contact_force = 1.
+kino_ID_settings.w_contact_motion = 1.
+
+kino_ID = KinodynamicsID(model_handler, dt_simu, kino_ID_settings)
+
 """ Initialize simulation"""
+N_simu = 10 # Number of substep the simulation does between two MPC computation
 device = BulletRobot(
     model_handler.getModel().names,
     erd.getModelPath(URDF_SUBPATH),
     URDF_SUBPATH,
-    1e-3,
+    dt_simu,
     model_handler.getModel(),
     model_handler.getReferenceState()[:3],
 )
@@ -163,9 +183,9 @@
 v = np.zeros(6)
 v[0] = 0.2
 mpc.velocity_base = v
-for t in range(600):
-    # print("Time " + str(t))
-    if t == 400:
+for step in range(600):
+    # print("Time " + str(step))
+    if step == 400:
         print("SWITCH TO STAND")
         mpc.switchToStand()
 
@@ -192,21 +212,34 @@
     forces1 = mpc.us[1][: nk * force_size]
     contact_states = mpc.ocp_handler.getContactState(0)
 
+    forces = [forces0, forces1]
+    ddqs = [a0, a1]
+    xss = [mpc.xs[0], mpc.xs[1]]
+    uss = [mpc.us[0], mpc.us[1]]
+
     device.moveMarkers(
         mpc.getReferencePose(0, "left_sole_link").translation,
         mpc.getReferencePose(0, "right_sole_link").translation,
     )
 
-    for j in range(10):
+    for sub_step in range(N_simu):
+        t = step * dt_mpc + sub_step * dt_simu
+
+        delay = sub_step / float(N_simu) * dt_mpc
+        xs_interp = interpolator.interpolateLinear(delay, dt_mpc, xss)
+        acc_interp = interpolator.interpolateLinear(delay, dt_mpc, ddqs)
+        force_interp = interpolator.interpolateLinear(delay, dt_mpc, forces).reshape((2,6))
+
+        q_interp = xs_interp[:mpc.getModelHandler().getModel().nq]
+        v_interp = xs_interp[mpc.getModelHandler().getModel().nq:]
+
         q_meas, v_meas = device.measureState()
         x_measured  = np.concatenate([q_meas, v_meas])
 
-        state_diff = model_handler.difference(x_measured, mpc.xs[0])
         mpc.getDataHandler().updateInternalData(x_measured, True)
 
-        a_interp = (10 - j) / 10 * a0 + j / 10 * a1
-        f_interp = (10 - j) / 10 * forces0 + j / 10 * forces1
-
-        # TODO: use TSID to compute inverse dynamics and get tau_cmd
+        # TODO: take 6D forces into account
+        kino_ID.setTarget(q_interp, v_interp, acc_interp, contact_states, force_interp)
+        tau_cmd = kino_ID.solve(t, q_meas, v_meas)
 
-        #device.execute(tau_cmd)
+        device.execute(tau_cmd)