Interpolation#
Frames interpolation#
The interpolate_frames function can be used to interpolate between two frames.
# Interpolating between frame1 and frame2
# The last argument (AtoB) is the interpolation factor: 0: frameA, 1: frameB
T_world_interpolated = placo.interpolate_frames(T_world_frameA, T_world_frameB, 0.2)
Cubic splines#
The CubicSpline class can be used to create a point to point trajectory,
while imposing velocities.
spline = placo.CubicSpline()
# Arguments are time, position and velocity
spline.add_point(0.0, 0.0, 0.0)
spline.add_point(1.0, 3.0, 0.0)
spline.add_point(2.0, -2.0, 0.0)
spline.add_point(3.0, 0.0, 0.0)
# Retrieving the position, velocity and acceleration at time t with:
t = 0.5
pos = spline.pos(t)
vel = spline.vel(t)
acc = spline.acc(t)
3D Cubic Splines#
The CubicSpline3D class can be used to create a point to point trajectory in 3D.
It basically wraps 3 CubicSpline objects together.
spline = placo.CubicSpline3D()
# The API is similar to CubicSpline, with a convenient wrapper of dimension 3
spline.add_point(0., np.array([0., 0., 0.]), np.array([0., 0., 0.]))
spline.add_point(1., np.array([3., 0., 0.]), np.array([0., 0., 0.]))
spline.add_point(2., np.array([0., 3., 0.]), np.array([0., 0., 0.]))
spline.add_point(3., np.array([0., 0., 3.]), np.array([0., 0., 0.]))
# Positions, velocities and acceleration will return 3D vectors
pos = spline.pos(0.5)