Update talos example

Author: earlaud

examples/talos_centroidal.py

@@ -2,7 +2,7 @@
 import pinocchio as pin
 import example_robot_data as erd
 from bullet_robot import BulletRobot
-from simple_mpc import RobotModelHandler, RobotDataHandler, CentroidalOCP, MPC
+from simple_mpc import RobotModelHandler, RobotDataHandler, CentroidalOCP, MPC, CentroidalID, CentroidalIDSettings, Interpolator
 from utils import loadTalos
 import time
 
@@ -15,8 +15,14 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
-model_handler.addQuadFoot("left_sole_link",  base_joint_name)
-model_handler.addQuadFoot("right_sole_link", base_joint_name)
+foot_points = np.array([
+    [0.1, 0.075, 0],
+    [-0.1, 0.075, 0],
+    [-0.1, -0.075, 0],
+    [0.1, -0.075, 0],
+])
+model_handler.addQuadFoot("left_sole_link",  base_joint_name, foot_points)
+model_handler.addQuadFoot("right_sole_link", base_joint_name, foot_points)
 model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 
@@ -44,8 +50,10 @@
 w_angular_mom = np.diag(np.array([0.1, 0.1, 1000]))
 w_angular_acc = 0.01 * np.eye(3)
 
+dt_mpc = 0.01
+
 problem_conf = dict(
-    timestep=0.01,
+    timestep=dt_mpc,
     w_u=w_u,
     w_com=w_com,
     w_linear_mom=w_linear_mom,
@@ -101,6 +109,25 @@
 
 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 """
+centroidal_ID_settings = CentroidalIDSettings()
+centroidal_ID_settings.kp_feet_tracking = 10.
+centroidal_ID_settings.kp_com = 10.
+centroidal_ID_settings.kp_posture = 1.
+centroidal_ID_settings.kp_contact = 10.
+centroidal_ID_settings.w_feet_tracking = 10
+centroidal_ID_settings.w_com = 10
+centroidal_ID_settings.w_posture = 0.1
+centroidal_ID_settings.w_contact_force = 1.
+centroidal_ID_settings.w_contact_motion = 1.
+
+centroidal_ID = CentroidalID(model_handler, dt_simu, centroidal_ID_settings)
+
 """ Initialize simulation"""
 device = BulletRobot(
     model_handler.getModel().names,
@@ -132,9 +159,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()
 
@@ -157,26 +184,30 @@
     )
 
     contact_states = mpc.ocp_handler.getContactState(0)
-    foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetFrameNames()]
-    foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetFrameNames()]
-    dH = mpc.getStateDerivative(0)[3:9]
+    feet_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetFrameNames()]
+    feet_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetFrameNames()]
+
+    pos_com = mpc.xs[0][:3]
+    pos_com_next = mpc.xs[1][:3]
 
-    for j in range(10):
-        time.sleep(0.001)
+    forces = mpc.us[0][: nk * force_size]
+    forces_next = mpc.us[0][: nk * force_size]
+
+    for sub_step in range(N_simu):
         q_meas, v_meas = device.measureState()
         x_measured = np.concatenate([q_meas, v_meas])
 
-        mpc.getDataHandler().updateInternalData(x_measured, True)
-        x_centroidal = mpc.getDataHandler().getCentroidalState()
-        state_diff = mpc.xs[0] - x_centroidal
-
-        forces = (
-            mpc.us[0][: nk * force_size]
-            - 1 * mpc.solver.results.controlFeedbacks()[0] @ state_diff
-        )
+        # Interpolate solution
+        pos_com_interp = interpolator.interpolateLinear(sub_step, N_simu, [pos_com, pos_com_next])
+        v_com = (pos_com_next - pos_com) / dt_simu
 
+        feet_ref_interp =  [pin.SE3.Interpolate(foot_ref, foot_ref_next, (1.0 * sub_step)/N_simu) for foot_ref, foot_ref_next in zip(feet_ref, feet_ref_next)]
+        feet_velocity =  [ pin.log6(foot_ref.actInv(foot_ref_next)) / dt_simu for foot_ref, foot_ref_next in zip(feet_ref, feet_ref_next)]
 
-        # TODO: use TSID to compute inverse dynamics and get tau_cmd
+        forces_interp = interpolator.interpolateLinear(sub_step, N_simu, [forces, forces_next])
+        forces_interp = forces_interp.reshape(2,6)
+        forces_interp = [forces_interp[i, :] for i in range(2)]
 
+        tau_cmd = centroidal_ID.setTarget(pos_com_interp, v_com, feet_ref_interp, feet_velocity, contact_states, forces_interp)
 
-        #device.execute(tau_cmd)
+        device.execute(tau_cmd)