public void add(T first, V second) { add(new ImmutablePair<T, V>(first, second)); }
public void add(L leftObjectToAdd, R rightObjectToAdd) { PairList<L, R> existingList = getCopyForReading(); PairList<L, R> updatedList = getCopyForWriting(); updatedList.clear(); if (existingList != null) updatedList.addAll(existingList); updatedList.add(new ImmutablePair<>(leftObjectToAdd, rightObjectToAdd)); this.commit(); }
public void add(T first, V second) { add(new ImmutablePair<T, V>(first, second)); }
public RawJointSensorDataHolderMapCopier(RawJointSensorDataHolderMap originalMap, RawJointSensorDataHolderMap targetMap) { List<RawJointSensorDataHolder> originals = new ArrayList<RawJointSensorDataHolder>(originalMap.values()); List<RawJointSensorDataHolder> targets = new ArrayList<RawJointSensorDataHolder>(targetMap.values()); if(originals.size() != targets.size()) { throw new RuntimeException("Original and Target are not of equal length"); } for(int i = 0; i < originals.size(); i++) { RawJointSensorDataHolder original = originals.get(i); RawJointSensorDataHolder target = targets.get(i); if(!original.getName().equals(target.getName())) { throw new RuntimeException("Original and Target don't match. Got: " + target.getName() + ", expected: " + original.getName()); } originalAndTarget.add(original, target); } }
@Override public void setJointDesiredOutputList(JointDesiredOutputList lowLevelDataHolder) { revoluteJoints.clear(); for (int i = 0; i < lowLevelDataHolder.getNumberOfJointsWithDesiredOutput(); i++) { OneDoFJointBasics revoluteJoint = lowLevelDataHolder.getOneDoFJoint(i); JointDesiredOutputReadOnly data = lowLevelDataHolder.getJointDesiredOutput(i); String name = revoluteJoint.getName(); OneDegreeOfFreedomJoint oneDoFJoint = robot.getOneDegreeOfFreedomJoint(name); ImmutablePair<OneDegreeOfFreedomJoint, JointDesiredOutputReadOnly> jointPair = new ImmutablePair<OneDegreeOfFreedomJoint, JointDesiredOutputReadOnly>(oneDoFJoint, data); this.revoluteJoints.add(jointPair); } }
@Override public void setLowLevelControllerCoreOutput(FullHumanoidRobotModel controllerRobotModel, JointDesiredOutputList lowLevelControllerCoreOutput, RawJointSensorDataHolderMap rawJointSensorDataHolderMap) { if (drcOutputWriter != null) { drcOutputWriter.setLowLevelControllerCoreOutput(controllerRobotModel, lowLevelControllerCoreOutput, rawJointSensorDataHolderMap); } torqueOffsetList = new PairList<>(); torqueOffsetMap = new HashMap<>(); for (int i = 0; i < lowLevelControllerCoreOutput.getNumberOfJointsWithDesiredOutput(); i++) { JointDesiredOutputBasics jointData = lowLevelControllerCoreOutput.getJointDesiredOutput(i); final YoDouble torqueOffset = new YoDouble("tauOffset_" + lowLevelControllerCoreOutput.getJointName(i), registry); torqueOffsetList.add(jointData, torqueOffset); torqueOffsetMap.put(lowLevelControllerCoreOutput.getOneDoFJoint(i), torqueOffset); } }
@Override public void setLowLevelControllerCoreOutput(FullHumanoidRobotModel controllerRobotModel, JointDesiredOutputList lowLevelControllerCoreOutput, RawJointSensorDataHolderMap rawJointSensorDataHolderMap) { if(drcOutputProcessor != null) { drcOutputProcessor.setLowLevelControllerCoreOutput(controllerRobotModel, lowLevelControllerCoreOutput, rawJointSensorDataHolderMap); } for (int i = 0; i < lowLevelControllerCoreOutput.getNumberOfJointsWithDesiredOutput(); i++) { JointDesiredOutputBasics jointData = lowLevelControllerCoreOutput.getJointDesiredOutput(i); String jointName = lowLevelControllerCoreOutput.getOneDoFJoint(i).getName(); AlphaFilteredYoVariable jointTorqueSmoothedAtStateChange = new AlphaFilteredYoVariable("smoothed_tau_" + jointName, registry, alphaForJointTorqueForStateChanges); jointTorquesSmoothedAtStateChange.add(jointData, jointTorqueSmoothedAtStateChange); } }
jointStateAndData.add(controllerRobotModel.getLegJoint(robotSide, jointName), lowLevelControllerCoreOutput.getJointDesiredOutput(controllerRobotModel.getLegJoint(robotSide, jointName))); for (ArmJointName jointName : armJointsForIntegratingAcceleration) jointStateAndData.add(controllerRobotModel.getArmJoint(robotSide, jointName), lowLevelControllerCoreOutput.getJointDesiredOutput(controllerRobotModel.getArmJoint(robotSide, jointName))); jointStateAndData.add(controllerRobotModel.getSpineJoint(jointName), lowLevelControllerCoreOutput.getJointDesiredOutput(controllerRobotModel.getSpineJoint(jointName))); jointStateAndData.add(lowLevelControllerCoreOutput.getOneDoFJoint(i), lowLevelControllerCoreOutput.getJointDesiredOutput(lowLevelControllerCoreOutput.getOneDoFJoint(i)));
controlledJoints.add(joint, lowLevelOutput.getJointDesiredOutput(joint));
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); }