60 lines
2.1 KiB
Python
60 lines
2.1 KiB
Python
|
# pip install toppra numpy scipy pandas matplotlib
|
|||
|
|
import numpy as np
|
|||
|
|
import pandas as pd
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
import toppra as ta
|
|||
|
|
import toppra.algorithm as algo
|
|||
|
|
import toppra.constraint as constraint
|
|||
|
|
import toppra.interpolator as interp
|
|||
|
|
|
|||
|
|
# === 1) 读取 CSV（只取 q1..q7）===
|
|||
|
|
CSV_PATH = "/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv" #
|
|||
|
|
joint_cols = ["q1","q2","q3","q4","q5","q6","q7"]
|
|||
|
|
df = pd.read_csv(CSV_PATH)
|
|||
|
|
assert all(c in df.columns for c in joint_cols), "CSV 缺少关节列 q1..q7"
|
|||
|
|
waypoints = df[joint_cols].to_numpy()
|
|||
|
|
N, dof = waypoints.shape
|
|||
|
|
assert N >= 2, "至少需要两行关节路点"
|
|||
|
|
|
|||
|
|
# === 2) 构造样条路径（路径参数 s∈[0,1]）===
|
|||
|
|
s_grid = np.linspace(0, 1, N)
|
|||
|
|
path = interp.SplineInterpolator(s_grid, waypoints)
|
|||
|
|
|
|||
|
|
# === 3) 约束（示例值：请改成你的真实上限）===
|
|||
|
|
vmax = np.array([1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]) # rad/s
|
|||
|
|
amax = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) # rad/s^2
|
|||
|
|
vlim = np.vstack([-vmax, vmax]).T
|
|||
|
|
alim = np.vstack([-amax, amax]).T
|
|||
|
|
pc_vel = constraint.JointVelocityConstraint(vlim)
|
|||
|
|
pc_acc = constraint.JointAccelerationConstraint(alim)
|
|||
|
|
|
|||
|
|
# === 4) toppra 时间最优轨迹（起止速度=0）===
|
|||
|
|
topp = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper="seidel")
|
|||
|
|
traj = topp.compute_trajectory(0.0, 0.0)
|
|||
|
|
T = traj.get_duration()
|
|||
|
|
print(f"Total duration: {T:.4f} s")
|
|||
|
|
|
|||
|
|
# === 5) 采样并绘图 ===
|
|||
|
|
ts = np.linspace(0, T, 300)
|
|||
|
|
q = traj.eval(ts) # (300,7)
|
|||
|
|
qd = traj.evald(ts)
|
|||
|
|
qdd = traj.evaldd(ts)
|
|||
|
|
|
|||
|
|
# 位置
|
|||
|
|
plt.figure()
|
|||
|
|
for i in range(dof):
|
|||
|
|
plt.plot(ts, q[:, i], label=f"q{i+1}")
|
|||
|
|
plt.xlabel("Time (s)"); plt.ylabel("Position (rad)"); plt.title("Joint Positions"); plt.legend(); plt.tight_layout(); plt.show()
|
|||
|
|
|
|||
|
|
# 速度
|
|||
|
|
plt.figure()
|
|||
|
|
for i in range(dof):
|
|||
|
|
plt.plot(ts, qd[:, i], label=f"qd{i+1}")
|
|||
|
|
plt.xlabel("Time (s)"); plt.ylabel("Velocity (rad/s)"); plt.title("Joint Velocities"); plt.legend(); plt.tight_layout(); plt.show()
|
|||
|
|
|
|||
|
|
# 加速度
|
|||
|
|
plt.figure()
|
|||
|
|
for i in range(dof):
|
|||
|
|
plt.plot(ts, qdd[:, i], label=f"qdd{i+1}")
|
|||
|
|
plt.xlabel("Time (s)"); plt.ylabel("Acceleration (rad/s²)"); plt.title("Joint Accelerations"); plt.legend(); plt.tight_layout(); plt.show()
|