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.
Returns:

P – Polynomial function of the Bezier curve.

Return type:

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.
Returns:

P – Polynomial function of the Hermite curve.

Return type:

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.
Returns:

pose – Linear interpolation between the first two arguments.

Return type:

(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.
Returns:

pose – Linear interpolation between the first two arguments.

Return type:

(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.
Returns:handle – OpenRAVE graphical handle. Must be stored in some variable, otherwise the drawn object will vanish instantly.
Return type: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.