Add getContactState to examples

Author: edantec

examples/go2_fulldynamics.py

@@ -232,7 +232,8 @@
         .dynamics_data.continuous_data.xdot[nv:]
     )
 
-    FL_f, FR_f, RL_f, RR_f, contact_states = extract_forces(mpc.getTrajOptProblem(), mpc.solver.workspace, 0)
+    FL_f, FR_f, RL_f, RR_f = extract_forces(mpc.getTrajOptProblem(), mpc.solver.workspace, 0)
+    contact_states = mpc.ocp_handler.getContactState(0)
     total_forces = np.concatenate((FL_f, FR_f, RL_f, RR_f))
     force_FL.append(FL_f)
     force_FR.append(FR_f)

examples/go2_kinodynamics.py

@@ -239,9 +239,7 @@
     a1[6:] = mpc.us[1][nk * force_size :]
     forces0 = mpc.us[0][: nk * force_size]
     forces1 = mpc.us[1][: nk * force_size]
-    contact_states = (
-        mpc.getTrajOptProblem().stages[0].dynamics.differential_dynamics.contact_states
-    )
+    contact_states = mpc.ocp_handler.getContactState(0)
 
     device.moveQuadrupedFeet(
         mpc.getReferencePose(0, "FL_foot").translation,

examples/talos_centroidal.py

@@ -201,11 +201,7 @@
         mpc.getReferencePose(0, "right_sole_link").translation,
     )
 
-    contact_states = (
-        mpc.getTrajOptProblem()
-        .stages[0]
-        .dynamics.differential_dynamics.contact_map.contact_states.tolist()
-    )
+    contact_states = mpc.ocp_handler.getContactState(0)
     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 = (

examples/talos_kinodynamics.py

@@ -224,9 +224,7 @@
     a1[6:] = mpc.us[1][nk * force_size :]
     forces0 = mpc.us[0][: nk * force_size]
     forces1 = mpc.us[1][: nk * force_size]
-    contact_states = (
-        mpc.getTrajOptProblem().stages[0].dynamics.differential_dynamics.contact_states
-    )
+    contact_states = mpc.ocp_handler.getContactState(0)
 
     device.moveMarkers(
         mpc.getReferencePose(0, "left_sole_link").translation,

examples/utils.py

@@ -71,21 +71,15 @@ def extract_forces(problem, workspace, id):
     force_FR = np.zeros(3)
     force_RL = np.zeros(3)
     force_RR = np.zeros(3)
-    contact_states = [False, False, False, False]
     in_contact = problem.stages[id].dynamics.differential_dynamics.constraint_models.__len__()
-
     for i in range(in_contact):
         if problem.stages[id].dynamics.differential_dynamics.constraint_models[i].name == 'FL_foot':
-            contact_states[0] = True
             force_FL = workspace.problem_data.stage_data[id].dynamics_data.continuous_data.constraint_datas[i].contact_force.linear
         elif problem.stages[id].dynamics.differential_dynamics.constraint_models[i].name == 'FR_foot':
-            contact_states[1] = True
             force_FR = workspace.problem_data.stage_data[id].dynamics_data.continuous_data.constraint_datas[i].contact_force.linear
         elif problem.stages[id].dynamics.differential_dynamics.constraint_models[i].name == 'RL_foot':
-            contact_states[2] = True
             force_RL = workspace.problem_data.stage_data[id].dynamics_data.continuous_data.constraint_datas[i].contact_force.linear
         elif problem.stages[id].dynamics.differential_dynamics.constraint_models[i].name == 'RR_foot':
-            contact_states[3] = True
             force_RR = workspace.problem_data.stage_data[id].dynamics_data.continuous_data.constraint_datas[i].contact_force.linear
 
-    return force_FL, force_FR, force_RL, force_RR, contact_states
+    return force_FL, force_FR, force_RL, force_RR