How to use

add

method

in

us.ihmc.tools.lists.PairList

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);
}