@Override public HighLevelBehavior createHighLevelBehavior(HighLevelControlManagerFactory variousWalkingManagers, HighLevelHumanoidControllerToolbox highLevelControllerToolbox) { jointTorqueOffsetEstimatorController = new JointTorqueOffsetEstimatorController(highLevelControllerToolbox, torqueOffsetPrinter); return jointTorqueOffsetEstimatorController; }
@Override public void doAction(double timeInState) { jointTorqueOffsetEstimatorController.doControl(); }
@Override public void onExit() { jointTorqueOffsetEstimatorController.estimateTorqueOffset(false); jointTorqueOffsetEstimatorController.exportTorqueOffsets(); if (forceSensorCalibrationModule != null) forceSensorCalibrationModule.requestFootForceSensorCalibrationAtomic(); }
private void updatePDControllers() { for (int i = 0; i < oneDoFJoints.size(); i++) { OneDoFJoint joint = oneDoFJoints.get(i); if (hasTorqueOffsetForJoint(joint)) updatePDController(joint, getTimeInCurrentState()); } }
private void updatePDControllers() { for (int i = 0; i < oneDoFJoints.size(); i++) { OneDoFJointBasics joint = oneDoFJoints.get(i); if (hasTorqueOffsetForJoint(joint)) updatePDController(joint, currentTime.getDoubleValue()); } }
@Override public void doControl() { bipedSupportPolygons.updateUsingContactStates(footContactStates); controllerToolbox.update(); updateDiagnosticsWhenHangingHelpers(); updatePDControllers(); if (transferTorqueOffsets.getBooleanValue()) { transferTorqueOffsets.set(false); transferTorqueOffsetsToOutputWriter(); } if (exportJointTorqueOffsetsToFile.getBooleanValue() && torqueOffsetPrinter != null) { exportJointTorqueOffsetsToFile.set(false); exportTorqueOffsets(); } lowLevelOneDoFJointDesiredDataHolder.setDesiredTorqueFromJoints(oneDoFJoints); }
lowLevelOneDoFJointDesiredDataHolder.setJointsControlMode(jointArray, LowLevelJointControlMode.FORCE_CONTROL); createHelpers(true); if (!hasTorqueOffsetForJoint(joint)) continue; initializeDesiredPositions(); setDefaultPDControllerGains();
@Override public void doControl() { if (enableEstimationAtomic.getAndSet(false)) estimateTorqueOffset.set(true); if (disableEstimationAtomic.getAndSet(false)) estimateTorqueOffset.set(false); bipedSupportPolygons.updateUsingContactStates(footContactStates); momentumBasedController.update(); updateDiagnosticsWhenHangingHelpers(); updatePDControllers(); if (transferTorqueOffsets.getBooleanValue()) { transferTorqueOffsets.set(false); transferTorqueOffsetsToOutputWriter(); } if (exportJointTorqueOffsetsToFile.getBooleanValue() && torqueOffsetPrinter != null) { exportJointTorqueOffsetsToFile.set(false); torqueOffsetPrinter.printTorqueOffsets(this); } lowLevelOneDoFJointDesiredDataHolder.setDesiredTorqueFromJoints(oneDoFJoints); controllerCoreCommand.completeLowLevelJointData(lowLevelOneDoFJointDesiredDataHolder); }
lowLevelOneDoFJointDesiredDataHolder.setJointsControlMode(jointArray, JointDesiredControlMode.EFFORT); createHelpers(true); if (!hasTorqueOffsetForJoint(joint)) continue; setDefaultPDControllerGains();
makeArmJointHelper(robotSide, true, ArmJointName.SHOULDER_PITCH); makeArmJointHelper(robotSide, false, ArmJointName.SHOULDER_ROLL); makeArmJointHelper(robotSide, false, ArmJointName.SHOULDER_YAW); makeArmJointHelper(robotSide, true, ArmJointName.ELBOW_PITCH); makeLegJointHelper(robotSide, false, LegJointName.HIP_YAW); makeLegJointHelper(robotSide, true, LegJointName.HIP_PITCH); makeLegJointHelper(robotSide, false, LegJointName.HIP_ROLL); makeLegJointHelper(robotSide, true, LegJointName.KNEE_PITCH); makeLegJointHelper(robotSide, true, LegJointName.ANKLE_PITCH); makeLegJointHelper(robotSide, false, LegJointName.ANKLE_ROLL);
@Override public void resetEstimatedJointTorqueOffset(OneDoFJoint joint) { if (hasTorqueOffsetForJoint(joint)) helpers.get(joint).setTorqueOffset(0.0); }
@Override public void onEntry() { for (int i = 0; i < jointsData.size(); i++) { OneDoFJointBasics joint = jointsData.get(i).getLeft(); TrajectoryData trajectoryData = jointsData.get(i).getRight(); YoDouble initialPosition = trajectoryData.getInitialPosition(); YoPolynomial trajectory = trajectoryData.getTrajectory(); JointDesiredOutputReadOnly jointDesiredOutput = highLevelControlOutput.getJointDesiredOutput(joint); double startAngle = jointDesiredOutput != null && jointDesiredOutput.hasDesiredPosition() ? jointDesiredOutput.getDesiredPosition() : joint.getQ(); double startVelocity = 0.0; double finalAngle = jointTorqueOffsetEstimatorController.getJointCalibrationPosition(joint); double finalVelocity = 0.0; initialPosition.set(startAngle); trajectory.setCubic(0.0, timeToMoveForCalibration.getDoubleValue(), startAngle, startVelocity, finalAngle, finalVelocity); } jointTorqueOffsetEstimatorController.initialize(); }
@Override public void doAction(double timeInState) { double timeInTrajectory = MathTools.clamp(timeInState, 0.0, timeToMoveForCalibration.getDoubleValue()); jointTorqueOffsetEstimatorController.doControl(); for (int jointIndex = 0; jointIndex < jointsData.size(); jointIndex++) { OneDoFJointBasics joint = jointsData.get(jointIndex).getLeft(); TrajectoryData trajectoryData = jointsData.get(jointIndex).getRight(); YoPolynomial trajectory = trajectoryData.getTrajectory(); trajectory.compute(timeInTrajectory); double desiredPosition = trajectory.getPosition(); JointDesiredOutputBasics lowLevelJointData = lowLevelOneDoFJointDesiredDataHolder.getJointDesiredOutput(joint); lowLevelJointData.clear(); lowLevelJointData.setDesiredPosition(desiredPosition); lowLevelJointData.setDesiredVelocity(trajectory.getVelocity()); lowLevelJointData.setDesiredAcceleration(trajectory.getAcceleration()); JointDesiredOutputReadOnly estimatorOutput = jointTorqueOffsetEstimatorController.getOutputForLowLevelController().getJointDesiredOutput(joint); if (estimatorOutput != null && estimatorOutput.hasDesiredTorque()) { double desiredTorque = estimatorOutput.getDesiredTorque(); desiredTorque *= timeInTrajectory / timeToMoveForCalibration.getDoubleValue(); lowLevelJointData.setDesiredTorque(desiredTorque); } } lowLevelOneDoFJointDesiredDataHolder.completeWith(getStateSpecificJointSettings()); }
public ValkyrieCalibrationControllerState(HighLevelHumanoidControllerToolbox highLevelControllerToolbox, HighLevelControllerParameters highLevelControllerParameters, JointDesiredOutputListReadOnly highLevelControlOutput, ValkyrieCalibrationParameters calibrationParameters, TorqueOffsetPrinter torqueOffsetPrinter) { super(controllerState, highLevelControllerParameters, MultiBodySystemTools.filterJoints(highLevelControllerToolbox.getControlledJoints(), OneDoFJoint.class)); this.highLevelControlOutput = highLevelControlOutput; for (OneDoFJointBasics controlledJoint : controlledJoints) { String jointName = controlledJoint.getName(); YoPolynomial trajectory = new YoPolynomial(jointName + "_CalibrationTrajectory", 4, registry); YoDouble initialPosition = new YoDouble(jointName + "_CalibrationInitialPosition", registry); jointsData.add(controlledJoint, new TrajectoryData(initialPosition, trajectory)); } timeToMoveForCalibration.set(timeToMove); timeForEstimatingOffset.set(highLevelControllerParameters.getCalibrationDuration()); jointTorqueOffsetEstimatorController = new JointTorqueOffsetEstimatorController(calibrationParameters, highLevelControllerToolbox, torqueOffsetPrinter); registry.addChild(jointTorqueOffsetEstimatorController.getYoVariableRegistry()); lowLevelOneDoFJointDesiredDataHolder.registerJointsWithEmptyData(controlledJoints); StateMachineFactory<CalibrationStates, CalibrationState> factory = new StateMachineFactory<>(CalibrationStates.class); factory.setNamePrefix("calibrationState").setRegistry(registry).buildYoClock(highLevelControllerToolbox.getYoTime()); factory.addStateAndDoneTransition(CalibrationStates.ENTRY, new CalibrationEntry(), CalibrationStates.CALIBRATE); factory.addStateAndDoneTransition(CalibrationStates.CALIBRATE, new Calibration(), CalibrationStates.EXIT); factory.addState(CalibrationStates.EXIT, new CalibrationExit()); stateMachine = factory.build(CalibrationStates.ENTRY); }
@Override public void doTransitionIntoAction() { initialize(); }
makeArmJointHelper(robotSide, true, ArmJointName.SHOULDER_PITCH); makeArmJointHelper(robotSide, false, ArmJointName.SHOULDER_ROLL); makeArmJointHelper(robotSide, false, ArmJointName.SHOULDER_YAW); makeArmJointHelper(robotSide, true, ArmJointName.ELBOW_PITCH); makeLegJointHelper(robotSide, false, LegJointName.HIP_YAW); makeLegJointHelper(robotSide, true, LegJointName.HIP_PITCH); makeLegJointHelper(robotSide, false, LegJointName.HIP_ROLL); makeLegJointHelper(robotSide, true, LegJointName.KNEE_PITCH); makeLegJointHelper(robotSide, true, LegJointName.ANKLE_PITCH); makeLegJointHelper(robotSide, false, LegJointName.ANKLE_ROLL);
@Override public void resetEstimatedJointTorqueOffset(OneDoFJointBasics joint) { if (hasTorqueOffsetForJoint(joint)) helpers.get(joint).setTorqueOffset(0.0); }
if (hasTorqueOffsetForJoint(armJoint)) desiredPositions.get(armJoint).set(armJointAngles[i]); if (hasTorqueOffsetForJoint(legJoint)) desiredPositions.get(legJoint).set(legJointAngles[i]); if (hasTorqueOffsetForJoint(spineJoint)) desiredPositions.get(spineJoint).set(spineJointAngles[i]);
@Override public void doAction() { doControl(); }