# Interpolation¶

Interpolation is the operation of computing trajectories that connect an initial state (position and its derivatives such as velocity and acceleration) to a desired one.

## Position¶

In the absence of inequality constraints, interpolation for positions can be solved by polynomial interpolation. The following functions and classes provide simple interfaces for this common operation.

pymanoid.interp.interpolate_cubic_bezier(p0, p1, p2, p3)

Compute the cubic Bezier curve corresponding to four control points.

Parameters: p0 ((3,) array) – First control point. p1 ((3,) array) – Second control point. p2 ((3,) array) – Third control point. p3 ((3,) array) – Fourth control point. P – Polynomial function of the Bezier curve. NDPolynomial
pymanoid.interp.interpolate_cubic_hermite(p0, v0, p1, v1)

Compute the third-order polynomial of the Hermite curve connecting $$(p_0, v_0)$$ to $$(p_1, v_1)$$.

Parameters: p0 ((3,) array) – Start point. v0 ((3,) array) – Start velocity. p1 ((3,) array) – End point. v1 ((3,) array) – End velocity. P – Polynomial function of the Hermite curve. NDPolynomial
class pymanoid.interp.LinearPosInterpolator(start_pos, end_pos, duration, body=None)

Linear interpolation between two positions.

Parameters: start_pos ((3,) array) – Initial position to interpolate from. end_pos ((3,) array) – End position to interpolate to. body (Body, optional) – Body affected by the update function.
class pymanoid.interp.CubicPosInterpolator(start_pos, end_pos, duration, body=None)

Cubic interpolation between two positions with zero-velocity boundary conditions.

Parameters: start_pos ((3,) array) – Initial position to interpolate from. end_pos ((3,) array) – End position to interpolate to. body (Body, optional) – Body affected by the update function.
class pymanoid.interp.QuinticPosInterpolator(start_pos, end_pos, duration, body=None)

Quintic interpolation between two positions with zero-velocity and zero-acceleration boundary conditions.

Parameters: start_pos ((3,) array) – Initial position to interpolate from. end_pos ((3,) array) – End position to interpolate to. body (Body, optional) – Body affected by the update function.

## Orientation¶

In the absence of inequality constraints, interpolation for orientations can be solved by spherical linear interpolation. The following functions and classes provide simple interfaces for this common operation. They compute poses: following OpenRAVE terminology, the pose of a rigid body denotes the 7D vector of its 4D orientation quaternion followed by its 3D position coordinates.

pymanoid.interp.interpolate_pose_linear(pose0, pose1, x)

Standalone function for linear pose interpolation.

Parameters: pose0 ((7,) array) – First pose. pose1 ((7,) array) – Second pose. x (scalar) – Number between 0 and 1. pose – Linear interpolation between the first two arguments. (7,) array
pymanoid.interp.interpolate_pose_quadratic(pose0, pose1, x)

Pose interpolation that is linear in orientation (SLERP) and quadratic (Bezier) in position.

Parameters: pose0 ((7,) array) – First pose. pose1 ((7,) array) – Second pose. x (scalar) – Number between 0 and 1. pose – Linear interpolation between the first two arguments. (7,) array

Note

Initial and final linear velocities on the trajectory are zero.

class pymanoid.interp.LinearPoseInterpolator(start_pose, end_pose, duration, body=None)

Interpolate a body trajectory between two poses. Intermediate positions are interpolated linearly, while orientations are computed by spherical linear interpolation.

Parameters: start_pose ((7,) array) – Initial pose to interpolate from. end_pose ((7,) array) – End pose to interpolate to. body (Body, optional) – Body affected by the update function.
eval_pos(s)

Evaluate position at index s between 0 and 1.

Parameters: s (scalar) – Normalized trajectory index between 0 and 1.
class pymanoid.interp.CubicPoseInterpolator(start_pose, end_pose, duration, body=None)

Interpolate a body trajectory between two poses. Intermediate positions are interpolated cubically with zero boundary velocities, while orientations are computed by spherical linear interpolation.

Parameters: start_pose ((7,) array) – Initial pose to interpolate from. end_pose ((7,) array) – End pose to interpolate to. body (Body, optional) – Body affected by the update function.
eval_pos(s)

Evaluate position at index s between 0 and 1.

Parameters: s (scalar) – Normalized trajectory index between 0 and 1.
class pymanoid.interp.PoseInterpolator(start_pose, end_pose, duration, body=None)

Interpolate a body trajectory between two poses. Orientations are computed by spherical linear interpolation.

Parameters: start_pose ((7,) array) – Initial pose to interpolate from. end_pose ((7,) array) – End pose to interpolate to. body (Body, optional) – Body affected by the update function.
draw(color='b')

Draw positions of the interpolated trajectory.

Parameters: color (char or triplet, optional) – Color letter or RGB values, default is ‘b’ for blue. handle – OpenRAVE graphical handle. Must be stored in some variable, otherwise the drawn object will vanish instantly. openravepy.GraphHandle
eval_pos(s)

Evaluate position at index s between 0 and 1.

Parameters: s (scalar) – Normalized trajectory index between 0 and 1.
class pymanoid.interp.QuinticPoseInterpolator(start_pose, end_pose, duration, body=None)

Interpolate a body trajectory between two poses. Intermediate positions are interpolated quintically with zero-velocity and zero-acceleration boundary conditions, while orientations are computed by spherical linear interpolation.

Parameters: start_pose ((7,) array) – Initial pose to interpolate from. end_pose ((7,) array) – End pose to interpolate to. body (Body, optional) – Body affected by the update function.
eval_pos(s)

Evaluate position at index s between 0 and 1.

Parameters: s (scalar) – Normalized trajectory index between 0 and 1.