How to use

getZ

method

in

us.ihmc.euclid.geometry.interfaces.Pose3DBasics

/**
* Rotates, then adds the translation (x, y, z) to this pose 3D.
* <p>
* Use this method if the translation (x, y, z) is expressed in the local coordinates described by
* this pose 3D. Otherwise, use {@link #prependTranslation(double, double, double)}.
* </p>
*
* @param x the translation distance along the x-axis.
* @param y the translation distance along the y-axis.
* @param z the translation distance along the z-axis.
*/
default void appendTranslation(double x, double y, double z)
{
 double thisX = getX();
 double thisY = getY();
 double thisZ = getZ();
 setPosition(x, y, z);
 getOrientation().transform(getPosition());
 getPosition().add(thisX, thisY, thisZ);
}

double desiredHeight = controllerRootJoint.getJointPose().getZ() - 0.2;
drcSimulationTestHelper.publishToController(HumanoidMessageTools.createPelvisHeightTrajectoryMessage(0.5, desiredHeight, worldFrame, worldFrame));