public KinematicsToolboxController(CommandInputManager commandInputManager, StatusMessageOutputManager statusOutputManager,
FloatingJointBasics rootJoint, OneDoFJointBasics[] oneDoFJoints, Collection<RigidBodyBasics> controllableRigidBodies,
YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry)
{
super(statusOutputManager, parentRegistry);
this.commandInputManager = commandInputManager;
this.rootJoint = rootJoint;
this.oneDoFJoints = oneDoFJoints;
this.yoGraphicsListRegistry = yoGraphicsListRegistry;
rootBody = MultiBodySystemTools.getRootBody(oneDoFJoints[0].getPredecessor());
centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", worldFrame, rootBody);
Arrays.stream(oneDoFJoints).forEach(joint -> jointHashCodeMap.put(joint.hashCode(), joint));
controllerCore = createControllerCore(controllableRigidBodies);
feedbackControllerDataHolder = controllerCore.getWholeBodyFeedbackControllerDataHolder();
inverseKinematicsSolution = MessageTools.createKinematicsToolboxOutputStatus(oneDoFJoints);
inverseKinematicsSolution.setDestination(-1);
gains.setProportionalGains(1200.0);
gains.setMaxFeedbackAndFeedbackRate(1500.0, Double.POSITIVE_INFINITY);
privilegedWeight.set(1.0);
privilegedConfigurationGain.set(50.0);
privilegedMaxVelocity.set(Double.POSITIVE_INFINITY);
}