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.