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 void requestSwing(RobotSide upcomingSwingSide, Footstep footstep, double swingTime) { if (!footstep.getTrustHeight()) { FramePoint supportAnklePosition = new FramePoint(ankleZUpFrames.get(upcomingSwingSide.getOppositeSide())); supportAnklePosition.changeFrame(footstep.getParentFrame()); double newHeight = supportAnklePosition.getZ(); footstep.setZ(newHeight); } FootControlModule footControlModule = footControlModules.get(upcomingSwingSide); footControlModule.setFootstep(footstep, swingTime); setContactStateForSwing(upcomingSwingSide); }