@Override public boolean epsilonEquals(DetectedObjectPacket other, double epsilon) { return pose.epsilonEquals(other.pose, epsilon) && id == other.id; }
/** * Returns true if each element of this is equal to each element of * transform within a default tolerance of 1e-10. * * @param transform * @return */ public final boolean equals(RigidBodyTransform transform) { return epsilonEquals(transform, 1e-10); }
public boolean epsilonEquals(BipedalFootstepPlannerNode nodeToCheck, double epsilon) { if (nodeToCheck.footstepSide != this.footstepSide) return false; if (!nodeToCheck.soleTransform.epsilonEquals(this.soleTransform, epsilon)) return false; return true; }
public boolean epsilonEquals(TimeStampedTransform3D other, double epsilon) { boolean haveTheSameTransform = transform3D.epsilonEquals(other.transform3D, epsilon); boolean haveTheSameTimestamp = timeStamp == other.timeStamp; return haveTheSameTransform && haveTheSameTimestamp; }
public boolean epsilonEquals(DoorLocationPacket doorPacket, double epsilon) { boolean transformEquals = doorTransformToWorld.epsilonEquals(doorPacket.getValveTransformToWorld(), epsilon); return transformEquals; } }
@Override public boolean epsilonEquals(DrillPacket other, double epsilon) { return drillTransform.epsilonEquals(other.getDrillTransform(), epsilon); }
@Override public boolean epsilonEquals(ScriptBehaviorInputPacket other, double epsilon) { boolean result = scriptName.equals(other.getScriptName()); result &= referenceTransform.epsilonEquals(other.getReferenceTransform(), epsilon); return result; }
public boolean epsilonEquals(ValveLocationPacket turnValvePacket, double epsilon) { boolean transformEquals = valveTransformToWorld.epsilonEquals(turnValvePacket.getValveTransformToWorld(), epsilon); boolean radiusEquals = valveRadius == turnValvePacket.getValveRadius(); return transformEquals && radiusEquals; } }
public boolean epsilonEquals(PlanarRegion other, double epsilon) { if (!fromLocalToWorldTransform.epsilonEquals(other.fromLocalToWorldTransform, epsilon)) return false; // Not necessary, but just in case if (!fromWorldToLocalTransform.epsilonEquals(other.fromWorldToLocalTransform, epsilon)) return false; if (getNumberOfConvexPolygons() != other.getNumberOfConvexPolygons()) return false; for (int i = 0; i < getNumberOfConvexPolygons(); i++) { if (!convexPolygons.get(i).epsilonEquals(other.convexPolygons.get(i), epsilon)) return false; } return true; }
static public boolean epsilonEquals(Transform a, Transform b, double epsilon) { RigidBodyTransform A = jmeTransformToTransform3D(a); RigidBodyTransform B = jmeTransformToTransform3D(b); return A.epsilonEquals(B, epsilon); }
if (!frame.transformToRoot.epsilonEquals(computedTransformToRoot, 1e-5))
public boolean epsilonEquals(DebrisData debrisData, double epsilon) { boolean transformEquals = debrisTransform.epsilonEquals(debrisData.getDebrisTransform(), epsilon); boolean vectorEquals = graspVector.epsilonEquals(debrisData.getGraspVector(), epsilon); boolean positionEquals = graspVectorPosition.epsilonEquals(debrisData.getGraspVectorPosition(), epsilon); return transformEquals && vectorEquals && positionEquals; } }
public static ReferenceFrame createMeasurementFrame(String sensorName, String frameName, IMUDefinition imuDefinition, RigidBody measurementBody) { RigidBodyTransform transformFromIMUToJoint = new RigidBodyTransform(); imuDefinition.getTransformFromIMUToJoint(transformFromIMUToJoint); ReferenceFrame perfectFrameAfterJoint = measurementBody.getParentJoint().getFrameAfterJoint(); if (transformFromIMUToJoint.epsilonEquals(new RigidBodyTransform(), 1e-10)) { return perfectFrameAfterJoint; } ReferenceFrame perfectMeasurementFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(sensorName + frameName, perfectFrameAfterJoint, transformFromIMUToJoint); return perfectMeasurementFrame; } }
private boolean exponentialCoordinatesOK() { Twist twist = new Twist(jacobian.getEndEffectorFrame(), jacobian.getBaseFrame(), jacobian.getJacobianFrame(), error); Matrix3d rotationCheck = new Matrix3d(); rotationCheck.setIdentity(); Vector3d positionCheck = new Vector3d(); ScrewTestTools.integrate(rotationCheck, positionCheck, 1.0, twist); RigidBodyTransform transformCheck = new RigidBodyTransform(rotationCheck, positionCheck); return transformCheck.epsilonEquals(errorTransform, 1e-5); }
public boolean epsilonEquals(AbstractPointCloudPacket other, double epsilon) { if (!(other instanceof FilteredPointCloudPacket)) { return false; } FilteredPointCloudPacket otherPacket = (FilteredPointCloudPacket) other; if (!super.epsilonEquals(otherPacket, epsilon)) { return false; } if ((getTransform() != null) && (otherPacket.getTransform() == null)) { return false; } else if ((getTransform() == null) && (otherPacket.getTransform() != null)) { return false; } if (!getTransform().epsilonEquals(otherPacket.getTransform(), epsilon)) { return false; } return true; }