How to use

getFrameTupleIncludingFrame

method

in

us.ihmc.robotics.math.filters.AlphaFilteredYoFrameVector

  public void getFootToPelvisPosition(FramePoint positionToPack, RigidBody foot)
  {
   footToRootJointPositions.get(foot).getFrameTupleIncludingFrame(positionToPack);
  }
}

@Override
public void getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector linearAccelerationBiasToPack)
{
 Integer imuIndex = imuToIndexMap.get(imu);
 if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null)
   linearAccelerationBiasToPack.setToZero(imu.getMeasurementFrame());
 else
   linearAccelerationBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(linearAccelerationBiasToPack);
}

@Override
public void getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector angularVelocityBiasToPack)
{
 Integer imuIndex = imuToIndexMap.get(imu);
 if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null)
   angularVelocityBiasToPack.setToZero(imu.getMeasurementFrame());
 else
   angularVelocityBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(angularVelocityBiasToPack);
}

private void updateTrustedFootPosition(RigidBody trustedFoot)
{
 YoFramePoint footPositionInWorld = footPositionsInWorld.get(trustedFoot);
 AlphaFilteredYoFrameVector footToRootJointPosition = footToRootJointPositions.get(trustedFoot);
 if (trustCoPAsNonSlippingContactPoint.getBooleanValue())
 {
   footToRootJointPosition.getFrameTupleIncludingFrame(tempPosition);
   rootJointPosition.getFrameTupleIncludingFrame(tempFramePoint);
   tempFramePoint.sub(tempPosition); // New foot position
   footPositionInWorld.getFrameTuple(tempPosition); // Previous foot position
   tempFrameVector.sub(tempFramePoint, tempPosition); // Delta from previous to new foot position
   copPositionsInWorld.get(trustedFoot).add(tempFrameVector); // New CoP position
 }
 footPositionInWorld.set(rootJointPosition);
 footPositionInWorld.sub(footToRootJointPosition);
}

private void updateWristMeasuredWrench(FrameVector measuredForceToPack, FrameVector measuredTorqueToPack)
{
 momentumBasedController.getWristMeasuredWrenchHandWeightCancelled(measuredWrench, robotSide);
 measuredWrench.getLinearPartIncludingFrame(tempForceVector);
 measuredWrench.getAngularPartIncludingFrame(tempTorqueVector);
 deadzoneMeasuredForce.update(tempForceVector);
 deadzoneMeasuredTorque.update(tempTorqueVector);
 filteredMeasuredForce.update();
 filteredMeasuredTorque.update();
 filteredMeasuredForce.getFrameTupleIncludingFrame(tempForceVector);
 filteredMeasuredTorque.getFrameTupleIncludingFrame(tempTorqueVector);
 measuredWrench.setLinearPart(tempForceVector);
 measuredWrench.setAngularPart(tempTorqueVector);
 measuredWrench.changeFrame(controlFrame);
 measuredWrench.getLinearPartIncludingFrame(measuredForceToPack);
 measuredWrench.getAngularPartIncludingFrame(measuredTorqueToPack);
}