package us.ihmc.commonWalkingControlModules.momentumBasedController.optimization;

import org.ejml.data.DMatrixRMaj;
import org.junit.jupiter.api.Test;
public class TypicalMotionConstraintsTest
{

	@Test
   public void testDoubleSupportKneesBentTypicalMotionConstraints()
   {
      // Have J x = p; A x = b. Check if an all out solution without requiring optimization. 
      // Since there are no ground reaction force constraints, there very well may be a solution.

      double[][] JPrimaryMotionConstraintsData = new double[][]{
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9997090677149058, 0.023630209305342938, -0.004836645198819714, 0.0, 0.0, 0.0, 0.0, -0.004836645198819714, 0.0, 0.0, 0.9999882412158135, 0.0, 0.0, 1.3552527156068805E-18, 0.0, 0.0, 0.0, 0.0, 1.3552527156068805E-18, 0.0, 0.0, 0.0, 1.3552527156068805E-18, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.023629866592815588, 0.9997207675252482, 1.2799818117073203E-4, 0.0, 0.0, 0.0, 0.0, 1.2799818117073203E-4, 0.0, 0.0, -3.519376154549118E-4, 0.0, 0.0, 0.9973631557511714, 0.0, 0.0, 0.0, 0.0, 0.9973631557511714, 0.0, 0.0, 0.0, 0.9973631557511714, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.004838319274223116, -1.3671661562458446E-5, 0.9999882951713418, 0.0, 0.0, 0.0, 0.0, 0.9999882951713418, 0.0, 0.0, 0.004836689985754854, 0.0, 0.0, 0.07257227810992713, 0.0, 0.0, 0.0, 0.0, 0.07257227810992713, 0.0, 0.0, 0.0, 0.07257227810992713, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.017365951200880296, -0.7632954155927968, -0.13975214626387292, 0.9997090677149058, 0.023630209305342938, -0.004836645198819714, 0.0, -0.05077803923724626, 0.0, 0.0, 2.3579820235447352E-5, 0.0, 0.0, -0.7154171404688644, 0.0, 0.0, 0.0, 0.0, -0.40815120325248044, 0.0, 0.0, 0.0, -0.06682333143532848, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.7635312721142093, 0.0180412271307821, 0.0465058578512905, -0.023629866592815588, 0.9997207675252482, 1.2799818117073203E-4, 0.0, 0.044402799724529915, 0.0, 0.0, 0.7637329534791978, 0.0, 0.0, -1.5571831022249093E-4, 0.0, 0.0, 0.0, 0.0, -0.01604948526622949, 0.0, 0.0, 0.0, 0.0019594515089680323, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.14080162467086133, -0.0468851558951085, -6.818921939019542E-4, 0.004838319274223116, -1.3671661562458446E-5, 0.9999882951713418, 0.0, -2.512817784960963E-4, 0.0, 0.0, 0.05069723555749218, 0.0, 0.0, 0.0021400417533604665, 0.0, 0.0, 0.0, 0.0, 0.2205685929972062, 0.0, 0.0, 0.0, -0.02692880520528163, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9997924620143747, 0.019789326935919258, -0.004838950160841789, 0.0, 0.0, 0.0, 0.0, 0.0, -0.004838950160841789, 0.0, 0.0, 0.9999882280172844, 0.0, 0.0, 1.0842021724855044E-18, 0.0, 0.0, 0.0, 0.0, 1.0842021724855044E-18, 0.0, 0.0, 0.0, 1.0842021724855044E-18, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.019788966833926634, 0.9998041717440552, 1.2228994040817392E-4, 0.0, 0.0, 0.0, 0.0, 0.0, 1.2228994040817392E-4, 0.0, 0.0, 3.5890893653860075E-4, 0.0, 0.0, 0.9972605917715024, 0.0, 0.0, 0.0, 0.0, 0.9972605917715024, 0.0, 0.0, 0.0, 0.9972605917715024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.004840422593282837, -2.6506736356319046E-5, 0.9999882847346322, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9999882847346322, 0.0, 0.0, 0.004838905994919433, 0.0, 0.0, -0.07396831821498377, 0.0, 0.0, 0.0, 0.0, -0.07396831821498377, 0.0, 0.0, 0.0, -0.07396831821498377, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0157895360257299, -0.7633486220884984, 0.14054776884043207, 0.9997924620143747, 0.019789326935919258, -0.004838950160841789, 0.0, 0.0, 0.051566239721152755, 0.0, 0.0, -2.4046898748066617E-5, 0.0, 0.0, -0.7154696552331623, 0.0, 0.0, 0.0, 0.0, -0.40956534106262044, 0.0, 0.0, 0.0, -0.0668164596486907, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.7635789401337406, 0.015107709828971194, 0.04650430663684653, -0.019788966833926634, 0.9998041717440552, 1.2228994040817392E-4, 0.0, 0.0, 0.04826552468506599, 0.0, 0.0, 0.7637392118452981, 0.0, 0.0, -1.2712453415205E-4, 0.0, 0.0, 0.0, 0.0, 0.016212768371996984, 0.0, 0.0, 0.0, -0.0019971445918045626, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.13962020109061735, -0.0529728016994851, 6.744245408114862E-4, 0.004840422593282837, -2.6506736356319046E-5, 0.9999882847346322, 0.0, 0.0, 2.4362693000930865E-4, 0.0, 0.0, -0.05167825390912521, 0.0, 0.0, -0.0017139268705377853, 0.0, 0.0, 0.0, 0.0, 0.2185848667522738, 0.0, 0.0, 0.0, -0.026926035977830574, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.12616062876024411, -0.001883031493817848, 0.9978865868463734, -0.002479410812757004, 0.1690730522901164, -1.410248284564283E-6, 0.9999999999999999, 0.0, 0.0, -4.23359807918362E-5, 0.0, 0.0, -0.10259116570094247, 0.0, 0.0, 0.0, 4.925496588283756E-19, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.015108762618625813, 1.037227819353895, -1.2266491355554789E-5, 0.067806461102902, 9.813193084443634E-4, 0.04227520063246482, -3.705769144237564E-21, 0.0, 0.0, 1.030706249829779, 0.0, 0.0, 4.9410939567605124E-6, 0.0, 0.0, 0.0, 1.0000000000000002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {1.0000000000000002, -2.6732326521326063E-17, -6.776263578034403E-21, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {1.023248887507697E-18, 1.0, 6.776263578034403E-21, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {1.3552527156068805E-20, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
      DMatrixRMaj JPrimaryMotionConstraints = new DMatrixRMaj(JPrimaryMotionConstraintsData);

      double[][] pPrimaryMotionConstraintsData = new double[][]{
            {-0.0014501147245202025}, 
            {-0.011980212855571541}, 
            {-0.023704737414711484}, 
            {-0.026118803489365676}, 
            {0.48902636136760047}, 
            {0.05229314150042668}, 
            {-0.002330395268424229}, 
            {-0.0058467208549515025}, 
            {0.02455270286856114}, 
            {0.0487777784703148}, 
            {-0.42098500299626435}, 
            {-0.1809994173835361}, 
            {-0.0077054083242214105}, 
            {-0.0016795662569441393}, 
            {6.92933129770321E-6}, 
            {1.527230088774106E-5}, 
            {2.1830359634291108E-7}, 
            {-7.327053256183001E-5}, 
            {3.333154844216624E-4}, 
            {-4.709694743225259E-5}, 
            {6.84360231212692E-6}, 
            {1.5372705949027826E-5}, 
            {7.685794044759115E-7}, 
            {-8.907718843837623E-5}, 
            {-3.333345035173473E-4}, 
            {-4.959264769504716E-5}, 
            {-0.08697928865430762}, 
            {0.10575496460695091}, 
            {-6.089406734865596E-4}, 
            {0.005462630681850569}, 
            {-0.022210425902852826}};
      DMatrixRMaj pPrimaryMotionConstraints = new DMatrixRMaj(pPrimaryMotionConstraintsData);

      double[][] centroidalMomentumAMatrixData = new double[][]{
            {0.027368778512757933, -0.005124532507787393, 7.629427951677898E-4, -0.16393673870175382, -0.7611530157399855, -0.007216491954633262, 0.13256882182513652, -0.004750565620989667, -0.003770097244967848, -0.00299789452960618, 1.0561267396826097, 1.0551334457345327, 0.017730098140217032, -0.19321701981218956, -0.4232805622752696, -1.1211126246257051E-4, 0.020891585521461448, 5.8660455703341304E-5, -0.15612754950254562, 0.29187272096546324, 4.350471533044089E-4, 4.963901208509411E-4, 0.35465951301760984, 0.13630534552954282, -8.820483812235037E-4, 7.691860569417269E-4, -1.0806224062196765, -1.0796452246472525, 0.00178370377200607, 0.0013020014832717873, 1.0165638305888756E-4, -1.0300837253197607E-4, 6.015551769002253E-4, 3.4335471957001336E-4}, 
            {0.006263972286946473, 0.022625334028931986, 1.8124661959398775E-4, 0.7173465579560518, -0.17414820885522186, 0.03008360186974377, 0.032281248421805554, 8.124415483579737E-4, -0.0029540519905342857, 0.012292313497080806, 0.2628213274571588, 0.2661371043938153, 0.004321932991614046, 1.250593392673386, 1.1839216037004157, 8.787850606533435E-5, -0.08539461920207567, 1.3176223132864312E-4, -0.3322363716133725, -0.20199014061435053, 2.335828035939598E-4, -3.807901152482933E-6, -0.9383846198033616, -1.0020477142864257, 3.1829498274080446E-5, 4.3204023676790716E-4, -0.268921700256821, -0.2721676086959204, -6.073193508580521E-4, 0.0013589261706641784, 2.1352426220572726E-5, -3.186009555872641E-5, -4.083975376876523E-4, 6.380304525541637E-4}, 
            {0.005331593420462876, 9.195793771427723E-5, 8.542395309160109E-4, 0.004494751338764933, -0.18721182385583188, 1.6013710367902156E-4, 0.04360231104322169, -0.0013044830920323168, -0.001305920224394955, -7.219099477297563E-6, 0.2608195454550491, 0.260766241086747, 0.0032286456872985337, 0.03641123718737558, -0.03478292312818856, 0.0041585509976696, -1.3501690207957026E-4, -0.0041549631579030424, -0.06285208967083827, 0.06215597759403542, 0.0017474929921141111, 0.0017445174116234373, 0.02631285716753588, -0.027196943270422897, -6.109314250117289E-4, 6.135029432135556E-4, -0.26544108134893474, -0.26538619902983235, -0.0015947679590180629, -0.0015953699408396771, -6.110201398920145E-5, 6.25621399193233E-5, -3.4189644837738644E-4, -3.4734329877433433E-4}, 
            {0.018322005008625603, 0.09260165567714637, 0.001231644380616366, 7.939061705169116, -1.9761866801928158, -0.5858554221889997, 0.3527227118995135, -0.007770911699740892, -0.024145075198828147, 0.015386199931676656, 3.0868417170601026, 2.9304163610126563, 0.002460128716302248, 11.771630895377092, 11.010572391941317, -0.009755218372677203, -0.6214426669866387, -0.008786640069481709, 0.24307013532445482, 1.7046163042965488, -0.005529514861712938, 4.387930449905746E-4, -12.068502938329562, -12.770194965102126, 6.543453168451895E-4, -0.0019729472815368633, -3.0978007432481767, -2.9415848039900014, 0.0038574927438451818, -0.003188873583453914, 3.4809653610631735E-5, -1.2698065677931167E-5, 8.268848864342076E-4, -7.901664382703052E-4}, 
            {-0.08239169027590112, 0.021324631260717365, -0.0050285820110479405, 1.8041630918892297, 8.614064548533403, -0.1520165841417361, -1.4469354769683866, 0.06578624041402249, 0.061346629114556295, 0.00375065750724262, -11.989221115695763, -12.027446292438457, -0.010019617219423106, 1.5741089976439322, 4.1818326600840905, -2.522406243550364E-4, -0.15115143049045174, -0.004264940336596171, 2.762237076270722, -2.2791517501137806, 0.009713431312175691, 0.011169481952450615, -4.35256771459186, -1.9135799688046014, -0.0055745157326203216, 0.005251260748150685, 12.034204934526215, 12.072377805829833, -5.212019647855364E-4, -0.002236408700922854, -1.0859408496698675E-4, 1.0993777475126522E-4, 1.2185348032470926E-4, -2.832907646035984E-4}, 
            {-3.647187903483253E-4, 0.011168807733183411, 7.09900900725399E-5, -0.9458208804572729, -0.026477705802655533, 1.6471589794594446, 0.0020273285240000123, 0.0024525149208914853, -0.002712383656097989, -0.008127415520891565, -0.16150611270529194, 0.18117064317530518, -4.867569791253047E-5, 1.8526644240202417, 1.8770160398727662, -0.0011915721691277978, -0.008520091887737084, -0.0011906000579936969, -6.339885171542983, -6.358528082347362, 0.0012434937656160955, -0.0012534864511113417, 4.467939257326714, 4.4622115805972635, -0.0011990320503862443, -0.0012005342035749267, 0.16162678490031424, -0.18100114544881593, 0.0044886501383102, -0.004485498135766024, 5.63720046742037E-6, 1.3241993973824969E-5, 0.001297534110449372, -0.0012936278528314755}};
      DMatrixRMaj centroidalMomentumAMatrix = new DMatrixRMaj(centroidalMomentumAMatrixData);

      double[][] momentumDotEquationRightHandSideData = new double[][]{
            {-0.019632871937077027}, 
            {-0.013049884842859067}, 
            {0.43878547383100497}, 
            {-0.27216479242510544}, 
            {0.2874138570654739}, 
            {0.014953015847885824}};
      DMatrixRMaj momentumDotEquationRightHandSide = new DMatrixRMaj(momentumDotEquationRightHandSideData);

      double[][] momentumSubspaceData = new double[][]{
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {1.0, 0.0, 0.0}, 
            {0.0, 1.0, 0.0}, 
            {0.0, 0.0, 1.0}};
      DMatrixRMaj momentumSubspace = new DMatrixRMaj(momentumSubspaceData);

      TypicalMotionConstraintsTestHelper helper = new TypicalMotionConstraintsTestHelper(JPrimaryMotionConstraintsData, pPrimaryMotionConstraintsData, centroidalMomentumAMatrixData, momentumDotEquationRightHandSideData, momentumSubspaceData);
      
      double minimumSingularValueOfCombinedConstraints = helper.getMinimumSingularValueOfCombinedConstraints();
      System.out.println("minimumSingularValueOfCombinedConstraints = " + minimumSingularValueOfCombinedConstraints);
      
      DMatrixRMaj combinedSolution = helper.getCombinedSolution();
      System.out.println("combinedSolution = " + combinedSolution);

   }

	@Test
   public void testDoubleSupportKneesStraightNoSingularity()
   {
      double[][] JPrimaryMotionConstraintsData = new double[][]{
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.999153741501768, 0.012095024654239407, 0.03931299049463765, 0.0, 0.0, 0.0, 0.0, 0.03931299049463765, 0.0, 0.0, 0.9992234056104683, 0.0, 0.0, 9.64939933512099E-18, 0.0, 0.0, 0.0, 0.0, 9.64939933512099E-18, 0.0, 0.0, 0.0, 9.64939933512099E-18, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.012332294973871867, 0.9999071415257151, 0.005798519341208985, 0.0, 0.0, 0.0, 0.0, 0.005798519341208985, 0.0, 0.0, 0.002433665683358734, 0.0, 0.0, 0.9980908016968247, 0.0, 0.0, 0.0, 0.0, 0.9980908016968247, 0.0, 0.0, 0.0, 0.9980908016968247, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.03923920671593076, -0.006278431690024209, 0.9992101210215086, 0.0, 0.0, 0.0, 0.0, 0.9992101210215086, 0.0, 0.0, -0.03932763597725099, 0.0, 0.0, 0.06176367515125119, 0.0, 0.0, 0.0, 0.0, 0.06176367515125119, 0.0, 0.0, 0.0, 0.06176367515125119, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.016792863961557342, -0.9112573227655211, -0.1464394593257991, 0.999153741501768, 0.012095024654239407, 0.03931299049463765, 0.0, -0.05751477633214178, 0.0, 0.0, -1.6305560078502042E-4, 0.0, 0.0, -0.8612266487730393, 0.0, 0.0, 0.0, 0.0, -0.4888688954009902, 0.0, 0.0, 0.0, -0.06687208371368726, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9109315942547055, 0.010949372387894849, 0.049240410988834304, -0.012332294973871867, 0.9999071415257151, 0.005798519341208985, 0.0, 0.048142836736159704, 0.0, 0.0, 0.9104779479735339, 0.0, 0.0, -0.0022036627857762612, 0.0, 0.0, 0.0, 0.0, 0.0015663018405066183, 0.0, 0.0, 0.0, 0.0016676192290837825, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.14130702938244416, -0.011678734904633527, 0.005475776798010841, -0.03923920671593076, -0.006278431690024209, 0.9992101210215086, 0.0, 0.0019834874002930057, 0.0, 0.0, 0.05219917021977317, 0.0, 0.0, 0.03561082709438376, 0.0, 0.0, 0.0, 0.0, -0.025311179360070598, 0.0, 0.0, 0.0, -0.026948451645814265, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9994278346039411, 0.009433214545792053, 0.032481038810525414, 0.0, 0.0, 0.0, 0.0, 0.0, 0.032481038810525414, 0.0, 0.0, 0.9994708326929086, 0.0, 0.0, -2.677979366039196E-17, 0.0, 0.0, 0.0, 0.0, -2.677979366039196E-17, 0.0, 0.0, 0.0, -2.677979366039196E-17, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.009617040831016964, 0.9999385857656661, 0.005507923620949952, 0.0, 0.0, 0.0, 0.0, 0.0, 0.005507923620949952, 0.0, 0.0, -0.0019225077338376475, 0.0, 0.0, 0.998251853244122, 0.0, 0.0, 0.0, 0.0, 0.998251853244122, 0.0, 0.0, 0.0, 0.998251853244122, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.03242708658717822, -0.005817143654124562, 0.9994571751181613, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9994571751181613, 0.0, 0.0, -0.032470887886808186, 0.0, 0.0, -0.059103616595567655, 0.0, 0.0, 0.0, 0.0, -0.059103616595567655, 0.0, 0.0, 0.0, -0.059103616595567655, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.004254126129127126, -0.9117834271555852, 0.13390448032349542, 0.9994278346039411, 0.009433214545792053, 0.032481038810525414, 0.0, 0.0, 0.04495540304374468, 0.0, 0.0, 1.288080181671337E-4, 0.0, 0.0, -0.861310458141067, 0.0, 0.0, 0.0, 0.0, -0.4888494915325795, 0.0, 0.0, 0.0, -0.06688287416735617, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9098801381119346, 0.008481038489013428, 0.04899065883904859, -0.009617040831016964, 0.9999385857656661, 0.005507923620949952, 0.0, 0.0, 0.049846575473009094, 0.0, 0.0, 0.9104086512036412, 0.0, 0.0, 0.001660671546796979, 0.0, 0.0, 0.0, 0.0, -0.0019095138846035602, 0.0, 0.0, 0.0, -0.0015957976480803278, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.1387316237027302, -0.020719972256911058, -0.004621702204269114, -0.03242708658717822, -0.005817143654124562, 0.9994571751181613, 0.0, 0.0, -0.0017356914980102504, 0.0, 0.0, -0.049937895796666625, 0.0, 0.0, 0.02804851115226343, 0.0, 0.0, 0.0, 0.0, -0.03225142358283077, 0.0, 0.0, 0.0, -0.026952800037591283, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.10403079028796303, -0.005879294095103906, 0.9982681294845341, 0.0027742116314638248, 0.13854964123729469, -3.8774206126244786E-4, 1.0000000000000002, 0.0, 0.0, -0.005194890652248281, 0.0, 0.0, -0.03893989159236364, 0.0, 0.0, 0.0, 2.9680034471790684E-18, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.009968886741645101, 1.0353150767034174, 1.0559545871763337E-5, 0.0638150334335171, -8.44763669746873E-4, 0.04807119952622117, -7.253312533928025E-17, 0.0, 0.0, 1.0290197865055377, 0.0, 0.0, 3.198390015893849E-4, 0.0, 0.0, 0.0, 1.0000000000000002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {1.0, -8.836247705756861E-18, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-3.65376132127615E-17, 0.9999999999999999, -8.673617379884035E-19, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 8.673617379884035E-19, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
      DMatrixRMaj JPrimaryMotionConstraints = new DMatrixRMaj(JPrimaryMotionConstraintsData);

      double[][] pPrimaryMotionConstraintsData = new double[][]{
            {4.994838130596103}, 
            {1.4490112596955034}, 
            {3.945794852572223}, 
            {4.571930137797139}, 
            {-0.14895872794496545}, 
            {-3.2070965577587174}, 
            {4.911353509110361}, 
            {-2.2988715881790736}, 
            {3.9811299517924246}, 
            {-5.2122223401802446}, 
            {-0.9335580006605622}, 
            {3.6682232884804913}, 
            {36.066606118699944}, 
            {2.2126396677861946}, 
            {-1.0414858587932723}, 
            {0.10454537696934271}, 
            {-0.14719861859217903}, 
            {0.1303338833750478}, 
            {-0.9006937859588185}, 
            {-1.059908191893325}, 
            {-0.48549853752688354}, 
            {0.03619762153629377}, 
            {-0.04030089391995188}, 
            {0.28253936759651654}, 
            {-0.5595794921220968}, 
            {-0.30972589006820905}, 
            {-0.17971915988497833}, 
            {2.095826901243708}, 
            {-1.9724919301947594}, 
            {-38.292008860356454}, 
            {11.342072346006573}};
      DMatrixRMaj pPrimaryMotionConstraints = new DMatrixRMaj(pPrimaryMotionConstraintsData);

      double[][] centroidalMomentumAMatrixData = new double[][]{
            {0.021313958450514817, -0.004484159088393217, 0.0012020412770974201, -0.20835471509748676, -0.9530726756076926, 0.002212263396226523, 0.13483707191236785, 0.042957426074269875, 0.03597963236516343, -0.0024998211313458914, 1.1018529866627338, 1.1055211995680907, 0.018227928071065734, -0.5067334763507748, -0.07535694522120472, -1.2172801735415737E-4, 0.01972404040408639, 1.443093268445781E-4, 0.4282805137330154, -0.3524871783562849, 2.2236043595482143E-4, 2.3168344118549978E-4, 0.08149960614872129, 0.4319537898417685, -7.552512048943202E-4, 6.584396377653535E-4, -1.1211118152693225, -1.1245374561215333, 0.0017994359803011968, 0.0013782712643602589, 1.0400999571734035E-4, -1.0591366558632881E-4, 5.883932619950583E-4, 3.713873534066485E-4}, 
            {0.003641060325675063, 0.025091663133026916, 2.1990917749315373E-4, 1.1090595151392257, -0.16527061862038026, -0.052176300395349276, 0.02052741413725167, 0.010454710630017702, 0.004836549787616529, 0.01310342710874108, 0.21495356338994467, 0.19875077988081843, 0.003478221268393314, 1.4780180641411613, 1.566777255059663, -8.531476751624562E-5, -0.10739066402715036, -2.7660437133136776E-5, -0.21626668210905814, -0.3759587995582071, 4.10847231038619E-5, 4.720971574607405E-5, -1.283886851179894, -1.2127808844019028, 1.303357563179271E-4, 3.999920628971098E-4, -0.21858860958225063, -0.20216912143176752, -7.89571496283582E-4, 0.0013958756232214822, 1.542357623798868E-5, -2.8167786274751416E-5, -4.699165852843123E-4, 6.522879298563336E-4}, 
            {0.0014054432957404225, -3.440431894474736E-5, 7.525818330485292E-4, -0.0023190570033028517, -0.012004427050124524, 5.548643802784163E-5, 0.013381912408184947, -7.495329951710995E-5, -1.6629759265377814E-4, -4.1887715758521457E-5, 0.011935083829166011, 0.01194722767739149, 0.0020301699012296556, 0.03655731553245753, -0.0407514433246522, 0.004245031513648246, 2.2637793906770805E-4, -0.0042451997871287795, -0.07035912972510586, 0.06691399202510906, 0.002202464251062562, 0.0021910939154887897, 0.03371248334355055, -0.026007315737846832, -8.446936342635789E-4, 8.453669171163493E-4, -0.013272146535395416, -0.013278650505202052, -0.0016612338771016953, -0.0016583699129099666, -6.851277602435941E-5, 6.898087020916538E-5, -3.4805038267481103E-4, -3.5214375531197433E-4}, 
            {-0.0018360428035244004, 0.10158611338073538, 0.0016200038639919178, 11.368845900643201, -1.5656479516616675, -0.38545150999972894, 0.18269440056874567, 0.08875363458853, 0.053905102636877096, 0.015027458476142074, 2.0542120141039475, 1.909018847094783, 0.0020885165458266303, 12.832950046104626, 13.727390045565658, -0.00974622657723914, -0.8266415607329328, -0.009009690986736957, 2.1924821060555737, 0.5808635426511901, -0.0050404718995333666, 9.977907024960747E-4, -15.08533163000594, -14.366773227274711, 3.7347781327980986E-4, -0.001706021051216013, -2.0490533497209817, -1.902869156100214, 0.003838991263213961, -0.003304493844680079, 2.9074608111214558E-5, -8.388493193310112E-6, 8.331160643515979E-4, -8.052027979411621E-4}, 
            {-0.00799827712254256, 0.018459188354027312, -0.009056449804633906, 2.1315299147655606, 9.260457728970014, 0.0017337034597680436, -1.2708395806695838, -0.4164457350156034, -0.3461302359727172, 0.0028818479553116456, -10.76465585797984, -10.797849377099123, -0.01087670013976243, 4.608952286523809, 0.45636395746170355, 2.434987076581303E-4, -0.15001536522756176, -0.003810263166421161, -3.3115694796539974, 4.199437648473184, 0.010001929682070827, 0.011143017769697118, -1.2963021286048515, -4.66686215167411, -0.005591061158770578, 0.005339410450218499, 10.746878638868264, 10.77756128465248, -7.059024348280129E-4, -0.002074583062527203, -1.1480130239905449E-4, 1.1208489250264264E-4, 8.039094031810536E-5, -2.393723840149504E-4}, 
            {-1.7803278550712672E-4, -0.006881801722621973, 6.11155430134533E-5, -0.9408922683264915, -0.002198114659436212, 0.18563237228741364, 0.0027537392214866353, -0.001558911115015283, 4.2653670766002914E-4, -0.007919809856985406, -0.026795070663612117, 0.004710764973457237, -1.0459122358879914E-4, -1.6080196998834748, -1.6465631780461851, -0.0011901586901152204, 0.06638171608857862, -0.0011944291166552932, 0.8285913566617787, 0.9008039893816359, 0.0012375924269162203, -0.001256940132312168, 0.7876964955876703, 0.753928047430594, -0.0011911029413977908, -0.0011997409497353489, 0.027442983194358835, -0.004870038191755142, 0.004479165508469224, -0.004479849634325471, 3.823156890340401E-6, 1.1796766609736702E-5, 0.0012972725989372175, -0.0012937269132908206}};
      DMatrixRMaj centroidalMomentumAMatrix = new DMatrixRMaj(centroidalMomentumAMatrixData);

      double[][] momentumDotEquationRightHandSideData = new double[][]{
            {-39.26720857891175}, 
            {195.18810829641387}, 
            {-1.0332191277276794}, 
            {1358.446408026797}, 
            {205.26126736307407}, 
            {61.640608531934504}};
      DMatrixRMaj momentumDotEquationRightHandSide = new DMatrixRMaj(momentumDotEquationRightHandSideData);

//      double[][] momentumSubspaceData = new double[][]{
//            {0.0, 0.0}, 
//            {0.0, 0.0}, 
//            {0.0, 0.0}, 
//            {1.0, 0.0}, 
//            {0.0, 1.0}, 
//            {0.0, 0.0}};
//      DMatrixRMaj momentumSubspace = new DMatrixRMaj(momentumSubspaceData);

      double[][] momentumSubspaceData = new double[][]{
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {1.0, 0.0, 0.0}, 
            {0.0, 1.0, 0.0}, 
            {0.0, 0.0, 1.0}};
      DMatrixRMaj momentumSubspace = new DMatrixRMaj(momentumSubspaceData);
  
      
      TypicalMotionConstraintsTestHelper helper = new TypicalMotionConstraintsTestHelper(JPrimaryMotionConstraintsData, pPrimaryMotionConstraintsData, centroidalMomentumAMatrixData, momentumDotEquationRightHandSideData, momentumSubspaceData);
      
      double minimumSingularValueOfCombinedConstraints = helper.getMinimumSingularValueOfCombinedConstraints();
      System.out.println("minimumSingularValueOfCombinedConstraints = " + minimumSingularValueOfCombinedConstraints);
      
      DMatrixRMaj combinedSolution = helper.getCombinedSolution();
      System.out.println("combinedSolution = " + combinedSolution);

   }

	@Test
   public void testDoubleSupportKneesStraightNoSingularityTwo()
   {
      double[][] JPrimaryMotionConstraintsData = new double[][]{
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9999808721064536, 0.004680924623378534, -0.004042816578409051, 0.0, 0.0, 0.0, 0.0, -0.004042816578409051, 0.0, 0.0, 0.9999917981422252, 0.0, 0.0, -1.0679391398982219E-17, 0.0, 0.0, 0.0, 0.0, -1.0679391398982219E-17, 0.0, 0.0, 0.0, -1.0679391398982219E-17, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.0046820327452754005, 0.9999890041976395, -2.646756225926094E-4, 0.0, 0.0, 0.0, 0.0, -2.646756225926094E-4, 0.0, 0.0, -2.445517824178844E-4, 0.0, 0.0, 0.9981754002614374, 0.0, 0.0, 0.0, 0.0, 0.9981754002614374, 0.0, 0.0, 0.0, 0.9981754002614374, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.004041533197757968, 2.835991595087134E-4, 0.9999917927567846, 0.0, 0.0, 0.0, 0.0, 0.9999917927567846, 0.0, 0.0, 0.00404275187275507, 0.0, 0.0, 0.060381042661742323, 0.0, 0.0, 0.0, 0.0, 0.060381042661742323, 0.0, 0.0, 0.0, 0.060381042661742323, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.003690810197001046, -0.9091179443514978, -0.1396978964393168, 0.9999808721064536, 0.004680924623378534, -0.004042816578409051, 0.0, -0.050699598821842454, 0.0, 0.0, 1.6384969421962936E-5, 0.0, 0.0, -0.8607334917802479, 0.0, 0.0, 0.0, 0.0, -0.48877897300543627, 0.0, 0.0, 0.0, -0.0668777518175163, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9090912303941616, 0.004256802670007533, 0.0013637595144449614, -0.0046820327452754005, 0.9999890041976395, -2.646756225926094E-4, 0.0, 9.470586001154527E-4, 0.0, 0.0, 0.9091247251644246, 0.0, 0.0, -0.0027521450613480587, 0.0, 0.0, 0.0, 0.0, 0.001079000436042747, 0.0, 0.0, 0.0, 0.0016302881518670423, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.13996057483468557, -0.004383975533060215, -5.644166500999105E-4, 0.004041533197757968, 2.835991595087134E-4, 0.9999917927567846, 0.0, -2.047201954994511E-4, 0.0, 0.0, 0.050941349710886194, 0.0, 0.0, 0.04549645678658032, 0.0, 0.0, 0.0, 0.0, -0.01783724898827648, 0.0, 0.0, 0.0, -0.026950735807058812, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9999900416269175, 0.001991644540908621, -0.003993744986636796, 0.0, 0.0, 0.0, 0.0, 0.0, -0.003993744986636796, 0.0, 0.0, 0.9999919946166462, 0.0, 0.0, 1.2088854223213374E-17, 0.0, 0.0, 0.0, 0.0, 1.2088854223213374E-17, 0.0, 0.0, 0.0, 1.2088854223213374E-17, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.001993017745586869, 0.9999979561759444, -3.3988851055409195E-4, 0.0, 0.0, 0.0, 0.0, 0.0, -3.3988851055409195E-4, 0.0, 0.0, 2.450255238518339E-4, 0.0, 0.0, 0.9981233210923524, 0.0, 0.0, 0.0, 0.0, 0.9981233210923524, 0.0, 0.0, 0.0, 0.9981233210923524, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0039930598870281594, 3.4784473044719685E-4, 0.9999919672061279, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9999919672061279, 0.0, 0.0, 0.003993828378137633, 0.0, 0.0, -0.06123590361522282, 0.0, 0.0, 0.0, 0.0, -0.06123590361522282, 0.0, 0.0, 0.0, -0.06123590361522282, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.002373556082626309, -0.9090641104861655, 0.14097041117232978, 0.9999900416269175, 0.001991644540908621, -0.003993744986636796, 0.0, 0.0, 0.051971297467534115, 0.0, 0.0, -1.6416710098078957E-5, 0.0, 0.0, -0.8607419612617091, 0.0, 0.0, 0.0, 0.0, -0.4887814207265446, 0.0, 0.0, 0.0, -0.06687426251318761, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9091248377664932, 0.001812288315459485, 0.0011258893040104415, -0.001993017745586869, 0.9999979561759444, -3.3988851055409195E-4, 0.0, 0.0, 0.0013032678833676797, 0.0, 0.0, 0.9090963669123516, 0.0, 0.0, 0.0027718866344262822, 0.0, 0.0, 0.0, 0.0, -0.0011113370430741927, 0.0, 0.0, 0.0, -0.0016533693976110155, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.1406516624184408, -0.005036840641669123, 5.633870752944546E-4, 0.0039930598870281594, 3.4784473044719685E-4, 0.9999919672061279, 0.0, 0.0, 2.0800474534894852E-4, 0.0, 0.0, -0.05166352064299081, 0.0, 0.0, 0.04518076046741446, 0.0, 0.0, 0.0, 0.0, -0.018114396208736866, 0.0, 0.0, 0.0, -0.026949329669493517, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.12539502276187758, 1.5270000626743403E-4, 0.9978911441829461, 3.2462644074127296E-4, 0.16870846536427575, -4.065276157804317E-6, 0.9999999999999994, 0.0, 0.0, -7.877951046523898E-5, 0.0, 0.0, -0.10626881923893229, 0.0, 0.0, 0.0, -2.2764010457459322E-20, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0019133830817925168, 1.0352194887573383, 1.7573503964610434E-6, 0.06585732106107792, -1.4058803171676697E-4, 0.04526218447123459, 1.500690919628404E-18, 0.0, 0.0, 1.0287283084261905, 0.0, 0.0, 1.1318749154717906E-5, 0.0, 0.0, 0.0, 0.9999999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {1.0, -1.2389392100811041E-17, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-1.2389339161251837E-17, 1.0000000000000002, -5.421010862427522E-20, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-2.1684043449710089E-19, -5.421010862427522E-20, 1.0000000000000002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
      DMatrixRMaj JPrimaryMotionConstraints = new DMatrixRMaj(JPrimaryMotionConstraintsData);

      double[][] pPrimaryMotionConstraintsData = new double[][]{
            {-0.18359246881084257}, 
            {-0.1270518383978717}, 
            {-0.20798551885700634}, 
            {-0.19863038680654155}, 
            {0.3355645483842893}, 
            {0.020848709701208}, 
            {-0.21158504067660444}, 
            {0.06679005544514915}, 
            {-0.12213659521653097}, 
            {0.2173754916499151}, 
            {-0.277084387899707}, 
            {-0.14491654100991824}, 
            {-0.0681840925701836}, 
            {0.0221912860170548}, 
            {8.655716542765888E-5}, 
            {-2.466884105986132E-6}, 
            {-2.878035946432554E-5}, 
            {-4.942450340108336E-5}, 
            {1.2787980325738009E-4}, 
            {-7.58820059077651E-4}, 
            {1.122968151767502E-4}, 
            {-7.11080915106924E-6}, 
            {-3.233404916432359E-5}, 
            {-5.4235653683414565E-5}, 
            {5.0707076296611885E-5}, 
            {-7.842338319093387E-4}, 
            {0.05858547497905853}, 
            {0.9294862317343395}, 
            {-0.01279140741042857}, 
            {0.16939699829925056}, 
            {-0.42108743815244987}};
      DMatrixRMaj pPrimaryMotionConstraints = new DMatrixRMaj(pPrimaryMotionConstraintsData);

      double[][] centroidalMomentumAMatrixData = new double[][]{
            {0.0207261222862488, -0.005500599008465904, 0.001456074596287772, -0.24317169347232678, -0.9161454926444003, 0.025730266566244837, 0.1572370171224416, -0.0061215170062449125, -0.005285527765533078, -0.0028550722928337505, 1.064543601192483, 1.0687596587543435, 0.018050039494921025, -0.5328722274284547, -0.1443470156863394, -1.157960143447809E-4, 0.023217621331733166, 1.3223302827114876E-4, 0.4168837194174082, -0.2892373259114869, 2.5574216609262933E-4, 2.579040063045145E-4, 0.12304986491126393, 0.4362634559077009, -7.689061366911362E-4, 6.537398861421669E-4, -1.0824944244977146, -1.0867082390774745, 0.001808538431776754, 0.0013273000694762976, 1.0086736084100201E-4, -1.0235254268234386E-4, 6.009207665011321E-4, 3.5290771340186337E-4}, 
            {0.004489342961039186, 0.024748049767478342, 3.232508883758225E-4, 1.0942459285817772, -0.20079351535287432, -0.1113986815401328, 0.03409202106808464, 4.306163014629022E-4, -0.003018229822419584, 0.0130225384614198, 0.24763465542713464, 0.22878063661415698, 0.003951113614606833, 1.4624944844044157, 1.5508056363472804, -7.00751654692821E-5, -0.10563975030420245, -1.1471029550051028E-5, -0.20842105127474467, -0.369001719559641, 5.508977090174365E-5, 5.911523411813096E-5, -1.2752137942460666, -1.2039409755055777, 1.0884813632075968E-4, 4.174186227416697E-4, -0.2515921132262559, -0.2327350160196665, -7.489238990104031E-4, 0.0014347432767444815, 1.849684096607389E-5, -2.9417950152472278E-5, -4.5685165233213203E-4, 6.647204729724779E-4}, 
            {0.002198386017723636, 1.762652545228544E-6, 8.059447547800626E-4, 6.598755905477595E-4, -0.026341968719544306, -5.457936324366249E-5, 0.016383429016699372, -7.739483134051782E-4, -7.718532927866267E-4, -6.511982887029587E-6, 0.028059658472118523, 0.02803865093387492, 0.0027791607970402135, 0.03855047950711828, -0.03633300853956275, 0.004255343491859866, -4.377794921604E-5, -0.0042483902880197395, -0.06729837907723792, 0.06589035724493021, 0.002170658940275946, 0.0021661950873693822, 0.028693200161994146, -0.02949008310405971, -8.380834373610391E-4, 8.417212526106166E-4, -0.0301210391976336, -0.030099428315202247, -0.0016688323243303708, -0.0016670016792317997, -7.174066729130918E-5, 7.299305656983425E-5, -3.4718312755836143E-4, -3.514833038188792E-4}, 
            {1.5892353530232864E-4, 0.1003601366814468, 0.0026668080127258743, 11.254674196173838, -1.9664047444091393, -1.0130165452395714, 0.3232914097955279, -0.004567352264200977, -0.01952828587966154, 0.014876391434338877, 2.4348867544972195, 2.2608063313594657, 0.002191502327608446, 12.74896721202979, 13.612110724751076, -0.009791726836215413, -0.8125521165206817, -0.008891137981444504, 2.198645346730547, 0.6288893253491684, -0.005223861984795623, 5.995439150662887E-4, -15.000004873889909, -14.30286630227092, 5.195969389417599E-4, -0.0018355952484224848, -2.427826986384706, -2.2540541609347953, 0.0038823678297318456, -0.003284004889836601, 3.5497850500806374E-5, -1.3938039475458194E-5, 8.305738542238386E-4, -7.980721018737781E-4}, 
            {-0.003738025032564664, 0.022276628216117252, -0.011918625402208245, 2.5025112574024573, 8.984533542065675, -0.23455158454721586, -1.4944756477326986, 0.055465198145206195, 0.05154718331575485, 0.003255771076609376, -10.49970554477746, -10.538664081719979, -0.009924199306162798, 4.858426906666547, 1.070702353882251, 4.3219773736095696E-5, -0.1788322346554053, -0.004124230553713279, -3.1176869167343484, 3.7674931307137918, 0.009939549486999487, 0.01121170826325487, -1.7740752898102101, -4.829936933997823, -0.005559347506992095, 0.005271903715272132, 10.47030284720178, 10.509163435565103, -5.957301232815897E-4, -0.0021564596791129303, -1.2638606416720264E-4, 1.2676196568575662E-4, 1.0121775283554408E-4, -2.6205573276788616E-4}, 
            {-1.892481970695081E-4, -0.006845331358779431, 1.3581750032015363E-5, -1.0895113284570228, -0.008466393814761212, 0.2472455285373446, 0.001737752798319347, -3.478047187843577E-4, 2.850220967999851E-4, -0.008168663239364275, -0.011720677888958489, 0.02389660830285495, -5.551134436277357E-5, -1.7629256354418816, -1.763724648421978, -0.0011755792865963237, 0.07601785399699018, -0.001173431718354338, 0.7442063683677906, 0.7459586246259338, 0.001270608597695419, -0.0012751727061916112, 1.026270793764793, 1.0253080585871348, -0.001183815085620447, -0.0011822560157900433, 0.012320709546064476, -0.02404344484168246, 0.004455535187760766, -0.004453703378166877, 3.770908508058566E-6, 1.1484368337969082E-5, 0.0012971141183869984, -0.0012934154066409304}};
      DMatrixRMaj centroidalMomentumAMatrix = new DMatrixRMaj(centroidalMomentumAMatrixData);

      double[][] momentumDotEquationRightHandSideData = new double[][]{
            {0.5055343078043402}, 
            {-1.5674077735872012}, 
            {-0.17428218022863748}, 
            {-15.500961948642885}, 
            {-5.483357904786595}, 
            {0.7566100210066392}};
      DMatrixRMaj momentumDotEquationRightHandSide = new DMatrixRMaj(momentumDotEquationRightHandSideData);

//      double[][] momentumSubspaceData = new double[][]{
//            {0.0, 0.0}, 
//            {0.0, 0.0}, 
//            {0.0, 0.0}, 
//            {1.0, 0.0}, 
//            {0.0, 1.0}, 
//            {0.0, 0.0}}; 
      
//      double[][] momentumSubspaceData = new double[][]{
//            {0.0}, 
//            {0.0}, 
//            {0.0}, 
//            {0.0}, 
//            {0.0}, 
//            {0.0}};
      
      double[][] momentumSubspaceData = new double[][]{
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {1.0, 0.0, 0.0}, 
            {0.0, 1.0, 0.0}, 
            {0.0, 0.0, 1.0}};
      DMatrixRMaj momentumSubspace = new DMatrixRMaj(momentumSubspaceData);
      
      
      TypicalMotionConstraintsTestHelper helper = new TypicalMotionConstraintsTestHelper(JPrimaryMotionConstraintsData, pPrimaryMotionConstraintsData, centroidalMomentumAMatrixData, momentumDotEquationRightHandSideData, momentumSubspaceData);
      
      double[] singularValuesOfCombinedConstraints = helper.getSingularValuesOfCombinedConstraints();
      System.out.println("singularValuesOfCombinedConstraints = ");
      for (int i=0; i<singularValuesOfCombinedConstraints.length; i++)
      {
         System.out.print("  " + singularValuesOfCombinedConstraints[i]);
      }
      System.out.println();

      double minimumSingularValueOfCombinedConstraints = helper.getMinimumSingularValueOfCombinedConstraints();
      System.out.println("minimumSingularValueOfCombinedConstraints = " + minimumSingularValueOfCombinedConstraints);
      
      DMatrixRMaj combinedSolution = helper.getCombinedSolution();
      System.out.println("combinedSolution = " + combinedSolution);

   }

	@Test
   public void testDoubleSupportKneesStraightThree()
   {
      double[][] JPrimaryMotionConstraintsData = new double[][]{
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9959555918356301, 0.08980383992246087, 0.0027801846199151884, 0.0, 0.0, 0.0, 0.0, 0.0027801846199151884, 0.0, 0.0, 0.9999961244209659, 0.0, 0.0, 7.068998164605489E-17, 0.0, 0.0, 0.0, 0.0, 7.068998164605489E-17, 0.0, 0.0, 0.0, 7.068998164605489E-17, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.08980891803718298, 0.9959574614422466, 0.0017587605028716526, 0.0, 0.0, 0.0, 0.0, 0.0017587605028716526, 0.0, 0.0, 1.4247592890974977E-4, 0.0, 0.0, 0.9986896965975492, 0.0, 0.0, 0.0, 0.0, 0.9986896965975492, 0.0, 0.0, 0.0, 0.9986896965975492, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.002611002169729655, -0.0020013327301928764, 0.9999945886528455, 0.0, 0.0, 0.0, 0.0, 0.9999945886528455, 0.0, 0.0, -0.0027804394720495483, 0.0, 0.0, 0.051175090717021754, 0.0, 0.0, 0.0, 0.0, 0.051175090717021754, 0.0, 0.0, 0.0, 0.051175090717021754, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.08214295715843681, -0.906861710730173, -0.1334708518924271, 0.9959555918356301, 0.08980383992246087, 0.0027801846199151884, 0.0, -0.044830804219055956, 0.0, 0.0, -9.545887236957566E-6, 0.0, 0.0, -0.8615095420361493, 0.0, 0.0, 0.0, 0.0, -0.48634657810429127, 0.0, 0.0, 0.0, -0.0669122096720358, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9071290604826547, 0.08179795787228292, 5.645842019020471E-4, -0.08980891803718298, 0.9959574614422466, 0.0017587605028716526, 0.0, -0.007428409503407253, 0.0, 0.0, 0.9106536465195881, 0.0, 0.0, -0.003061334013642097, 0.0, 0.0, 0.0, 0.0, -9.96020905632485E-4, 0.0, 0.0, 0.0, 0.0013817274493595922, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.13115962667379025, 0.013802088478606228, 3.700826438891152E-4, -0.002611002169729655, -0.0020013327301928764, 0.9999945886528455, 0.0, 1.3770345078317398E-4, 0.0, 0.0, 0.04323071052980196, 0.0, 0.0, 0.05974239995340368, 0.0, 0.0, 0.0, 0.0, 0.019437499809259372, 0.0, 0.0, 0.0, -0.026964621808133832, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9960589178139352, 0.0886326391970352, 0.0032996230542836564, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0032996230542836564, 0.0, 0.0, 0.9999945297051042, 0.0, 0.0, 7.914675859144182E-18, 0.0, 0.0, 0.0, 0.0, 7.914675859144182E-18, 0.0, 0.0, 0.0, 7.914675859144182E-18, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.08863915995899713, 0.9960620189666727, 0.001885124345727707, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001885124345727707, 0.0, 0.0, -2.365416534238407E-4, 0.0, 0.0, 0.9974396340298856, 0.0, 0.0, 0.0, 0.0, 0.9974396340298856, 0.0, 0.0, 0.0, 0.9974396340298856, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.0031195456553023237, -0.0021701707314632918, 0.9999927793708816, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9999927793708816, 0.0, 0.0, -0.003299182916119085, 0.0, 0.0, -0.07151347052358582, 0.0, 0.0, 0.0, 0.0, -0.07151347052358582, 0.0, 0.0, 0.0, -0.07151347052358582, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.08027314796408982, -0.9076036580765882, 0.1474479543809741, 0.9960589178139352, 0.0886326391970352, 0.0032996230542836564, 0.0, 0.0, 0.058798710695533864, 0.0, 0.0, 1.5848290779479015E-5, 0.0, 0.0, -0.8628900604452912, 0.0, 0.0, 0.0, 0.0, -0.4866371094595932, 0.0, 0.0, 0.0, -0.06682845548000234, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.9072847895423517, 0.08073787625671877, 5.461481960704665E-4, -0.08863915995899713, 0.9960620189666727, 0.001885124345727707, 0.0, 0.0, 0.008435033432421201, 0.0, 0.0, 0.9110371691944601, 0.0, 0.0, 0.0031755861274677067, 0.0, 0.0, 0.0, 0.0, 0.0011406328149240013, 0.0, 0.0, 0.0, -0.0019308637041368175, 0.0, 0.0, 0.0, 0.06700000000000006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.14879625088717077, -0.010771285652306516, -4.875557473024405E-4, -0.0031195456553023237, -0.0021701707314632918, 0.9999927793708816, 0.0, 0.0, -2.099161839805334E-4, 0.0, 0.0, -0.06051499396160456, 0.0, 0.0, 0.04429173191597649, 0.0, 0.0, 0.0, 0.0, 0.015909063972850163, 0.0, 0.0, 0.0, -0.026930870118806922, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-0.1403266573041171, -0.0016309641509496104, 0.9978553172064734, 0.0012471354091592585, 0.17157462348207433, -1.1159415378663064E-4, 0.9999999999999993, 0.0, 0.0, -0.00175712579361639, 0.0, 0.0, -0.10741418018192332, 0.0, 0.0, 0.0, 1.060485249962384E-18, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-2.683308262272203E-4, 1.0425793619487145, 6.7396643647617765E-6, 0.06826596943240926, -5.391731491818587E-4, 0.04153749275116169, -1.1445109183300106E-17, 0.0, 0.0, 1.0310031139198652, 0.0, 0.0, 2.971158695831626E-4, 0.0, 0.0, 0.0, 1.0000000000000002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {1.0, -2.8990549652725683E-17, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {-1.2349740370967699E-18, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 
            {0.0, 4.3368086899420177E-19, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
      DMatrixRMaj JPrimaryMotionConstraints = new DMatrixRMaj(JPrimaryMotionConstraintsData);

      double[][] pPrimaryMotionConstraintsData = new double[][]{
            {-1.310763246533376}, 
            {1.8733872563939082}, 
            {0.4845485174526627}, 
            {0.026321675882858164}, 
            {-21.86625281561411}, 
            {0.01423726901713096}, 
            {-1.4480323377584527}, 
            {-0.22644489960693104}, 
            {0.3756338392659724}, 
            {1.2028344769320076}, 
            {-21.791785597849927}, 
            {-1.0028158658583914}, 
            {1.3153106822048737}, 
            {-2.0727982751103005}, 
            {-0.022573576580355116}, 
            {22.455027689031112}, 
            {0.04180312942329916}, 
            {0.3377875668486667}, 
            {0.025810545679696317}, 
            {-2.668042736927493}, 
            {0.002603528981539856}, 
            {10.964642964869219}, 
            {-0.001402120926733094}, 
            {0.0767179972111637}, 
            {-0.08907258212372023}, 
            {-2.318738993652405}, 
            {-0.03809972699872233}, 
            {-0.09211996245001111}, 
            {1.7052950815904024}, 
            {-0.9761440672455828}, 
            {-0.8602050925615581}};
      DMatrixRMaj pPrimaryMotionConstraints = new DMatrixRMaj(pPrimaryMotionConstraintsData);

      double[][] centroidalMomentumAMatrixData = new double[][]{
            {0.042422480153818, -0.01274230236334784, 0.0028226073551008597, -0.4902992442380379, -1.461062125871295, 0.06995897718143854, 0.2628473331127499, 0.001436931997925527, 0.00491517359092655, -0.006397555106192451, 1.629541473622672, 1.6360959011150964, 0.0351088424336117, -0.8470272604019279, -0.8890480001211885, -4.319628823117237E-5, 0.049502194881929404, -4.3463142371675325E-4, 0.13918788187075018, 0.26750562747586304, 0.0015740653322906726, 0.0016746064916821042, 0.7249708738499954, 0.6340788433317653, -0.0015496112592226995, 0.0013589869460442405, -1.6662806833391341, -1.6727366196243698, 0.0020164971519754044, 0.0011294295898431085, 1.0905988923855162E-4, -1.126210414702922E-4, 3.848370168269017E-4, 1.0898491178070995E-4}, 
            {0.004391890683332061, 0.05329720715555922, 8.951016690866535E-4, 1.6834203555503415, -0.18887893262173783, -0.15310416734543048, 0.03210218137008577, 0.0036263853758071573, -0.0034358950997735653, 0.029874679499380954, 0.40475307224946994, 0.3786994744485309, 0.007576486945746743, 2.302855439987357, 2.278724480121585, 0.0011542550738813046, -0.19439145235121105, 0.001074954554345796, -0.5183539171537095, -0.4606096094961603, 5.681354872309493E-4, 1.368527710985591E-4, -1.8312622988387184, -1.867251156481972, 1.382267338869163E-4, 7.642835045196773E-4, -0.4128234951497257, -0.38671337572542475, -0.001717231472629023, 0.0023912788088160525, 2.299754097786037E-5, -3.0554331522823536E-5, -5.854528131088611E-4, 6.902986927145576E-4}, 
            {0.00880950308123092, -2.865941474600855E-4, 0.0011474537045612872, -0.027202150925097406, -0.17211224399389394, 0.005076724620869628, 0.049414477530278264, -4.276855597748949E-4, -1.4984021256355025E-4, -3.9235094901579785E-5, 0.18971983517622418, 0.18988393749789934, 0.00836131802989704, 0.013749391519205624, -0.12728810384192835, 0.0047078129875288395, 0.0018877252633310137, -0.004710399785702042, -0.12156769492999198, 0.13955966527584326, 0.0032938260501944967, 0.0032851609348998655, 0.10879907320468876, -0.011348171307903382, -0.0014288880537242053, 0.0014274769656239708, -0.19788987021953025, -0.19803628231784468, -0.0019298032566176723, -0.0019318662621629948, 7.642524414115378E-7, 4.569271487407728E-7, -4.419237574049621E-4, -4.4516076866712073E-4}, 
            {-0.035613947232502136, 0.20962820504180435, 0.007533422718434721, 18.013240653785605, -1.803052973764893, -1.6657131312527036, 0.27495096656355983, 0.01798058353220446, -0.011908411124088913, 0.05209180814066833, 4.121652135486617, 3.8390811621837027, 0.009435099055548847, 20.965634093508758, 20.72921928775201, -0.01042755778780408, -1.4299219168554727, -0.009530279077127975, 1.740367006966139, 2.3059437911491374, -0.005859893069811926, 7.943474157517625E-4, -22.83894378307209, -23.190922419715328, 6.029593499107745E-4, -0.0019552565068128155, -4.091589205500439, -3.8090007124807035, 0.004260547001803801, -0.0036417677026764055, -1.853290124789703E-5, -1.4654221882661693E-6, 8.812604316044589E-4, -7.741055664860914E-4}, 
            {-0.04195311185189033, 0.055576169359844045, -0.02202872187324236, 5.181513960164598, 14.526475011256931, -0.7295234289959255, -2.480312720674353, -0.01934478205612309, -0.03975542527211892, 0.011018607510674915, -16.324957946564194, -16.396522008719568, -0.04345971489762509, 7.722374614906942, 8.13380013061738, 6.200967875076548E-6, -0.38387372190675584, -0.004291244142396245, 0.7371130788871731, -0.5097287342249192, 0.011042820936172393, 0.01246662255549744, -8.51820453685804, -7.638553858609176, -0.006099359169612458, 0.005807691806349653, 16.310764765860327, 16.381189568223725, -5.801295642459261E-4, -0.0022896177715168438, 1.9691674374785017E-5, -2.8283116157507858E-5, -7.795604635767376E-5, -4.35616846474015E-4}, 
            {0.0015016380135244763, 0.006657487795877577, 5.851593676901455E-4, -1.804641044803288, -0.40430227692406784, 0.37380027436281255, 0.07110931697848531, 6.240876694767449E-4, -2.421080436103528E-4, -5.82141260231908E-4, 0.2580607686363667, 0.3111896172980726, -8.220434382906689E-5, -2.246112205406058, -2.136320784840718, -0.0012720736395895266, 0.10100309619632673, -0.001271246581632524, -0.1389510181969278, -0.33141990939795446, 0.0019314107510401243, -0.0019541319154148, 2.3778850968347367, 2.460415662355725, -0.0012431528907740925, -0.0012525307528964262, -0.2588348215093925, -0.3131930767584668, 0.00497153761741595, -0.004968159155948201, -1.7439713359256015E-5, -8.36513583041488E-6, 8.736360448667524E-4, -8.689226307125356E-4}};
      DMatrixRMaj centroidalMomentumAMatrix = new DMatrixRMaj(centroidalMomentumAMatrixData);

      double[][] momentumDotEquationRightHandSideData = new double[][]{
            {19.900706641588048}, 
            {-54.950623366869415}, 
            {2.8945480768255756}, 
            {-851.5691337315677}, 
            {-304.93925369391604}, 
            {398.9739368194259}};
      DMatrixRMaj momentumDotEquationRightHandSide = new DMatrixRMaj(momentumDotEquationRightHandSideData);
      
      double[][] momentumSubspaceData = new double[][]{
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {0.0, 0.0, 0.0}, 
            {1.0, 0.0, 0.0}, 
            {0.0, 1.0, 0.0}, 
            {0.0, 0.0, 1.0}};
      
//      double[][] momentumSubspaceData = new double[][]{
//            {0.0, 0.0, 0.0}, 
//            {0.0, 0.0, 0.0}, 
//            {0.0, 0.0, 0.0}, 
//            {1.0, 0.0, 0.0}, 
//            {0.0, 1.0, 0.0}, 
//            {0.0, 0.0, 1.0}};
      
//      double[][] momentumSubspaceData = new double[][]{
//            {0.0, 0.0}, 
//            {0.0, 0.0}, 
//            {0.0, 0.0}, 
//            {1.0, 0.0}, 
//            {0.0, 1.0}, 
//            {0.0, 0.0}}; 
      DMatrixRMaj momentumSubspace = new DMatrixRMaj(momentumSubspaceData);
      
      
      TypicalMotionConstraintsTestHelper helper = new TypicalMotionConstraintsTestHelper(JPrimaryMotionConstraintsData, pPrimaryMotionConstraintsData, centroidalMomentumAMatrixData, momentumDotEquationRightHandSideData, momentumSubspaceData);
      
      double[] singularValuesOfCombinedConstraints = helper.getSingularValuesOfCombinedConstraints();
      System.out.println("singularValuesOfCombinedConstraints = ");
      for (int i=0; i<singularValuesOfCombinedConstraints.length; i++)
      {
         System.out.print("  " + singularValuesOfCombinedConstraints[i]);
      }
      System.out.println();

      double minimumSingularValueOfCombinedConstraints = helper.getMinimumSingularValueOfCombinedConstraints();
      System.out.println("minimumSingularValueOfCombinedConstraints = " + minimumSingularValueOfCombinedConstraints);
      
      DMatrixRMaj combinedSolution = helper.getCombinedSolution();
      System.out.println("combinedSolution = " + combinedSolution);
   }
}
