How to use

first

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

OneDoFJointBasics joint = controlledJoints.first(i);
String jointName = joint.getName();

final OneDoFJointBasics jointState = jointStateAndData.first(i);
final JointDesiredOutputBasics jointData = jointStateAndData.second(i);

OneDoFJointBasics jointState = jointStateAndData.first(i);
YoDouble qd_d_joint = desiredVelocities.get(jointData);
YoDouble q_d_joint = desiredPositions.get(jointData);

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

JointDesiredOutputBasics jointData = jointTorquesSmoothedAtStateChange.first(i);
double tau = jointData.hasDesiredTorque() ? jointData.getDesiredTorque() : 0.0;
AlphaFilteredYoVariable smoothedJointTorque = jointTorquesSmoothedAtStateChange.second(i);

OneDoFJointBasics state = controlledJoints.first(i);
JointDesiredOutputBasics output = controlledJoints.second(i);
PDController jointPDController = jointPDControllerMap.get(state);

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

 if (controlledJoints.first(i).getName().equals(jointToIgnore))
   controlledJoints.remove(i);
OneDoFJointBasics joint = controlledJoints.first(i);
String name = joint.getName();
OneDoFJointQuinticTrajectoryGenerator jointTrajectory = new OneDoFJointQuinticTrajectoryGenerator(name, joint, trajectoryTimeProvider, registry);

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