diff --git a/aerial_robot_model/script/sysid_inertia/inertial_params.yaml b/aerial_robot_model/script/sysid_inertia/inertial_params.yaml new file mode 100644 index 000000000..135b9572c --- /dev/null +++ b/aerial_robot_model/script/sysid_inertia/inertial_params.yaml @@ -0,0 +1,30 @@ +mini_quadrotor: + # with lidar and battery + Simulation: + mass: 1.05642 # kg + g: 9.80665 # m/s^2 + Ixx: 0.00387334 # kg*m^2 + Iyy: 0.00407316 # kg*m^2 + Izz: 0.00529428 # kg*m^2 + Ixy: 1.50847e-08 # kg*m^2 + Experiment: + mass: 1.086 + g: 9.798 # m/s^2 Tokyo + Ixx: 0.0086 # kg*m^2 + Iyy: 0.0087 # kg*m^2 + Izz: + Ixy: 0 # kg*m^2 difficult to measure through experiment + +beetle_art: + Simulation: # with outer shell + mass: 2.99194 # kg + g: 9.80665 # m/s^2 + Ixx: 0.0621273 # kg*m^2 + Iyy: 0.0927884 # kg*m^2 + Izz: 0.130061 # kg*m^2 + Experiment: + mass: 2.773 # kg + g: 9.798 # m/s^2 Tokyo + Ixx: 0.04170 # kg*m^2 0.04242, 0.04098 + Iyy: 0.03945 # kg*m^2 0.03866, 0.04025 + Izz: 0.07068 # kg*m^2 0.07134, 0.07001 diff --git a/aerial_robot_model/script/sysid_inertia/process_data_and_identify.py b/aerial_robot_model/script/sysid_inertia/process_data_and_identify.py new file mode 100644 index 000000000..523759fb0 --- /dev/null +++ b/aerial_robot_model/script/sysid_inertia/process_data_and_identify.py @@ -0,0 +1,86 @@ +''' +created by Jinjie LI, 2023/02/06 +''' + +import pandas as pd +import numpy as np +import matplotlib.pyplot as plt +from scipy.fft import fft +import argparse + + +def read_data(file_name): + # Read the data, assuming space-separated values and the first row as header + data = pd.read_csv(file_name, delim_whitespace=True, header=None) + # Assign column names based on the number of columns + col_names = ['time'] + [f'col_{i}' for i in range(1, data.shape[1])] + data.columns = col_names + return data + + +def process_data(data, L, D, m, g): + # Determine the number of subplots needed: 2 plots per data column, excluding 'time' + num_cols = data.shape[1] - 1 + fig, axs = plt.subplots(num_cols, 2, figsize=(25, num_cols * 5)) + + # Adjust layout for better readability + plt.tight_layout(pad=3.0) + + # Drop the time column for visualization + data_for_vis = data.drop(columns=['time']) + + if num_cols == 6: + name_list = ['x', 'y', 'z', 'roll', 'pitch', 'yaw'] + else: + name_list = ['x', 'y', 'z', 'qw', 'qx', 'qy', 'qz'] + + # convert "time" data to PeriodIndex + data['time'] = pd.to_datetime(data['time'], unit='s') + + # Interpolation to 100hz + data = data.set_index('time').resample('10ms').mean().interpolate().reset_index() + average_interval = 0.01 + + for i, col in enumerate(data_for_vis.columns): + # XY plot + axs[i, 0].plot(data['time'].to_numpy(), data[col].to_numpy()) + axs[i, 0].set_title(f'XY Plot of {name_list[i]}') + axs[i, 0].set_xlabel('Time (s)') + axs[i, 0].set_ylabel(name_list[i]) + + # FFT transformation + yf = fft(data[col].to_numpy() - np.mean(data[col].to_numpy())) # Remove the DC component + + xf = np.linspace(0.0, 1.0 / (2.0 * average_interval), len(data[col]) // 2) + axs[i, 1].plot(xf, 2.0 / len(data[col]) * np.abs(yf[:len(data[col]) // 2])) + axs[i, 1].set_title(f'FFT of {name_list[i]}') + axs[i, 1].set_xlabel('Frequency') + axs[i, 1].set_ylabel('Amplitude') + + # Find and plot the peak + peak_freq = xf[np.argmax(np.abs(yf[:len(data[col]) // 2]))] + peak_amp = 2.0 / len(data[col]) * np.max(np.abs(yf[:len(data[col]) // 2])) + print(f'- Peak frequency for {name_list[i]}: {peak_freq:.8f} Hz; Peak amplitude: {peak_amp:.8f}') + axs[i, 1].plot(peak_freq, peak_amp, 'r*', markersize=10) + axs[i, 1].text(peak_freq, peak_amp, f'Peak: {peak_freq:.8f} Hz, {peak_amp:.8f}', ha='left', va='bottom') + + # Calculate the inertial + I = m * g * (D ** 2) / (16 * (np.pi ** 2) * L * peak_freq ** 2) + print(f"Calculate inertial parameters with {name_list[i]}: {I} kg*m^2 \n") + + plt.show() + print("+++++ Please choose a proper inertial parameter from the above results. +++++") + print("+++++ Note that the Peak amplitude should be large enough. +++++") + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Process and visualize data from a text file.') + parser.add_argument('file_name', help='Name of the text file containing the data') + parser.add_argument('L', help='Length of the pendulum', default=1.920) + parser.add_argument('D', help='Diameter of the pendulum', default=0.495) + parser.add_argument('m', help='Mass of the object', default=2.773) + parser.add_argument('--g', help='Acceleration due to gravity', default=9.798) + args = parser.parse_args() + + data = read_data(args.file_name) + process_data(data, float(args.L), float(args.D), float(args.m), float(args.g)) diff --git a/aerial_robot_model/script/sysid_inertia/record_data.py b/aerial_robot_model/script/sysid_inertia/record_data.py new file mode 100644 index 000000000..02ef2649c --- /dev/null +++ b/aerial_robot_model/script/sysid_inertia/record_data.py @@ -0,0 +1,68 @@ +''' +created by Jinjie LI, 2023/02/06 + +subscribe to the /mocap/pose topic and record the data in a txt file under ~/.ros/ folder +''' +import rospy +import argparse +import os +from geometry_msgs.msg import PoseStamped +import tf.transformations +from datetime import datetime + + +class MocapPoseSubscriber: + def __init__(self, file_name, folder_path, convert_to_euler): + # Initialize the ROS node + rospy.init_node('mocap_pose_subscriber', anonymous=True) + + # Store the conversion flag and folder path + self.convert_to_euler = convert_to_euler + self.folder_path = os.path.expanduser(folder_path) + + # Ensure the directory exists + if not os.path.exists(self.folder_path): + os.makedirs(self.folder_path) + + # Prepare the file path with a timestamp + timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + self.file_path = os.path.join(self.folder_path, f"{file_name}_{timestamp}.txt") + + # Subscribe to the /mocap/pose topic + self.subscriber = rospy.Subscriber('/mocap/pose', PoseStamped, self.callback) + + def callback(self, data): + # Extract position + position = data.pose.position + x, y, z = position.x, position.y, position.z + + # Extract orientation + orientation = data.pose.orientation + qw, qx, qy, qz = orientation.w, orientation.x, orientation.y, orientation.z + + if self.convert_to_euler: + euler = tf.transformations.euler_from_quaternion((qx, qy, qz, qw)) + roll, pitch, yaw = euler + # change stamp to time in seconds + formatted_data = f"{data.header.stamp.to_sec()} {x} {y} {z} {roll} {pitch} {yaw}\n" + else: + formatted_data = f"{data.header.stamp.to_sec()} {x} {y} {z} {qw} {qx} {qy} {qz}\n" + + # Open the file and append the formatted data + with open(self.file_path, 'a') as file: + file.write(formatted_data) + rospy.loginfo("Pose data recorded.") + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Subscribe to /mocap/pose and record data.") + parser.add_argument("--file_name", help="Name of the recording file", default="mocap_pose_data") + parser.add_argument("--is_euler", help="Convert quaternion to Euler angles", action="store_true") + parser.add_argument("--folder", help="Folder to save the recording files", default="~/.ros/") + args = parser.parse_args() + + try: + recorder = MocapPoseSubscriber(file_name=args.file_name, folder_path=args.folder, convert_to_euler=args.is_euler) + rospy.spin() + except rospy.ROSInterruptException: + pass