def test_consistency(request, env, traj_string):
"Check the consistency between the OpenRAVE trajectory and the interpolator."
robot = env.GetRobots()[0]
active_indices = robot.GetActiveDOFIndices()
traj = orpy.RaveCreateTrajectory(env, "")
traj.deserialize(traj_string)
path = toppra.RaveTrajectoryWrapper(traj, robot)
spec = traj.GetConfigurationSpecification()
N = 100
ss = np.linspace(0, path.duration, N)
# Openrave samples
qs_rave = []
qds_rave = []
qdds_rave = []
for s in ss:
data = traj.Sample(s)
qs_rave.append(spec.ExtractJointValues(data, robot, active_indices, 0))
qds_rave.append(spec.ExtractJointValues(data, robot, active_indices, 1))
qdds_rave.append(spec.ExtractJointValues(data, robot, active_indices, 2))
# Interpolator samples
qs_ra = path.eval(ss)
qds_ra = path.evald(ss)
if path._interpolation != "quadratic":
qdds_ra = path.evaldd(ss)
# plt.plot(qs_ra)
# plt.title(request.node.name)
# plt.show()
np.testing.assert_allclose(qs_rave, qs_ra, atol=1e-8)
np.testing.assert_allclose(qds_rave, qds_ra, atol=1e-8)
if path._interpolation != "quadratic":
np.testing.assert_allclose(qdds_rave, qdds_ra, atol=1e-8)
def plot_trajectories(traj1, traj2, robot, N=50):
"Plot the trajectory (position, velocity and acceleration)."
fig, axs = plt.subplots(3, 1, sharex=True)
for idx, traj in enumerate([traj1, traj2]):
ts = np.linspace(0, traj.GetDuration(), N)
traj_ra = toppra.RaveTrajectoryWrapper(traj, robot)
qs = traj_ra.eval(ts)
qds = traj_ra.evald(ts)
qdds = traj_ra.evaldd(ts)
if idx == 0:
style = "o"
alpha = 0.5
else:
style = "-"
alpha = 1.0
for i in range(traj_ra.get_dof()):
axs[0].plot(ts, qs[:, i], style, c="C"+str(i), alpha=alpha)
axs[1].plot(ts, qds[:, i], style, c="C"+str(i), alpha=alpha)
axs[2].plot(ts, qdds[:, i], style, c="C"+str(i), alpha=alpha)
vel_max = robot.GetActiveDOFMaxVel()
accel_max = robot.GetActiveDOFMaxAccel()
style = "--"
for i in range(traj_ra.get_dof()):
axs[1].plot([0, ts[-1]], [vel_max[i], vel_max[i]], style, c="C" + str(i))
axs[1].plot([0, ts[-1]], [-vel_max[i], -vel_max[i]], style, c="C" + str(i))
axs[2].plot([0, ts[-1]], [accel_max[i], accel_max[i]], style, c="C" + str(i))
axs[2].plot([0, ts[-1]], [-accel_max[i], -accel_max[i]], style, c="C" + str(i))
plt.show()
def plot_trajectories(traj1, traj2, robot, N=50):
"Plot the trajectory (position, velocity and acceleration)."
fig, axs = plt.subplots(3, 1, sharex=True)
for idx, traj in enumerate([traj1, traj2]):
ts = np.linspace(0, traj.GetDuration(), N)
traj_ra = toppra.RaveTrajectoryWrapper(traj, robot)
qs = traj_ra.eval(ts)
qds = traj_ra.evald(ts)
qdds = traj_ra.evaldd(ts)
if idx == 0:
style = "o"
alpha = 0.5
else:
style = "-"
alpha = 1.0
for i in range(traj_ra.dof):
axs[0].plot(range(N), qs[:, i], style, c="C" + str(i), alpha=alpha)
axs[1].plot(range(N),
qds[:, i],
style,
c="C" + str(i),
alpha=alpha)
axs[2].plot(range(N),
qdds[:, i],
style,
c="C" + str(i),
alpha=alpha)
plt.show()