Merge pull request #155 from TheoMF/topic/tmartinez/model_sensibility

add script to analyze panda model sensibility to inertia, center of mass, and mass of links

agimus_controller_examples/agimus_controller_examples/main/model_sensibility/evaluate_model_sensibility.py

@@ -0,0 +1,134 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import crocoddyl
+import pinocchio as pin
+import yaml
+import example_robot_data
+
+
+def get_next_state_delta_inertia(model, x, u, dt, link_idx, row_idx, col_idx, delta):
+    """Integrate state to get next one, add small delta to link's inertia matrix."""
+    curr_inertia = model.differential.pinocchio.inertias[link_idx].inertia.copy()
+    new_inertia = curr_inertia.copy()
+    new_inertia[row_idx, col_idx] += delta
+    new_inertia[col_idx, row_idx] += delta
+    model.differential.pinocchio.inertias[link_idx].inertia = new_inertia
+    curr_dt = model.dt
+    model.dt = dt
+    d = model.createData()
+    model.calc(d, x, u)
+    model.dt = curr_dt
+    model.differential.pinocchio.inertias[link_idx].inertia = curr_inertia
+    return d.xnext.copy()
+
+
+def get_next_state_delta_com(model, x, u, dt, link_idx, com_idx, delta):
+    """Integrate state to get next one, add small delta to link's com pose."""
+    curr_com = model.differential.pinocchio.inertias[link_idx].lever
+    new_com = curr_com.copy()
+    new_com[com_idx] += delta
+    model.differential.pinocchio.inertias[link_idx].lever = new_com
+    curr_dt = model.dt
+    model.dt = dt
+    d = model.createData()
+    model.calc(d, x, u)
+    model.dt = curr_dt
+    model.differential.pinocchio.inertias[link_idx].lever = curr_com
+    return d.xnext.copy()
+
+
+def get_next_state_delta_mass(model, x, u, dt, link_idx, delta):
+    """Integrate state to get next one, add small delta to link's mass."""
+    model.differential.pinocchio.inertias[link_idx].mass += delta
+    curr_dt = model.dt
+    model.dt = dt
+    d = model.createData()
+    model.calc(d, x, u)
+    model.dt = curr_dt
+    model.differential.pinocchio.inertias[link_idx].mass -= delta
+    return d.xnext.copy()
+
+
+def get_reduced_panda_robot_model(robot):
+    """Get pinocchio panda's reduced robot model."""
+    q0 = np.zeros((9))
+    locked_joints = [
+        robot.model.getJointId("panda_finger_joint1"),
+        robot.model.getJointId("panda_finger_joint2"),
+    ]
+    return pin.buildReducedModel(robot.model, locked_joints, np.array(q0))
+
+
+if __name__ == "__main__":
+    # retrieve x0 and u0 from real data on the robot
+    with open("state_and_control_expe_data.yaml", "r") as file:
+        state_control_expe_data = yaml.safe_load(file)
+
+    # get model and set parameters
+    robot = example_robot_data.load("panda")
+    rmodel = get_reduced_panda_robot_model(robot)
+    dt = 0.01  # integration step
+    delta_inertia = 0.01
+    delta_com = 0.01
+    delta_mass = 0.01
+    nq = rmodel.nq
+    nv = rmodel.nv
+    armature = np.array([0.1] * nq)
+    point_idx = 2  # range from 1 to 5
+
+    # get starting state and control
+    point_data = state_control_expe_data[f"point_{point_idx}"]
+    x0 = np.array(point_data["x0"])
+    u0 = np.array(point_data["u0"])
+
+    # create crocoddyl model
+    state = crocoddyl.StateMultibody(rmodel)
+    actuation = crocoddyl.ActuationModelFull(state)
+    cost_model = crocoddyl.CostModelSum(state)
+    differential_model = crocoddyl.DifferentialActionModelFreeFwdDynamics(
+        state, actuation, cost_model
+    )
+    differential_model.armature = armature
+    model = crocoddyl.IntegratedActionModelEuler(differential_model, dt)
+
+    # compute sensibilities
+    # for each link we have 10 values we're gonna test sensibility on,
+    # 6 for inertia matrix, 3 for center of mass pose, 1 for mass
+    model_sensibility = np.zeros(((nq + nv), nq * 10))
+    x1_base = get_next_state_delta_inertia(model, x0, u0, dt, 0, 0, 0, 0)
+    for link_idx in range(1, nq + 1):
+        idx_link_inerta = (link_idx - 1) * 10
+        for row_idx in range(3):
+            for col_idx in range(0, row_idx + 1):
+                x1 = get_next_state_delta_inertia(
+                    model, x0, u0, dt, link_idx, row_idx, col_idx, delta_inertia
+                )
+                sensi_inertia = (x1 - x1_base) / delta_inertia
+                model_sensibility[:, idx_link_inerta] = abs(sensi_inertia)
+                idx_link_inerta += 1
+
+    for link_idx in range(1, nq + 1):
+        for com_idx in range(3):
+            x1 = get_next_state_delta_com(
+                model, x0, u0, dt, link_idx, com_idx, delta_com
+            )
+            sensi_com = (x1 - x1_base) / delta_com
+            model_sensibility[:, 6 + (link_idx - 1) * 10 + com_idx] = abs(sensi_com)
+        x1 = get_next_state_delta_mass(model, x0, u0, dt, link_idx, delta_mass)
+        sensi_mass = (x1 - x1_base) / delta_mass
+        model_sensibility[:, 9 + (link_idx - 1) * 10] = abs(sensi_mass)
+
+    # plots
+    u, s, vh = np.linalg.svd(model_sensibility)
+    plt.plot(s, "+")
+    plt.title("eigen values of sensibility matrix")
+    plt.show()
+    plt.imshow(np.abs(u))
+
+    plt.colorbar()
+    plt.title("eigen vectors on the left of sensibility matrix")
+    plt.show()
+    plt.imshow(np.abs(vh))
+    plt.title("eigen vectors on the right of sensibility matrix")
+    plt.colorbar()
+    plt.show()

