- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {BufferedReader b =
InputStream in;new BufferedReader(new InputStreamReader(in))
- Smart code suggestions by Tabnine
}
private boolean areFoostepsEqual(Footstep sentFootstep, Footstep receivedFootstep, double epsilon) { boolean arePosesEqual = sentFootstep.getFootstepPose().epsilonEquals(receivedFootstep.getFootstepPose(), epsilon); boolean sameRobotSide = sentFootstep.getRobotSide() == receivedFootstep.getRobotSide(); boolean isTrustHeightTheSame = sentFootstep.getTrustHeight() == receivedFootstep.getTrustHeight(); // FIXME the field is set in FootstepDataListMessage only, which makes impossible to properly convert messages to footsteps. boolean isAdjustableTheSame = true; //sentFootstep.getIsAdjustable() == receivedFootstep.getIsAdjustable(); boolean sameWaypoints = sentFootstep.getCustomPositionWaypoints().size() == receivedFootstep.getCustomPositionWaypoints().size(); if (sameWaypoints) { for (int i = 0; i < sentFootstep.getCustomPositionWaypoints().size(); i++) { FramePoint3D waypoint = sentFootstep.getCustomPositionWaypoints().get(i); FramePoint3D otherWaypoint = receivedFootstep.getCustomPositionWaypoints().get(i); sameWaypoints = sameWaypoints && waypoint.epsilonEquals(otherWaypoint, epsilon); } } boolean sameBlendDuration = MathTools.epsilonEquals(sentFootstep.getSwingTrajectoryBlendDuration(), receivedFootstep.getSwingTrajectoryBlendDuration(), epsilon); return arePosesEqual && sameRobotSide && isTrustHeightTheSame && isAdjustableTheSame && sameWaypoints && sameBlendDuration; }
public static FootstepDataMessage createFootstepDataMessage(Footstep footstep) { FootstepDataMessage message = new FootstepDataMessage(); message.setRobotSide(footstep.getRobotSide().toByte()); FramePoint3D location = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(location, orientation); footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getLocation().set(location); message.getOrientation().set(orientation); packPredictedContactPoints(footstep.getPredictedContactPoints(), message); message.setTrajectoryType(footstep.getTrajectoryType().toByte()); message.setSwingHeight(footstep.getSwingHeight()); message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration()); if (footstep.getCustomPositionWaypoints().size() != 0) { for (int i = 0; i < footstep.getCustomPositionWaypoints().size(); i++) { FramePoint3D framePoint = footstep.getCustomPositionWaypoints().get(i); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getCustomPositionWaypoints().add().set(framePoint); } } return message; }