/** {@inheritDoc} */ @Override public boolean containsNaN() { return shapePose.containsNaN(); }
@Override public boolean isCommandValid() { if (coefficientOfFriction <= 0.0) return false; if (bodyFrameToContactFrame.containsNaN()) return false; if (contactNormalInWorldFrame.containsNaN()) return false; return true; }
assertFalse(transform.containsNaN()); transform.setUnsafe(Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN); assertTrue(transform.containsNaN());
private void notifyListenerSolutionWasFound(FootstepNode endNode) { FootstepPlan plan = new FootstepPlan(); List<FootstepNode> path = footstepGraph.getPathFromStart(bestGoalNode); path.add(goalNodes.get(bestGoalNode.getRobotSide().getOppositeSide())); for (int i = 1; i < path.size(); i++) { RobotSide robotSide = path.get(i).getRobotSide(); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(path.get(i).getYaw()); footstepPose.setTranslationX(path.get(i).getX()); footstepPose.setTranslationY(path.get(i).getY()); RigidBodyTransform snapTransform = snapper.snapFootstepNode(path.get(i)).getSnapTransform(); if (!snapTransform.containsNaN()) snapTransform.transform(footstepPose); plan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); } } }
@Override public FootstepPlan getPlan() { if (!footstepGraph.doesNodeExist(bestGoalNode)) return null; FootstepPlan plan = new FootstepPlan(); List<FootstepNode> path = footstepGraph.getPathFromStart(bestGoalNode); path.add(goalNodes.get(bestGoalNode.getRobotSide().getOppositeSide())); for (int i = 1; i < path.size(); i++) { RobotSide robotSide = path.get(i).getRobotSide(); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(path.get(i).getYaw()); footstepPose.setTranslationX(path.get(i).getX()); footstepPose.setTranslationY(path.get(i).getY()); RigidBodyTransform snapTransform = snapper.snapFootstepNode(path.get(i)).getSnapTransform(); if (!snapTransform.containsNaN()) snapTransform.transform(footstepPose); plan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); } return plan; }
RigidBodyTransform snapTransform = snapData.getSnapTransform(); if (snapTransform.containsNaN())
if (snapTransform.containsNaN())