Remove qp from python examples

Author: edantec

examples/go2_fulldynamics.py

@@ -5,7 +5,6 @@
     RobotDataHandler,
     FullDynamicsOCP,
     MPC,
-    IDSolver,
     Interpolator,
     FrictionCompensation
 )
@@ -48,12 +47,12 @@
 u0 = np.zeros(model_handler.getModel().nv - 6)
 
 w_basepos = [0, 0, 0, 0, 0, 0]
-w_legpos = [10, 10, 10]
+w_legpos = [1, 1, 1]
 
 w_basevel = [10, 10, 10, 10, 10, 10]
 w_legvel = [0.1, 0.1, 0.1]
 w_x = np.array(w_basepos + w_legpos * 4 + w_basevel + w_legvel * 4)
-w_cent_lin = np.array([0.0, 0.0, 0])
+w_cent_lin = np.array([0.04, 0.04, 0])
 w_cent_ang = np.array([0, 0, 0])
 w_forces_lin = np.array([0.0001, 0.0001, 0.0001])
 w_frame = np.diag(np.array([1000, 1000, 1000]))
@@ -140,24 +139,6 @@
 
 mpc.generateCycleHorizon(contact_phases)
 
-""" Initialize whole-body inverse dynamics QP"""
-contact_ids = model_handler.getFeetIds()
-id_conf = dict(
-    contact_ids=contact_ids,
-    x0=model_handler.getReferenceState(),
-    mu=0.8,
-    Lfoot=0.01,
-    Wfoot=0.01,
-    force_size=3,
-    kd=0,
-    w_force=0,
-    w_acc=0,
-    w_tau=1,
-    verbose=False,
-)
-
-qp = IDSolver(id_conf, model_handler.getModel())
-
 """ Friction """
 fcompensation = FrictionCompensation(model_handler.getModel(), True)
 """ Interpolation """
@@ -288,19 +269,8 @@
             x_measured, x_interp
         )
 
-        qp.solveQP(
-            mpc.getDataHandler().getData(),
-            contact_states,
-            x_measured[nq:],
-            acc_interp,
-            current_torque,
-            force_interp,
-            mpc.getDataHandler().getData().M,
-        )
-
-        qp_torque = qp.solved_torque.copy()
-        friction_torque = fcompensation.computeFriction(x_interp[nq + 6:], qp_torque)
-        device.execute(friction_torque)
+        friction_torque = fcompensation.computeFriction(x_interp[nq + 6:], current_torque)
+        device.execute(current_torque)
 
         u_multibody.append(copy.deepcopy(friction_torque))
         x_multibody.append(x_measured)

examples/go2_kinodynamics.py

@@ -1,6 +1,6 @@
 import numpy as np
 from bullet_robot import BulletRobot
-from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver, Interpolator
+from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, Interpolator
 import example_robot_data as erd
 import pinocchio as pin
 import time
@@ -134,24 +134,6 @@
 contact_phases += [contact_phase_lift_FR] * T_ss
 mpc.generateCycleHorizon(contact_phases)
 
-""" Initialize whole-body inverse dynamics QP"""
-contact_ids = model_handler.getFeetIds()
-id_conf = dict(
-    contact_ids=contact_ids,
-    x0=model_handler.getReferenceState(),
-    mu=0.8,
-    Lfoot=0.01,
-    Wfoot=0.01,
-    force_size=3,
-    kd=0,
-    w_force=100,
-    w_acc=1,
-    w_tau=0,
-    verbose=False,
-)
-
-qp = IDSolver(id_conf, model_handler.getModel())
-
 """ Interpolation """
 interpolator = Interpolator(model_handler.getModel())
 
@@ -266,19 +248,11 @@
 
         mpc.getDataHandler().updateInternalData(x_measured, True)
 
-        qp.solveQP(
-            mpc.getDataHandler().getData(),
-            contact_states,
-            x_measured[nq:],
-            acc_interp,
-            np.zeros(12),
-            force_interp,
-            mpc.getDataHandler().getData().M,
-        )
+        # TODO: use TSID to compute inverse dynamics and get tau_cmd
 
-        device.execute(qp.solved_torque)
-        u_multibody.append(copy.deepcopy(qp.solved_torque))
-        x_multibody.append(x_measured)
+        #device.execute(tau_cmd)
+        #u_multibody.append(copy.deepcopy(tau_cmd))
+        #x_multibody.append(x_measured)
 
 
 force_FL = np.array(force_FL)
@@ -297,6 +271,6 @@
 com_measured = np.array(com_measured)
 L_measured = np.array(L_measured)
 
