private void setupTest()
{
YoVariableRegistry registry = new YoVariableRegistry(getClass().getSimpleName());
YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry();
double controlDT = 0.005;
fullHumanoidRobotModel = new FullRobotModelTestTools.RandomFullHumanoidRobotModel(random);
fullHumanoidRobotModel.updateFrames();
CommonHumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullHumanoidRobotModel);
TwistCalculator twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), fullHumanoidRobotModel.getElevator());
ControllerCoreOptimizationSettings momentumOptimizationSettings = new GeneralMomentumOptimizationSettings();
ArrayList<ContactablePlaneBody> contactablePlaneBodies = new ArrayList<>();
for (RobotSide robotSide : RobotSide.values)
{
RigidBodyBasics footBody = fullHumanoidRobotModel.getFoot(robotSide);
ReferenceFrame soleFrame = fullHumanoidRobotModel.getSoleFrame(robotSide);
contactablePlaneBodies.add(ContactablePlaneBodyTools.createTypicalContactablePlaneBodyForTests(footBody, soleFrame));
}
JointBasics[] jointsToOptimizeFor = HighLevelHumanoidControllerToolbox.computeJointsToOptimizeFor(fullHumanoidRobotModel, new JointBasics[0]);
FloatingJointBasics rootJoint = fullHumanoidRobotModel.getRootJoint();
ReferenceFrame centerOfMassFrame = referenceFrames.getCenterOfMassFrame();
toolbox = new WholeBodyControlCoreToolbox(controlDT, gravityZ, rootJoint, jointsToOptimizeFor, centerOfMassFrame, momentumOptimizationSettings,
yoGraphicsListRegistry, registry);
toolbox.setupForInverseDynamicsSolver(contactablePlaneBodies);
jointIndexHandler = toolbox.getJointIndexHandler();
coriolisMatrixCalculator = new GravityCoriolisExternalWrenchMatrixCalculator(toolbox.getRootBody(), new ArrayList<>(), toolbox.getGravityZ());
degreesOfFreedom = jointIndexHandler.getNumberOfDoFs();
floatingBaseDoFs = fullHumanoidRobotModel.getRootJoint().getDegreesOfFreedom();
bodyDoFs = degreesOfFreedom - floatingBaseDoFs;
}