How to use

get

method

in

us.ihmc.tools.lists.PairList

public T first(int i)
{
 return get(i).getLeft();
}

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

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

public T first(int i)
{
 return get(i).getLeft();
}

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

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