/// <summary> Generates a calibration pose that corresponds to a fully gclosed hand (finger flexion, thumb abduction calibration) </summary>
/// <param name="interpolator"></param>
/// <returns></returns>
public static CalibrationPose GetFullFist(ref InterpolationSet_IMU interpolator)
{
int[][] affects = SetupArray(ref interpolator);
int[][] calibrateW = SetupArray(ref interpolator);
int f = 0; //thumb
{
//affects x0 of flexion / extension
affects[f][(int)IntAngles_IMU_Thumb.CMC_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Thumb.CMC_FE] = flex;
affects[f][(int)IntAngles_IMU_Thumb.MCP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Thumb.MCP_FE] = flex;
affects[f][(int)IntAngles_IMU_Thumb.IP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Thumb.IP_FE] = flex;
//affects x1 of abduction / adduction
affects[f][(int)IntAngles_IMU_Thumb.CMC_Abd] = x1;
calibrateW[f][(int)IntAngles_IMU_Thumb.CMC_Abd] = abd; //abduction
}
//for the rest of the fingers
for (f = 1; f < affects.Length; f++)
{
affects[f][(int)IntAngles_IMU_Finger.MCP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Finger.MCP_FE] = flex;
affects[f][(int)IntAngles_IMU_Finger.PIP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Finger.PIP_FE] = flex;
affects[f][(int)IntAngles_IMU_Finger.DIP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Finger.DIP_FE] = flex;
}
return(new CalibrationPose(affects, calibrateW));
}
/// <summary> Utility function that creates an array of floats of the appropriate size, with default values 0 </summary>
/// <param name="interpolator"></param>
/// <returns></returns>
protected static float[][] SetupFloatArray(ref InterpolationSet_IMU interpolator)
{
float[][] res = new float[interpolator.myAngles.Length][];
for (int f = 0; f < interpolator.myAngles.Length; f++)
{
res[f] = new float[interpolator.myAngles[f].Length];
}
return(res);
}
/// <summary> Generates SG_CalibrationPoses for this interpolator. </summary>
/// <param name="intepolator"></param>
private void LoadProfiles(InterpolationSet_IMU intepolator)
{
poses = new CalibrationPose[(int)CalPose.All];
poses[(int)CalPose.HandOpen] = CalibrationPose.GetFullOpen(ref intepolator);
poses[(int)CalPose.HandClosed] = CalibrationPose.GetFullFist(ref intepolator);
poses[(int)CalPose.FingersExt] = CalibrationPose.GetOpenHand(ref intepolator);
poses[(int)CalPose.FingersFlexed] = CalibrationPose.GetFist(ref intepolator);
poses[(int)CalPose.ThumbUp] = CalibrationPose.GetThumbsUp(ref intepolator);
poses[(int)CalPose.ThumbFlex] = CalibrationPose.GetThumbFlexed(ref intepolator);
poses[(int)CalPose.AbdOut] = CalibrationPose.GetThumbAbd(ref intepolator);
poses[(int)CalPose.NoThumbAbd] = CalibrationPose.GetThumbNoAbd(ref intepolator);
}
/// <summary> Utility function that creates an array of integers of the appropriate size, with default values -1 (none) </summary>
/// <param name="interpolator"></param>
/// <returns></returns>
protected static int[][] SetupArray(ref InterpolationSet_IMU interpolator)
{
int[][] res = new int[interpolator.myAngles.Length][];
for (int f = 0; f < interpolator.myAngles.Length; f++)
{
res[f] = new int[interpolator.myAngles[f].Length];
for (int j = 0; j < res[f].Length; j++)
{
res[f][j] = none;
}
}
return(res);
}
/// <summary> Generates a calibration pose that corresponds to a thumb flat against the hand palm (thumb adduction calibration) </summary>
/// <param name="interpolator"></param>
/// <returns></returns>
public static CalibrationPose GetThumbNoAbd(ref InterpolationSet_IMU interpolator)
{
int[][] affects = SetupArray(ref interpolator);
int[][] calibrateW = SetupArray(ref interpolator);
int f = 0; //thumb
{
//affects x0 of abduction / adduction
affects[f][(int)IntAngles_IMU_Thumb.CMC_Abd] = x0;
calibrateW[f][(int)IntAngles_IMU_Thumb.CMC_Abd] = abd; //abduction
}
return(new CalibrationPose(affects, calibrateW /*, yAffect, yValues*/));
}
private CalibrationPose[] LoadProfiles(InterpolationSet_IMU interpolator)
{
// order in this array needs to match the one in the Enum Pose
return(new CalibrationPose[]
{
CalibrationPose.GetFullOpen(ref interpolator),
CalibrationPose.GetFullFist(ref interpolator),
CalibrationPose.GetOpenHand(ref interpolator),
CalibrationPose.GetFist(ref interpolator),
CalibrationPose.GetThumbsUp(ref interpolator),
CalibrationPose.GetThumbFlexed(ref interpolator),
CalibrationPose.GetThumbAbd(ref interpolator),
CalibrationPose.GetThumbNoAbd(ref interpolator)
});
}
/// <summary> Generates a calibration pose that corresponds to all fingers flexed (finger flexion calibration). </summary>
/// <param name="interpolator"></param>
/// <returns></returns>
public static CalibrationPose GetFist(ref InterpolationSet_IMU interpolator)
{
int[][] affects = SetupArray(ref interpolator);
int[][] calibrateW = SetupArray(ref interpolator);
//affects x1 of flexion / extension, using y (flexion) as input
for (int f = 1; f < affects.Length; f++)
{
affects[f][(int)IntAngles_IMU_Finger.MCP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Finger.MCP_FE] = flex;
affects[f][(int)IntAngles_IMU_Finger.PIP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Finger.PIP_FE] = flex;
affects[f][(int)IntAngles_IMU_Finger.DIP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Finger.DIP_FE] = flex;
}
return(new CalibrationPose(affects, calibrateW));
}
/// <summary> Generates a calibration pose that corresponds to a flexed thumb (thumb flexed calibration) </summary>
/// <param name="interpolator"></param>
/// <returns></returns>
public static CalibrationPose GetThumbFlexed(ref InterpolationSet_IMU interpolator)
{
int[][] affects = SetupArray(ref interpolator);
int[][] calibrateW = SetupArray(ref interpolator);
int f = 0; //thumb
{
//affects x0 of flexion / extension
affects[f][(int)IntAngles_IMU_Thumb.CMC_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Thumb.CMC_FE] = flex;
affects[f][(int)IntAngles_IMU_Thumb.MCP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Thumb.MCP_FE] = flex;
affects[f][(int)IntAngles_IMU_Thumb.IP_FE] = x1;
calibrateW[f][(int)IntAngles_IMU_Thumb.IP_FE] = flex;
}
return(new CalibrationPose(affects, calibrateW));
}
/// <summary> Generates a calibration pose that corresponds to all fingers extended (finger extension calibration). </summary>
/// <param name="interpolator"></param>
/// <returns></returns>
public static CalibrationPose GetOpenHand(ref InterpolationSet_IMU interpolator)
{
int[][] affects = SetupArray(ref interpolator);
int[][] calibrateW = SetupArray(ref interpolator);
for (int f = 1; f < affects.Length; f++)
{
//affects x0 of flexion / extension
affects[f][(int)IntAngles_IMU_Finger.MCP_FE] = x0;
calibrateW[f][(int)IntAngles_IMU_Finger.MCP_FE] = flex;
affects[f][(int)IntAngles_IMU_Finger.PIP_FE] = x0;
calibrateW[f][(int)IntAngles_IMU_Finger.PIP_FE] = flex;
affects[f][(int)IntAngles_IMU_Finger.DIP_FE] = x0;
calibrateW[f][(int)IntAngles_IMU_Finger.DIP_FE] = flex;
}
return(new CalibrationPose(affects, calibrateW));
}
/// <summary> Calibrate all parameters of an InterpolationSet, based on this pose's parameters and a set of input values. </summary>
/// <param name="calibrationValues"></param>
/// <param name="interpolator"></param>
public void CalibrateParameters(Vector3[] calibrationValues, ref InterpolationSet_IMU interpolator)
{
for (int f = 0; f < interpolator.myAngles.Length; f++)
{
for (int j = 0; j < interpolator.myAngles[f].Length; j++)
{
if (calbrUsing[f][j] != none)
{
float value = calibrationValues[f][calbrUsing[f][j]];
if (xAffect[f][j] == x0 && interpolator.myAngles[f][j].x1 != value)
{
interpolator.myAngles[f][j].x0 = value;
}
else if (xAffect[f][j] == x1 && interpolator.myAngles[f][j].x0 != value) //chekcing for nullrefs
{
interpolator.myAngles[f][j].x1 = value;
}
if (yAffect.Length > f && yAffect[f].Length > j && yAffect[f][j] != none) //activating y
{
if (yValue.Length > f && yValue[f].Length > j)
{
if (yAffect[f][j] == y0)
{
interpolator.myAngles[f][j].x0 = yValue[f][j];
}
else if (yAffect[f][j] == y1)
{
interpolator.myAngles[f][j].x1 = yValue[f][j];
}
}
}
}
}
}
}
