| 142 | |
| 143 | |
| 144 | def plot_data_summary( |
| 145 | data_dir, |
| 146 | trajectories, |
| 147 | ee_pose, |
| 148 | in_contact, |
| 149 | forces, |
| 150 | frictional_coeff, |
| 151 | slip_percentage, |
| 152 | ): |
| 153 | _, axs = plt.subplots(2, 3, figsize=(15, 10)) |
| 154 | |
| 155 | filtered_ee_pose = ee_pose[in_contact, :] |
| 156 | shear_magnitude = np.linalg.norm(forces[:, :2], axis=-1) |
| 157 | shear_direction = np.arctan2(forces[:, 1], forces[:, 0]) |
| 158 | normal_force = forces[:, 2] |
| 159 | |
| 160 | # Plot: End effector pose as line segments on a 2D plot |
| 161 | axs[0, 0].plot( |
| 162 | ee_pose[:, 0], |
| 163 | ee_pose[:, 1], |
| 164 | linestyle="-", |
| 165 | color="gray", |
| 166 | alpha=0.2, |
| 167 | label=f"{len(trajectories)} trajectories", |
| 168 | ) |
| 169 | |
| 170 | for t in trajectories: |
| 171 | axs[0, 0].plot( |
| 172 | ee_pose[t, 0], |
| 173 | ee_pose[t, 1], |
| 174 | linestyle="-", |
| 175 | ) |
| 176 | axs[0, 0].set_xlabel("X") |
| 177 | axs[0, 0].set_ylabel("Y") |
| 178 | axs[0, 0].set_title("Filtered End Effector Pose") |
| 179 | axs[0, 0].legend() |
| 180 | axs[0, 0].grid(True) |
| 181 | |
| 182 | # Plot: friction cone |
| 183 | axs[0, 1].plot( |
| 184 | shear_magnitude, |
| 185 | normal_force, |
| 186 | marker="o", |
| 187 | markersize=2, |
| 188 | linestyle="", |
| 189 | label="Force Z vs Magnitude of Force X and Y", |
| 190 | color="purple", |
| 191 | ) |
| 192 | axs[0, 1].set_xlabel("Magnitude of Force X and Y") |
| 193 | axs[0, 1].set_ylabel("Force Z") |
| 194 | axs[0, 1].set_title("Force Z vs Magnitude of Force X and Y") |
| 195 | axs[0, 1].legend() |
| 196 | axs[0, 1].grid(True) |
| 197 | |
| 198 | # Plot: friction cone labels |
| 199 | f_limit = frictional_coeff * normal_force |
| 200 | slip_state = np.ones(len(normal_force)) |
| 201 | slip_state[shear_magnitude <= f_limit] = 0.0 |