How to use

PairList

in

us.ihmc.tools.lists

public void add(T first, V second)
{
 add(new ImmutablePair<T, V>(first, second));
}

  public void copy()
  {
   for(int i = 0; i < originalAndTarget.size(); i++)
   {
     RawJointSensorDataHolder original = originalAndTarget.first(i);
     RawJointSensorDataHolder target = originalAndTarget.second(i);
          target.set(original);
   }
  }
}

  public V second(int i)
  {
   return get(i).getRight();
  }
}

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 ImmutablePair<L, R> remove(int indexToRemove)
{
 PairList<L, R> existingList = getCopyForReading();
 PairList<L, R> updatedList = getCopyForWriting();
 updatedList.clear();
 if (existingList != null)
   updatedList.addAll(existingList);
 ImmutablePair<L, R> objectToReturn = updatedList.remove(indexToRemove);
 this.commit();
 return objectToReturn;
}

jointStateAndData = new PairList<>();
     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)));
positionTorqueMap = new LinkedHashMap<>();
for (int i = 0; i < jointStateAndData.size(); i++)
  final OneDoFJointBasics jointState = jointStateAndData.first(i);
  final JointDesiredOutputBasics jointData = jointStateAndData.second(i);

  controlledJoints.add(joint, lowLevelOutput.getJointDesiredOutput(joint));
  for (int i = controlledJoints.size() - 1; i >= 0; i--)
   if (controlledJoints.first(i).getName().equals(jointToIgnore))
     controlledJoints.remove(i);
submitDiagnostic(new WaitDiagnosticTask(trajectoryTimeProvider.getValue()));
for (int i = 0; i < controlledJoints.size(); i++)
  OneDoFJointBasics joint = controlledJoints.first(i);
  String name = joint.getName();
  OneDoFJointQuinticTrajectoryGenerator jointTrajectory = new OneDoFJointQuinticTrajectoryGenerator(name, joint, trajectoryTimeProvider, registry);

@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();
   double startAngle = jointTorqueOffsetEstimatorController.getJointCalibrationPosition(joint);
   double startVelocity = 0.0;
   double finalAngle = initialPosition.getDoubleValue();
   double finalVelocity = 0.0;
   trajectory.setCubic(0.0, timeToMoveForCalibration.getDoubleValue(), startAngle, startVelocity, finalAngle, finalVelocity);
  }
}

@Override
public void initialize()
{
 for (int i = 0; i < controlledJoints.size(); i++)
 {
   OneDoFJointBasics state = controlledJoints.first(i);
   jointTrajectories.get(state).initialize(state.getQ(), 0.0);
 }
 startTime.set(yoTime.getDoubleValue());
 hasBeenInitialized = true;
}

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

for (int i = 0; i < controlledJoints.size(); i++)
  OneDoFJointBasics joint = controlledJoints.first(i);
  String jointName = joint.getName();

@Override
public void processAfterController(long timestamp)
{
 for (int i = 0; i < torqueOffsetList.size(); i++)
 {
   JointDesiredOutputBasics jointData = torqueOffsetList.first(i);
   YoDouble torqueOffsetVariable = torqueOffsetList.second(i);
   double desiredAcceleration = jointData.hasDesiredAcceleration() ? jointData.getDesiredAcceleration() : 0.0;
   if (resetTorqueOffsets.getBooleanValue())
    torqueOffsetVariable.set(0.0);
   double offsetTorque = torqueOffsetVariable.getDoubleValue();
   double ditherTorque = 0.0;
   double alpha = alphaTorqueOffset.getDoubleValue();
   offsetTorque = alpha * (offsetTorque + desiredAcceleration * updateDT) + (1.0 - alpha) * offsetTorque;
   torqueOffsetVariable.set(offsetTorque);
   double desiredTorque = jointData.hasDesiredTorque() ? jointData.getDesiredTorque() : 0.0;
   jointData.setDesiredTorque(desiredTorque + offsetTorque + ditherTorque);
 }
 if (drcOutputWriter != null)
 {
   drcOutputWriter.processAfterController(timestamp);
 }
}

@Override
public void doAction(double timeInState)
{
  double timeInTrajectory = MathTools.clamp(timeInState, 0.0, timeToMoveForCalibration.getDoubleValue());
  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 *= 1.0 - timeInTrajectory / timeToMoveForCalibration.getDoubleValue();
     lowLevelJointData.setDesiredTorque(desiredTorque);
   }
  }
  lowLevelOneDoFJointDesiredDataHolder.completeWith(getStateSpecificJointSettings());
}

public void add(T first, V second)
{
 add(new ImmutablePair<T, V>(first, second));
}

  public V second(int i)
  {
   return get(i).getRight();
  }
}

for (int i = 0; i < jointTorquesSmoothedAtStateChange.size(); i++)
  JointDesiredOutputBasics jointData = jointTorquesSmoothedAtStateChange.first(i);
  double tau = jointData.hasDesiredTorque() ? jointData.getDesiredTorque() : 0.0;
  AlphaFilteredYoVariable smoothedJointTorque = jointTorquesSmoothedAtStateChange.second(i);
  smoothedJointTorque.update(tau);
  jointData.setDesiredTorque(smoothedJointTorque.getDoubleValue());

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