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 thirdorder 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 zerovelocity 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 zerovelocity and zeroacceleration 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 zerovelocity and zeroacceleration 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.