import numpy as np from scipy.interpolate import CubicSpline from scipy.spatial.transform import Rotation as R, Slerp from scipy.interpolate import BSpline from scipy.interpolate import make_interp_spline from scipy.signal import savgol_filter from scipy.interpolate import interp1d import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import math import copy def linear_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1): """ 进行位置和/或姿态的线性插值,支持多个点 :param positions: 位置序列,单位为米 (Nx2 或 Nx3 numpy array, 可选) :param quaternions: 姿态序列,四元数 (Nx4 numpy array, 可选) :param time_points: 时间点序列 (N个浮点数) :param time_step: 离散时间步长,单位为秒 (float) :return: 插值序列,包含位置序列、姿态序列或两者兼有 """ if positions is None and quaternions is None: raise ValueError("至少需要输入位置或姿态的序列") if time_points is None or len(time_points) < 2: raise ValueError("需要提供至少两个时间点") # 由于浮点数精度问题,time_step要乘0.9 times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) if positions is not None: all_positions = np.zeros((len(times), positions.shape[1])) segment_durations = np.diff(time_points) segment_counts = np.floor(segment_durations / time_step).astype(int) current_idx = 0 for i in range(len(segment_counts)): segment_times = np.linspace(0, segment_durations[i], segment_counts[i] + 1) if i > 0: segment_times = segment_times[1:] start_pos = positions[i] end_pos = positions[i + 1] segment_positions = start_pos + np.outer(segment_times, (end_pos - start_pos) / segment_durations[i]) all_positions[current_idx:current_idx + len(segment_positions)] = segment_positions current_idx += len(segment_positions) # 确保最后一个位置被处理 if current_idx < len(times): all_positions[current_idx:] = positions[-1] if quaternions is not None: slerp = Slerp(time_points, R.from_quat(quaternions)) all_quaternions = slerp(times).as_quat() all_quaternions = np.array([quat / np.linalg.norm(quat) for quat in all_quaternions]) if positions is not None and quaternions is not None: return all_positions, all_quaternions elif positions is not None: return all_positions elif quaternions is not None: return all_quaternions def spline_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1): """ 进行位置和/或姿态的样条插值,支持多个点 :param positions: 位置序列,单位为米 (Nx2 或 Nx3 numpy array, 可选) :param quaternions: 姿态序列,四元数 (Nx4 numpy array, 可选) :param time_points: 时间点序列 (N个浮点数) :param time_step: 离散时间步长,单位为秒 (float) :return: 插值序列,包含位置序列、姿态序列或两者兼有 """ if positions is None and quaternions is None: raise ValueError("至少需要输入位置或姿态的序列") if time_points is None or len(time_points) < 2: raise ValueError("需要提供至少两个时间点") all_positions = [] all_quaternions = [] times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) if positions is not None: cs = CubicSpline(time_points, positions, axis=0) all_positions = cs(times) if quaternions is not None: slerp = Slerp(time_points, R.from_quat(quaternions)) all_quaternions = slerp(times).as_quat() if positions is not None and quaternions is not None: return np.array(all_positions), np.array(all_quaternions) elif positions is not None: return np.array(all_positions) elif quaternions is not None: return np.array(all_quaternions) def generate_circle_trajectory(center, omega=0.4, radius=0.02, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None): """ Generate a 3D trajectory of a circular motion from the center to the specified radius. Parameters: center (list): The center of the circle [x, y, z , r , p , y]. omega (float): The angular velocity. radius (float): The radius of the circle. reverse (bool): If True, rotates counterclockwise. If False, rotates clockwise. time_points (list or np.ndarray): List or array of time points. time_step (float): Time step for generating the trajectory. start_transition_duration (float): Duration for the transition at the start. end_transition_duration (float): Duration for the transition at the end. Returns: np.ndarray: Array of positions over time. """ # print(time_points) if time_points is None or len(time_points) < 2: raise ValueError("需要提供至少两个时间点") if start_transition_duration is None: start_transition_duration = 2 if end_transition_duration is None: end_transition_duration = 2 t_points = time_points.copy() t_points[-1] = time_points[-1] + end_transition_duration + start_transition_duration t_points[-1] = round(t_points[-1] / time_step) * time_step times = np.arange(t_points[0], t_points[-1] + time_step * 0.9, time_step) if reverse: angles = -omega * times else: angles = omega * times radii = np.ones_like(times) * radius start_transition = times < start_transition_duration end_transition = times > (times[-1] - end_transition_duration) radii[start_transition] = radius * (1 - np.cos(np.pi * times[start_transition] / start_transition_duration)) / 2 radii[end_transition] = radius * (1 + np.cos(np.pi * (times[end_transition] - (times[-1] - end_transition_duration)) / end_transition_duration)) / 2 x_positions = center[0][0] + radii * np.cos(angles) y_positions = center[0][1] + radii * np.sin(angles) z_positions = np.full_like(x_positions, center[0][2]) # Z position remains constant positions = np.column_stack((x_positions, y_positions, z_positions)) return positions def bezier_curve(control_points, n_points=100): # 确保输入为二维 float64 数组 control_points = np.array(control_points, dtype=np.float64) if control_points.ndim != 2: raise ValueError("control_points must be a 2D array, got shape {}".format(control_points.shape)) n = len(control_points) - 1 t = np.linspace(0.0, 1.0, n_points) curve = np.zeros((n_points, control_points.shape[1]), dtype=np.float64) def bernstein(i, n, t): from scipy.special import comb return comb(n, i) * ((1 - t) ** (n - i)) * (t ** i) for i in range(n + 1): # 明确每个控制点是 float64 向量 point = np.asarray(control_points[i], dtype=np.float64) if point.shape != (control_points.shape[1],): raise ValueError(f"Invalid point shape: {point.shape}") b = bernstein(i, n, t) curve += np.outer(b, point) return curve def cloud_point_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1): """ 进行输入的点云位置和/或姿态的曲线插值,支持多个点,采用B样条插值法 :param positions: 位置序列,单位为米 (Nx2 或 Nx3 numpy array, 可选) :param quaternions: 姿态序列,四元数 (Nx4 numpy array, 可选) :param time_points: 时间点序列 (N个浮点数) :param time_step: 离散时间步长,单位为秒 (float) :return: 插值序列,包含位置序列、姿态序列或两者兼有 """ if positions is None and quaternions is None: raise ValueError("至少需要输入位置或姿态的序列") if time_points is None or len(time_points) < 2: raise ValueError("需要提供至少两个时间点") temp_positions = np.array(positions) temp_quaternions = np.zeros((len(quaternions), 4)) # 将RPY角度转换为四元数 for i in range(len(quaternions)): temp_quaternions[i] = R.from_euler('xyz', quaternions[i]).as_quat() time_points = np.linspace(time_points[0], time_points[-1], len(temp_positions)) times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) temp_positions_smoothed = copy.deepcopy(temp_positions) # temp_positions = np.array(positions, dtype=np.float64) # temp_positions_smoothed = bezier_curve(temp_positions, n_points=len(temp_positions)) temp_quaternions_smoothed = copy.deepcopy(temp_quaternions) all_positions, all_quaternions = [], [] # 进行B样条插值 if temp_positions_smoothed is not None: BS = make_interp_spline(time_points, temp_positions_smoothed, k=3, bc_type='natural') all_positions = BS(times) # 进行四元数LERP插值 if temp_quaternions_smoothed is not None: temp_quaternions_smoothed = np.array(temp_quaternions_smoothed) all_quaternions = [] for t in times: # 在时间点序列中找到最近的两个时间点,用于线性插值 idx = np.searchsorted(time_points, t) - 1 idx = max(0, min(idx, len(time_points) - 2)) t1, t2 = time_points[idx], time_points[idx + 1] q1, q2 = temp_quaternions_smoothed[idx], temp_quaternions_smoothed[idx + 1] # 计算插值因子 alpha alpha = (t - t1) / (t2 - t1) # 进行四元数LERP插值 q_interp = (1 - alpha) * q1 + alpha * q2 q_interp /= np.linalg.norm(q_interp) # 归一化四元数 all_quaternions.append(q_interp) euler_angles = R.from_quat(all_quaternions).as_euler('xyz') # 检查数据点数量 if len(euler_angles) >= 5: # 数据点数量大于等于3时使用 Savitzky-Golay 滤波器 euler_angles_smoothed = savgol_filter(euler_angles, window_length=5, polyorder=3, axis=0) else: # 数据点小于5时直接使用原始数据 euler_angles_smoothed = euler_angles # 将平滑后的欧拉角转换回四元数 all_quaternions = R.from_euler('xyz', euler_angles_smoothed).as_quat() if temp_positions is not None and temp_quaternions is not None: return np.array(all_positions), np.array(all_quaternions) elif temp_positions is not None: return np.array(all_positions) elif temp_quaternions is not None: return np.array(all_quaternions) def oscillation_wrench_interpolate(wrench=None, time_points=None, time_step=0.1): if wrench is None: raise ValueError("至少需要输入力矩的序列") if time_points is None or len(time_points) < 2: raise ValueError("需要提供至少两个时间点") # 由于浮点数精度问题,time_step要乘0.9 times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) if wrench is not None: all_wrenchs = np.zeros((len(times), wrench.shape[1])) current_idx = 0 up_wrench = np.array([0, 0, 0, 0, 0, 0]) down_wrench = np.array([0, 0, 0, 0, 0, 0]) up_wrench[2] = wrench[0][2] + 15 if wrench[0][2] + 15 < 0 else 0 down_wrench[2] = wrench[0][2] - 15 if wrench[0][2] - 15 > -50 else -50 for i in range(len(times)): # 计算 phase phase = np.sin(20 * np.pi * i / len(times)) # 使用 i 计算 phase = 1 if phase >= 0 else -1 # 计算 amplitude # amplitude = np.array([0, 0, -50, 0, 0, 0]) if phase < 0 else np.array([0, 0, 0, 0, 0, 0]) amplitude = down_wrench if phase < 0 else up_wrench # 计算 segment_wrenchs segment_wrenchs = wrench + phase * (amplitude - wrench) # 将 segment_wrenchs 填充到 all_wrenchs all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs current_idx += len(segment_wrenchs) # 确保最后一个位置被处理 if current_idx < len(times): all_wrenchs[current_idx:] = wrench all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0])) if wrench is not None: return np.array(all_wrenchs) def linear_wrench_interpolate(wrench=None, time_points=None, time_step=0.1): if wrench is None: raise ValueError("至少需要输入力矩的序列") if time_points is None or len(time_points) < 2: raise ValueError("需要提供至少两个时间点") # 由于浮点数精度问题,time_step要乘0.9 times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) if wrench is not None: all_wrenchs = np.zeros((len(times), wrench.shape[1])) current_idx = 0 start_wrench = np.array([0, 0, 0, 0, 0, 0]) end_wrench = wrench for i in range(len(times)): # 计算 phase phase = i / len(times) # 使用 i 计算 # 计算 segment_wrenchs segment_wrenchs = start_wrench + phase * (end_wrench - start_wrench) # 将 segment_wrenchs 填充到 all_wrenchs all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs current_idx += len(segment_wrenchs) # 确保最后一个位置被处理 if current_idx < len(times): all_wrenchs[current_idx:] = wrench all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0])) if wrench is not None: return np.array(all_wrenchs) def resample_curve_strict(points, num_resampled_points): """ 修正的曲线重新采样函数,移除重复点并确保累积弧长严格递增。支持三维曲线。 :param points: Nx3 numpy array,三维轨迹点 :param num_resampled_points: 重新采样的点数 :return: Nx3 numpy array,重采样后的三维轨迹 """ points = np.array(points, dtype=np.float64) # 计算累积弧长 distances = np.linalg.norm(np.diff(points, axis=0), axis=1) cumulative_length = np.insert(np.cumsum(distances), 0, 0) # 移除重复点(累积弧长未变化的) unique_indices = np.where(np.diff(cumulative_length, prepend=-np.inf) > 0)[0] cumulative_length = cumulative_length[unique_indices] points = points[unique_indices] # 生成等间隔弧长采样点 target_lengths = np.linspace(0, cumulative_length[-1], num_resampled_points) # 对每个维度做线性插值 interp_funcs = [interp1d(cumulative_length, points[:, i], kind='linear', fill_value="extrapolate") for i in range(3)] new_coords = [interp(target_lengths) for interp in interp_funcs] return np.column_stack(new_coords) def circle_trajectory(center, omega=8.0, radius=0.04, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None): if time_points is None or len(time_points) < 2: raise ValueError("需要提供至少两个时间点") if start_transition_duration is None: start_transition_duration = 2 if end_transition_duration is None: end_transition_duration = 2 t_points = time_points.copy() # t_points[-1] = time_points[-1] + end_transition_duration + start_transition_duration t_points[-1] = time_points[-1] t_points[-1] = round(t_points[-1] / time_step) * time_step times = np.arange(t_points[0], t_points[-1] + time_step * 0.9, time_step) if reverse: angles = -omega * times else: angles = omega * times radii = np.ones_like(times) * radius start_transition = times < start_transition_duration end_transition = times > (times[-1] - end_transition_duration) radii[start_transition] = radius * (1 - np.cos(np.pi * times[start_transition] / start_transition_duration)) / 2 radii[end_transition] = radius * (1 + np.cos(np.pi * (times[end_transition] - (times[-1] - end_transition_duration)) / end_transition_duration)) / 2 x_positions = radii * np.cos(angles) y_positions = radii * np.sin(angles) z_positions = np.full_like(x_positions, 0) # Z position remains constant positions = np.column_stack((x_positions, y_positions, z_positions)) positions = resample_curve_strict(positions, len(positions)) # print("circle_positions:") tempToolRPY = R.from_euler('xyz', center[0][3:], degrees=False).as_matrix() for i in range(len(positions)): positions[i] = tempToolRPY @ positions[i] + center[0][:3] # 将RPY角度转换为四元数 # print(positions[i]) return positions # def generate_circle_cloud_points(center, start_point, radius, delta_theta = 10*np.pi/180, num_turns = 3): # """ # center: 圆心坐标,形如 (x_c, y_c) # start_point: 起始点坐标,形如 (x_0, y_0) # radius: 圆的半径 # delta_theta: 每次插补的角度增量 # num_turns: 绕圈的次数 # """ # # 确定总共需要生成的插补点数 # num_points = int((2 * np.pi * num_turns) / delta_theta) # # 圆心 # x_c, y_c = center # # 计算起始点的初始角度 # x_0, y_0 = start_point # theta_0 = np.arctan2(y_0 - y_c, x_0 - x_c) # # 初始化存储插补点的列表 # circle_points = [] # # 生成插补点 # for i in range(num_points): # # 当前角度 # theta_i = theta_0 + i * delta_theta # # 计算插补点的坐标 # x_i = x_c + radius * np.cos(theta_i) # y_i = y_c + radius * np.sin(theta_i) # # 将点添加到列表中 # circle_points.append((np.round(x_i).astype(int), np.round(y_i).astype(int))) # circle_points.append((np.round(x_0).astype(int), np.round(y_0).astype(int))) # return circle_points def calculate_target_Euler(point): temp_euler = np.zeros((len(point), 3), dtype=np.float64) for i in range(len(point)): if(point[i][5]<0): temp_euler[i][0] = -math.asin(-point[i][4]) temp_euler[i][1] = math.atan2(-point[i][3],-point[i][5]) else: temp_euler[i][0] = -math.asin(point[i][4]) temp_euler[i][1] = math.atan2(point[i][3],point[i][5]) temp_euler[i][2] = 0.0 return temp_euler if __name__ == "__main__": # import pathlib # import sys # sys.path.append(str(pathlib.Path.cwd())) # from MassageControl.tools.draw_tools import plot_trajectory import numpy as np import pandas as pd import matplotlib.pyplot as plt # 示例使用 center = (80, 90) # 圆心 start_point = (70, 0) # 起点 radius = np.linalg.norm(np.array(start_point) - np.array(center)) # 半径 delta_theta = np.pi / 9 # 每次插补的角度增量 num_turns = 2 # 绕2圈 # 生成圆的插补点 circle_points_with_start = generate_circle_cloud_points(center, start_point, radius, delta_theta, num_turns) # 将生成的插补点转换为可视化的DataFrame circle_points_with_start_df = pd.DataFrame({ "x": [point[0] for point in circle_points_with_start], "y": [point[1] for point in circle_points_with_start] }) # 打印生成的插补点 print(circle_points_with_start_df) # 绘制插补点的图像 x_vals = [point[0] for point in circle_points_with_start] y_vals = [point[1] for point in circle_points_with_start] plt.figure(figsize=(6, 6)) plt.scatter(x_vals, y_vals, marker='o', linestyle='-', color='b') plt.scatter([x_vals[0]], [y_vals[0]], color='r', label='Start Point') # 标记起点 plt.gca().set_aspect('equal', adjustable='box') plt.title('Circle Interpolation Points with Start Point') plt.xlabel('X') plt.ylabel('Y') plt.legend() plt.grid(True) # 保存并显示图片 plt.show() # start1pos = np.array([0.0392,-0.408,0.752]) # end1pos = np.array([0.0392,0.2,0.752]) # start2pos = np.array([0.2281194148213992,-0.1320499159555817,0.7499999952316284]) # end2pos = np.array([0.14268880718774088,-0.13746791895961052,0.7350000095367432]) # start3pos = np.array([0.23648124242722512,-0.2097320409627573,0.7430000257492065]) # end3pos = np.array([0.1493414817211018,-0.21703966731273366,0.7340000224113464]) # start4pos = np.array([0.24389595888042098,-0.30559190060482105,0.7499999952316284]) # end4pos = np.array([0.15822969083491112,-0.3106326911577041,0.7440000128746033]) # start5pos = np.array([0.2535787008200847,-0.402571052456421,0.7559999775886536]) # end5pos = np.array([0.16737854928028986,-0.41016720685793384,0.7580000114440918]) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(start2pos - end1pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = end1pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (start2pos - end1pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = start2pos # # points = np.vstack((points, temppoints)) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(end2pos - start2pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = start2pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (end2pos - start2pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = end2pos # # points = np.vstack((points, temppoints)) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(start3pos - end2pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = end2pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (start3pos - end2pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = start3pos # # points = np.vstack((points, temppoints)) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(end3pos - start3pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = start3pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (end3pos - start3pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = end3pos # # points = np.vstack((points, temppoints)) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(start4pos - end3pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = end3pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (start4pos - end3pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = start4pos # # points = np.vstack((points, temppoints)) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(end4pos - start4pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = start4pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (end4pos - start4pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = end4pos # # points = np.vstack((points, temppoints)) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(start5pos - end4pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = end4pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (start5pos - end4pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = start5pos # # points = np.vstack((points, temppoints)) # # #-------------------------- # # # 计算起始点和结束点之间的总距离 # # total_distance = np.linalg.norm(end5pos - start5pos) # # # 根据欧式距离步长为0.005估算所需的点数 # # num_points = int(total_distance / dis) # # # 初始化用于存储点的数组 # # temppoints = np.zeros((num_points + 1, 3)) # # temppoints[0] = start5pos # # # 生成欧式距离差约为0.01的点 # # for i in range(1, num_points + 1): # # direction = (end5pos - start5pos) / total_distance # 单位方向向量 # # temppoints[i] = temppoints[i-1] + direction * dis # # # 确保最后一个点与endpos完全重合 # # temppoints[-1] = end5pos # # points = np.vstack((points, temppoints)) # positions_2d = np.vstack([start1pos,end1pos]) # #print(points) # time_points = np.array([0,10]) # time_step = 0.01 # #print(time_points) # #使用点云B样条插值 # temppose = np.array([1,1,1,1,0,0]) # positions_2d_interp = circle_trajectory(center=temppose,radius=0.05,time_points=time_points,time_step=time_step) # quaternions_interp = np.tile(R.from_euler('xyz', np.array(temppose[3:])).as_quat(), (positions_2d_interp.shape[0], 1)) # # positions_2d_interp, quaternions_interp = cloud_point_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) # # print("2D Position Trajectory (Spline):") # # print(positions_2d) # # # 分解轨迹的x, y, z坐标 # x_trajectory, y_trajectory, z_trajectory = zip(*positions_2d_interp) # # 分解散点的x, y, z坐标 # x_scatter, y_scatter, z_scatter = zip(*positions_2d) # # 创建一个3D图形 # fig = plt.figure() # ax = fig.add_subplot(111, projection='3d') # # # 设置字体为 SimHei (黑体) # # plt.rcParams['font.sans-serif'] = ['SimHei'] # 使用黑体 # # plt.rcParams['axes.unicode_minus'] = False # 解决负号无法正常显示的问题 # # 绘制3D连续轨迹 # ax.plot(x_trajectory, y_trajectory, z_trajectory, label='B-spline interpolation trajectory', color='b') # # 绘制3D散点图 # ax.scatter(x_scatter, y_scatter, z_scatter, label='cloud points', color='r', marker='o') # # 设置轴标签 # ax.set_xlabel('X Label') # ax.set_ylabel('Y Label') # ax.set_zlabel('Z Label') # # 设置标题 # ax.set_title('3D Trajectory and Scatter Plot') # # 添加图例 # ax.legend() # # 设置轴的比例为相等 # plt.gca().set_aspect('equal', adjustable='box') # # # 显示图形 # # plt.show() # plot_trajectory(positions_2d_interp, quaternions_interp) # # for i in range(10): # # tempX = np.linspace(startpos[0], endpos[0], 10) # # 示例使用 # positions_2d = np.array([[0, 0], [1, 1], [1, 2],[2,2]]) #, [1, 1], [2, 1], [1, 1], [1, 1],[0, 0]]) # positions_3d = np.array([[0, 0, 0], [1, 1, 1], [2, 0, 2], [3, 1, 3]]) # quaternions = np.array([[0, 0, 0, 1], [0.707, 0, 0, 0.707], [1, 0, 0, 0], [1, 0, 0, 0]]) # time_points = np.array([0, 1, 3, 4]) # time_step = 0.5 # # # 只插值2D位置 # # positions_2d_interp = linear_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) # # print("2D Position Trajectory:") # # print(positions_2d_interp) # # # 只插值3D位置 # # positions_3d_interp = linear_interpolate(positions=positions_3d, time_points=time_points, time_step=time_step) # # print("3D Position Trajectory:") # # print(positions_3d_interp) # # # 只插值姿态 # # quaternions_interp = linear_interpolate(quaternions=quaternions, time_points=time_points, time_step=time_step) # # print("Quaternion Trajectory:") # # print(quaternions_interp) # # # 同时插值3D位置和姿态 # # positions_3d_interp, quaternions_interp = linear_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step) # # print("3D Position and Quaternion Trajectory:") # # print(positions_3d_interp) # # print(quaternions_interp) # # # 绘制插值轨迹 # # plot_trajectory(positions_2d_interp) # # plot_trajectory(positions_3d_interp) # # plot_trajectory(quaternions_interp) # # plot_trajectory(positions_3d_interp, quaternions_interp) # # # 使用样条插值 # # positions_2d_interp = spline_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) # # print("2D Position Trajectory (Spline):") # # print(positions_2d_interp) # # plot_trajectory(positions_2d_interp) # # positions_3d_interp, quaternions_interp = spline_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step) # # print("3D Position and Quaternion Trajectory (Spline):") # # print(positions_3d_interp) # # print(quaternions_interp) # # plot_trajectory(positions_3d_interp, quaternions_interp) # # # 使用点云B样条插值 # # positions_2d_interp = cloud_point_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) # # print("2D Position Trajectory (Spline):") # # print(positions_2d_interp) # # plot_trajectory(positions_2d_interp) # # positions_3d_interp, quaternions_interp = cloud_point_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step) # # print("3D Position and Quaternion Trajectory (Spline):") # # print(positions_3d_interp) # # print(quaternions_interp) # # plot_trajectory(positions_3d_interp, quaternions_interp)