agimus_controller_examples/agimus_controller_examples/main/model_sensibility/state_and_control_expe_data.yaml

@@ -0,0 +1,144 @@
+point_1:
+  x0:
+    [
+      -0.01234266,
+      0.04483726,
+      -0.01245294,
+      -1.65970849,
+      0.0053825,
+      1.67475624,
+      0.73647535,
+      -0.03952127,
+      0.09115266,
+      -0.03780158,
+      -0.19657774,
+      0.0110891,
+      0.30519501,
+      -0.12058271,
+    ]
+  u0:
+    [
+      -1.50062189,
+      -32.96001341,
+      -1.25585792,
+      23.93996965,
+      0.94498913,
+      3.57828055,
+      -0.51308917,
+    ]
+
+point_2:
+  x0:
+    [
+      -0.17150362,
+      0.10615764,
+      -0.16229535,
+      -1.95177738,
+      0.03691598,
+      2.25664708,
+      0.52969295,
+      -0.08740141,
+      0.23581081,
+      -0.09771547,
+      -0.26409425,
+      0.00576253,
+      0.1715259,
+      -0.09583149,
+    ]
+  u0:
+    [
+      0.51098499,
+      -32.34277148,
+      0.5212811,
+      22.93049917,
+      1.25434371,
+      3.9713109,
+      -0.78057266,
+    ]
+
+point_3:
+  x0:
+    [
+      0.13049612,
+      0.16498611,
+      0.11533246,
+      -2.08334287,
+      0.05353042,
+      2.09693776,
+      0.70178061,
+      0.34143127,
+      -0.11871808,
+      0.31681011,
+      0.03695333,
+      0.00816043,
+      0.0315556,
+      0.23940798,
+    ]
+  u0:
+    [
+      1.39650455,
+      -36.29590193,
+      2.34490956,
+      23.19352057,
+      0.99217375,
+      3.18790063,
+      0.97390622,
+    ]
+
+point_4:
+  x0:
+    [
+      0.97031325,
+      -0.02127223,
+      0.9304275,
+      -2.16655581,
+      0.13665267,
+      2.19277893,
+      1.37555517,
+      0.45229686,
+      -0.08920446,
+      0.43320022,
+      -0.06813646,
+      0.04852861,
+      0.05199259,
+      0.38024724,
+    ]
+  u0:
+    [
+      1.2735004,
+      -18.00693956,
+      0.93743486,
+      21.89665671,
+      1.0686884,
+      3.29290768,
+      0.81871872,
+    ]
+
+point_5:
+  x0:
+    [
+      1.0425574,
+      -0.15902984,
+      0.53682131,
+      -1.86540792,
+      0.17898847,
+      1.76978178,
+      1.32322549,
+      -0.54787866,
+      0.0933039,
+      -0.27467942,
+      0.12831353,
+      -0.08944301,
+      -0.08760853,
+      -0.29462839,
+    ]
+  u0:
+    [
+      -1.89533092,
+      -20.87217118,
+      -4.17067257,
+      25.78918204,
+      -0.29369269,
+      1.60877,
+      -0.8094384,
+    ]