-save_trajectory(x_multibody, u_multibody, com_measured, force_FL, force_FR, force_RL, force_RR, solve_time,
+""" save_trajectory(x_multibody, u_multibody, com_measured, force_FL, force_FR, force_RL, force_RR, solve_time,
                 FL_measured, FR_measured, RL_measured, RR_measured,
-                FL_references, FR_references, RL_references, RR_references, L_measured, "kinodynamics")
+                FL_references, FR_references, RL_references, RR_references, L_measured, "kinodynamics") """

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, IKIDSolver
+from simple_mpc import RobotModelHandler, RobotDataHandler, CentroidalOCP, MPC
 from utils import loadTalos
 import time
 
@@ -101,42 +101,6 @@
 
 mpc.generateCycleHorizon(contact_phases)
 
-""" Initialize inverse dynamics QP """
-g_basepos = [0, 0, 0, 10, 10, 10]
-g_legpos = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
-g_torsopos = [1, 1]
-g_armpos = [10, 10, 100, 10]
-
-g_q = np.array(g_basepos + g_legpos * 2 + g_torsopos + g_armpos * 2) * 10
-
-g_p = np.array([2000, 2000, 2000, 2000, 2000, 2000])
-g_b = np.array([10, 10, 10])
-
-Kp_gains = [g_q, g_p, g_b]
-Kd_gains = [2 * np.sqrt(g_q), 2 * np.sqrt(g_p), 2 * np.sqrt(g_b)]
-contact_ids = model_handler.getFeetIds()
-fixed_frame_ids = [model_handler.getBaseFrameId(), model_handler.getModel().getFrameId("torso_2_link")]
-ikid_conf = dict(
-    Kp_gains=Kp_gains,
-    Kd_gains=Kd_gains,
-    contact_ids=contact_ids,
-    fixed_frame_ids=fixed_frame_ids,
-    x0=model_handler.getReferenceState(),
-    dt=0.01,
-    mu=0.8,
-    Lfoot=0.1,
-    Wfoot=0.075,
-    force_size=6,
-    w_qref=500,
-    w_footpose=50000,
-    w_centroidal=10,
-    w_baserot=1000,
-    w_force=100,
-    verbose=False,
-)
-
-qp = IKIDSolver(ikid_conf, model_handler.getModel())
-
 """ Initialize simulation"""
 device = BulletRobot(
     model_handler.getModel().names,
@@ -196,9 +160,6 @@
     foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetNames()]
     foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetNames()]
     dH = mpc.getStateDerivative(0)[3:9]
-    qp.computeDifferences(
-        mpc.getDataHandler().getData(), x_measured, foot_ref, foot_ref_next
-    )
 
     for j in range(10):
         time.sleep(0.001)
@@ -215,14 +176,7 @@
         )
 
 
-        qp.solve_qp(
-            mpc.getDataHandler().getData(),
-            contact_states,
-            x_measured[nq:],
-            forces,
-            dH,
-            mpc.getDataHandler().getData().M,
-        )
+        # TODO: use TSID to compute inverse dynamics and get tau_cmd
 
 
-        device.execute(qp.solved_torque)
+        #device.execute(tau_cmd)

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, IDSolver
+from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC
 
 # RobotWrapper
 URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf"
@@ -136,24 +136,6 @@
 
 mpc.generateCycleHorizon(contact_phases)
 
-""" Initialize whole-body inverse dynamics QP"""
-contact_ids = model_handler.getFeetIds()
-id_conf = dict(
-    contact_ids=contact_ids,
-    x0=model_handler.getReferenceState(),
-    mu=0.8,
-    Lfoot=0.1,
-    Wfoot=0.075,
-    force_size=6,
-    kd=0,
-    w_force=100,
-    w_acc=1,
-    w_tau=0,
-    verbose=False,
-)
-
-qp = IDSolver(id_conf, model_handler.getModel())
-
 """ Initialize simulation"""
 device = BulletRobot(
     model_handler.getModel().names,
@@ -225,14 +207,6 @@
         a_interp = (10 - j) / 10 * a0 + j / 10 * a1
         f_interp = (10 - j) / 10 * forces0 + j / 10 * forces1
 
-        qp.solveQP(
-            mpc.getDataHandler().getData(),
-            contact_states,
-            x_measured[nq:],
-            a_interp,
-            np.zeros(nu),
-            f_interp,
-            mpc.getDataHandler().getData().M,
-        )
-
-        device.execute(qp.solved_torque)
+        # TODO: use TSID to compute inverse dynamics and get tau_cmd
+
+        #device.execute(tau_cmd)