How to use

size

method

in

us.ihmc.tools.lists.PairList

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

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

for (int i = 0; i < controlledJoints.size(); i++)

positionTorqueMap = new LinkedHashMap<>();
for (int i = 0; i < jointStateAndData.size(); i++)

private void doIdleControl()
{
 for (int i = 0; i < controlledJoints.size(); i++)
 {
   OneDoFJointBasics state = controlledJoints.first(i);
   JointDesiredOutputBasics output = controlledJoints.second(i);
   PDController jointPDController = jointPDControllerMap.get(state);
   OneDoFJointQuinticTrajectoryGenerator jointTrajectory = jointTrajectories.get(state);
   jointTrajectory.compute(trajectoryTimeProvider.getValue());
   YoDouble jointDesiredPosition = jointDesiredPositionMap.get(state);
   YoDouble jointDesiredVelocity = jointDesiredVelocityMap.get(state);
   jointDesiredPosition.set(jointTrajectory.getValue());
   jointDesiredVelocity.set(0.0);
   double q = state.getQ();
   double qDesired = jointDesiredPosition.getDoubleValue();
   double qd = state.getQd();
   double qdDesired = jointDesiredVelocity.getDoubleValue();
   double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired);
   double qdMax = qdMaxIdle.getDoubleValue();
   double qddMax = qddMaxIdle.getDoubleValue();
   double tauMax = tauMaxIdle.getDoubleValue();
   qDesired = q + MathTools.clamp(qDesired - q, qdMax * controlDT);
   qdDesired = qd + MathTools.clamp(qdDesired - qd, qddMax * controlDT);
   tauDesired = MathTools.clamp(tauDesired, tauMax);
   output.setDesiredTorque(tauDesired);
   output.setDesiredPosition(qDesired);
   output.setDesiredVelocity(qdDesired);
   jointDesiredTauMap.get(state).set(tauDesired);
 }
}

@Override
public void processAfterController(long timestamp)
 for (int i = 0; i < jointStateAndData.size(); i++)

for (int i = 0; i < jointTorquesSmoothedAtStateChange.size(); i++)

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

DiagnosticTask currentTask = diagnosticTaskExecutor.getCurrentTask();
for (int i = 0; i < controlledJoints.size(); i++)

@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 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 = controlledJoints.size() - 1; i >= 0; i--)
submitDiagnostic(new WaitDiagnosticTask(trajectoryTimeProvider.getValue()));
for (int i = 0; i < controlledJoints.size(); i++)

protected void write()
{
 for (int i = 0; i < revoluteJoints.size(); i++)
 {
   OneDegreeOfFreedomJoint pinJoint = revoluteJoints.first(i);
   JointDesiredOutputReadOnly data = revoluteJoints.second(i);
   if(data.hasDesiredTorque())
   {
    pinJoint.setTau(data.getDesiredTorque());
   }
   if (data.hasStiffness())
   {
    pinJoint.setKp(data.getStiffness());
   }
   if (data.hasDamping())
   {
    pinJoint.setKd(data.getDamping());
   }
   if (data.hasDesiredPosition())
   {
    pinJoint.setqDesired(data.getDesiredPosition());
   }
   if (data.hasDesiredVelocity())
   {
    pinJoint.setQdDesired(data.getDesiredVelocity());
   }
 }
}

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