|
| 1 | +import numpy as np |
| 2 | +import matplotlib.pyplot as plt |
| 3 | +import crocoddyl |
| 4 | +import pinocchio as pin |
| 5 | +import yaml |
| 6 | +import example_robot_data |
| 7 | + |
| 8 | + |
| 9 | +def get_next_state_delta_inertia(model, x, u, dt, link_idx, row_idx, col_idx, delta): |
| 10 | + """Integrate state to get next one, add small delta to link's inertia matrix.""" |
| 11 | + curr_inertia = model.differential.pinocchio.inertias[link_idx].inertia.copy() |
| 12 | + new_inertia = curr_inertia.copy() |
| 13 | + new_inertia[row_idx, col_idx] += delta |
| 14 | + new_inertia[col_idx, row_idx] += delta |
| 15 | + model.differential.pinocchio.inertias[link_idx].inertia = new_inertia |
| 16 | + curr_dt = model.dt |
| 17 | + model.dt = dt |
| 18 | + d = model.createData() |
| 19 | + model.calc(d, x, u) |
| 20 | + model.dt = curr_dt |
| 21 | + model.differential.pinocchio.inertias[link_idx].inertia = curr_inertia |
| 22 | + return d.xnext.copy() |
| 23 | + |
| 24 | + |
| 25 | +def get_next_state_delta_com(model, x, u, dt, link_idx, com_idx, delta): |
| 26 | + """Integrate state to get next one, add small delta to link's com pose.""" |
| 27 | + curr_com = model.differential.pinocchio.inertias[link_idx].lever |
| 28 | + new_com = curr_com.copy() |
| 29 | + new_com[com_idx] += delta |
| 30 | + model.differential.pinocchio.inertias[link_idx].lever = new_com |
| 31 | + curr_dt = model.dt |
| 32 | + model.dt = dt |
| 33 | + d = model.createData() |
| 34 | + model.calc(d, x, u) |
| 35 | + model.dt = curr_dt |
| 36 | + model.differential.pinocchio.inertias[link_idx].lever = curr_com |
| 37 | + return d.xnext.copy() |
| 38 | + |
| 39 | + |
| 40 | +def get_next_state_delta_mass(model, x, u, dt, link_idx, delta): |
| 41 | + """Integrate state to get next one, add small delta to link's mass.""" |
| 42 | + model.differential.pinocchio.inertias[link_idx].mass += delta |
| 43 | + curr_dt = model.dt |
| 44 | + model.dt = dt |
| 45 | + d = model.createData() |
| 46 | + model.calc(d, x, u) |
| 47 | + model.dt = curr_dt |
| 48 | + model.differential.pinocchio.inertias[link_idx].mass -= delta |
| 49 | + return d.xnext.copy() |
| 50 | + |
| 51 | + |
| 52 | +def get_reduced_panda_robot_model(robot): |
| 53 | + """Get pinocchio panda's reduced robot model.""" |
| 54 | + q0 = np.zeros((9)) |
| 55 | + locked_joints = [ |
| 56 | + robot.model.getJointId("panda_finger_joint1"), |
| 57 | + robot.model.getJointId("panda_finger_joint2"), |
| 58 | + ] |
| 59 | + return pin.buildReducedModel(robot.model, locked_joints, np.array(q0)) |
| 60 | + |
| 61 | + |
| 62 | +if __name__ == "__main__": |
| 63 | + # retrieve x0 and u0 from real data on the robot |
| 64 | + with open("state_and_control_expe_data.yaml", "r") as file: |
| 65 | + state_control_expe_data = yaml.safe_load(file) |
| 66 | + |
| 67 | + # get model and set parameters |
| 68 | + robot = example_robot_data.load("panda") |
| 69 | + rmodel = get_reduced_panda_robot_model(robot) |
| 70 | + dt = 0.01 # integration step |
| 71 | + delta_inertia = 0.01 |
| 72 | + delta_com = 0.01 |
| 73 | + delta_mass = 0.01 |
| 74 | + nq = rmodel.nq |
| 75 | + nv = rmodel.nv |
| 76 | + armature = np.array([0.1] * nq) |
| 77 | + point_idx = 2 # range from 1 to 5 |
| 78 | + |
| 79 | + # get starting state and control |
| 80 | + point_data = state_control_expe_data[f"point_{point_idx}"] |
| 81 | + x0 = np.array(point_data["x0"]) |
| 82 | + u0 = np.array(point_data["u0"]) |
| 83 | + |
| 84 | + # create crocoddyl model |
| 85 | + state = crocoddyl.StateMultibody(rmodel) |
| 86 | + actuation = crocoddyl.ActuationModelFull(state) |
| 87 | + cost_model = crocoddyl.CostModelSum(state) |
| 88 | + differential_model = crocoddyl.DifferentialActionModelFreeFwdDynamics( |
| 89 | + state, actuation, cost_model |
| 90 | + ) |
| 91 | + differential_model.armature = armature |
| 92 | + model = crocoddyl.IntegratedActionModelEuler(differential_model, dt) |
| 93 | + |
| 94 | + # compute sensibilities |
| 95 | + # for each link we have 10 values we're gonna test sensibility on, |
| 96 | + # 6 for inertia matrix, 3 for center of mass pose, 1 for mass |
| 97 | + model_sensibility = np.zeros(((nq + nv), nq * 10)) |
| 98 | + x1_base = get_next_state_delta_inertia(model, x0, u0, dt, 0, 0, 0, 0) |
| 99 | + for link_idx in range(1, nq + 1): |
| 100 | + idx_link_inerta = (link_idx - 1) * 10 |
| 101 | + for row_idx in range(3): |
| 102 | + for col_idx in range(0, row_idx + 1): |
| 103 | + x1 = get_next_state_delta_inertia( |
| 104 | + model, x0, u0, dt, link_idx, row_idx, col_idx, delta_inertia |
| 105 | + ) |
| 106 | + sensi_inertia = (x1 - x1_base) / delta_inertia |
| 107 | + model_sensibility[:, idx_link_inerta] = abs(sensi_inertia) |
| 108 | + idx_link_inerta += 1 |
| 109 | + |
| 110 | + for link_idx in range(1, nq + 1): |
| 111 | + for com_idx in range(3): |
| 112 | + x1 = get_next_state_delta_com( |
| 113 | + model, x0, u0, dt, link_idx, com_idx, delta_com |
| 114 | + ) |
| 115 | + sensi_com = (x1 - x1_base) / delta_com |
| 116 | + model_sensibility[:, 6 + (link_idx - 1) * 10 + com_idx] = abs(sensi_com) |
| 117 | + x1 = get_next_state_delta_mass(model, x0, u0, dt, link_idx, delta_mass) |
| 118 | + sensi_mass = (x1 - x1_base) / delta_mass |
| 119 | + model_sensibility[:, 9 + (link_idx - 1) * 10] = abs(sensi_mass) |
| 120 | + |
| 121 | + # plots |
| 122 | + u, s, vh = np.linalg.svd(model_sensibility) |
| 123 | + plt.plot(s, "+") |
| 124 | + plt.title("eigen values of sensibility matrix") |
| 125 | + plt.show() |
| 126 | + plt.imshow(np.abs(u)) |
| 127 | + |
| 128 | + plt.colorbar() |
| 129 | + plt.title("eigen vectors on the left of sensibility matrix") |
| 130 | + plt.show() |
| 131 | + plt.imshow(np.abs(vh)) |
| 132 | + plt.title("eigen vectors on the right of sensibility matrix") |
| 133 | + plt.colorbar() |
| 134 | + plt.show() |
0 commit comments