/// <summary> /// Run regression algorithm on collected data, returns two arrays of fitted coefficients, one for each segment /// </summary> public double[][] LearnGrasp() { // Remove outliers from main parabolic grasp trajectories // 1. Perform preliminary regression, remove entries where abs residuals > 2*SD double[] _preliminaryRegressionCoeffs = TofRegression.QuadraticFit(dataCollector[0], dataCollector[1]); List <double> _preliminaryAbsResiduals = TofRegression.RegressionAbsDeviations(dataCollector[0], dataCollector[1], _preliminaryRegressionCoeffs); double _stdDev = TofRegression.SStDev(_preliminaryAbsResiduals, _meanIsZero: true); DataCollector _reducedDataCollector = dataCollector.RemoveAboveThresh(2d * _stdDev, _preliminaryAbsResiduals); // 2. New regression with reduced sample, remove entries of original sample where abs deviations according to new regression > 1.5*SD double[] _newRegressionCoeffs = TofRegression.QuadraticFit(_reducedDataCollector[0], _reducedDataCollector[1]); List <double> _newAbsDeviations = TofRegression.RegressionAbsDeviations(dataCollector[0], dataCollector[1], _newRegressionCoeffs); double _stdDevNew = TofRegression.SStDev(_newAbsDeviations, _meanIsZero: false); DataCollector _finalDataCollector = dataCollector.RemoveAboveThresh(1.5d * _stdDevNew, _newAbsDeviations); // Segment lists List <double>[] _dc1 = new List <double> [2]; List <double>[] _dc2 = new List <double> [2]; bool _segmentSuccess = _finalDataCollector.SegmentLists(ref _dc1, ref _dc2, (CurLearningGrasp == LearningGrasp.Power) ? Max_SegmentPoint : Min_SegmentPoint); // Perform linear regression on cleaner dataset double[][] _betas = new double[2][]; _betas[0] = TofRegression.LinearFit(_dc1[0], _dc1[1]); _betas[1] = (_segmentSuccess) ? TofRegression.LinearFit(_dc2[0], _dc2[1]) : _betas[0]; //if not segmented, just use same coeffs return(_betas); }
/// <summary> /// Run thumb regression algorithm on collected data, returns fitted coefficients (two sets both the same) /// </summary> public double[][] LearnGraspThumb() { // Skip removing outliers and segment, and directly regress // Perform quadratic regression on dataset double[][] _betas = new double[2][]; _betas[0] = TofRegression.QuadraticFit(dataCollector[0], dataCollector[1]); _betas[1] = _betas[0]; //just use same coeffs return(_betas); }
/// <summary> /// Return grasp prediction between plate(min, graspPrediction=0) and power(max, graspPrediction=100) for given graspness value /// </summary> /// <param name="_graspness">Current graspness value</param> public float PredictGrasp(float _graspness) { // Retrieve Tof reading float _tofreading = TofManager.TofReading[(int)fingerType]; // Use coeffs from correct segment int _segmentIndMax = (_graspness < GraspLearner.Max_SegmentPoint) ? 0 : 1; int _segmentIndMin = (_graspness < GraspLearner.Min_SegmentPoint) ? 0 : 1; // Predict tof max and min based on regression (max = power, min = plate) float _tofGraspMax = TofRegression.RegressionPrediction(GraspPredictManager.GraspTofCoeffsMax[(int)fingerType][_segmentIndMax], _graspness); float _tofGraspMin = TofRegression.RegressionPrediction(GraspPredictManager.GraspTofCoeffsMin[(int)fingerType][_segmentIndMin], _graspness); _tofGraspMax = Mathf.Clamp(_tofGraspMax, 0, _tofGraspMax); _tofGraspMin = Mathf.Clamp(_tofGraspMin, 0, _tofGraspMin); // Predict grasp between max and min float _graspPrediction = _linearPercentage(_tofGraspMax, _tofGraspMin, _tofreading); //Debug.Log($"{fingerType} graspness {_graspness} tof {_tofreading} pred {_graspPrediction} [min {_tofGraspMin} max {_tofGraspMax}] from predictor"); return(_graspPrediction); }
/// <summary> /// Scales 2 curves given by _coeffs1 and _coeffs2 so they intersect at x=_thresh /// </summary> private static void _scaleCurvesIntersectAtThreshold(ref double[] _coeffs1, ref double[] _coeffs2, double _thresh) { // find (y at segment point) to y-scale about double _yMaxPivot = TofRegression.RegressionPrediction(_coeffs1, GraspLearner.Max_SegmentPoint); double _yMinPivot = TofRegression.RegressionPrediction(_coeffs2, GraspLearner.Min_SegmentPoint); // find each y at the actual threshold double _yMaxThresh = TofRegression.RegressionPrediction(_coeffs1, _thresh); double _yMinThresh = TofRegression.RegressionPrediction(_coeffs2, _thresh); // find y at threshold so power and plate intersect at their average double _yAvgAtThreshold = 0.5d * (_yMaxThresh + _yMinThresh); // find y scale factor so intersects at (x,y) = (threshold, avg of power and plate), where avg = yPivot + k_i*(f_i - yPivot) double _kMax = (_yAvgAtThreshold - _yMaxPivot) / (_yMaxThresh - _yMaxPivot); double _kMin = (_yAvgAtThreshold - _yMinPivot) / (_yMinThresh - _yMinPivot); // scale coeffs //a,b -> ka,kb // c-> kc + ym (1-k) _scaleVectorInPlace(ref _coeffs1, _kMax); _scaleVectorInPlace(ref _coeffs2, _kMin); // offset constant term _coeffs1[0] += (_yMaxPivot * (1d - _kMax)); _coeffs2[0] += (_yMinPivot * (1d - _kMin)); }