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; // This will find the root body without using rootJoint so it can be null. 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 used for everything. It is as high as possible to reduce the convergence time. gains.setMaxFeedbackAndFeedbackRate(1500.0, Double.POSITIVE_INFINITY); privilegedWeight.set(1.0); privilegedConfigurationGain.set(50.0); privilegedMaxVelocity.set(Double.POSITIVE_INFINITY); }
jointspaceGains.put(joint1.getName(), new YoPIDGains("Joint1Gains", testRegistry)); jointspaceGains.put(joint2.getName(), new YoPIDGains("Joint2Gains", testRegistry)); YoPID3DGains taskspaceOrientationGains = new SymmetricYoPIDSE3Gains("OrientationGains", testRegistry); YoPID3DGains taskspacePositionGains = new SymmetricYoPIDSE3Gains("PositionGains", testRegistry); YoDouble weight = new YoDouble("JointWeights", testRegistry); weight.set(1.0);