maspack.spatialmotion

Class EulerFilter

java.lang.Object

maspack.spatialmotion.EulerFilter

public class EulerFilter
extends java.lang.Object

Constructor Summary

Constructors

Constructor and Description
EulerFilter()

Method Summary

Modifier and Type	Method and Description
static int	filter(double[] ref, double[] rpy, double eps, double[] out) Performs an Euler filter to prevent artifacts from gimbal lock.
static double	findNearestAngle(double ref, double ang) Given an angle ang, find an equivalent angle that is within +/- PI of a given reference angle ref.
static boolean	isFlipped(int flag)
static boolean	isShifted(int flag)

Methods inherited from class java.lang.Object

equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail

EulerFilter

public EulerFilter()

Method Detail

filter

public static int filter(double[] ref,
                         double[] rpy,
                         double eps,
                         double[] out)

Performs an Euler filter to prevent artifacts from gimbal lock. Out of all possible solutions, finds the one that is closest to the reference set of angles.

Parameters:

ref - Reference set of angles {roll, pitch, yaw}

rpy - Current rpy angles to adjust {roll, pitch, yaw}

eps - Threshold to deal with singularity at (yaw - k*Pi/2) < eps

out - Adjusted rpy angles

Returns:

Flag consisting of potentially FLIP_FLAG if angles flipped, or SHIFT_FLAG if yaw/roll shifted

findNearestAngle

public static double findNearestAngle(double ref,
                                      double ang)

Given an angle ang, find an equivalent angle that is within +/- PI of a given reference angle ref.

Parameters:

ref - reference angle (radians)

ang - initial angle (radians)

Returns:

angle equivalent to ang within +/- PI of ref.

isShifted

public static boolean isShifted(int flag)

isFlipped

public static boolean isFlipped(int flag)