@Override public boolean epsilonEquals(PlanarVelocityPacket other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.velocity_.epsilonEquals(other.velocity_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(ComVelocityPacket other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.velocity_.epsilonEquals(other.velocity_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(Wrench other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.force_.epsilonEquals(other.force_, epsilon)) return false; if (!this.torque_.epsilonEquals(other.torque_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(Accel other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.linear_.epsilonEquals(other.linear_, epsilon)) return false; if (!this.angular_.epsilonEquals(other.angular_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(Twist other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.linear_.epsilonEquals(other.linear_, epsilon)) return false; if (!this.angular_.epsilonEquals(other.angular_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(SpatialVectorMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!this.angular_part_.epsilonEquals(other.angular_part_, epsilon)) return false; if (!this.linear_part_.epsilonEquals(other.linear_part_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(SO3Waypoint other, double epsilon) { if (!orientation.epsilonEquals(other.orientation, epsilon)) return false; if (!angularVelocity.epsilonEquals(other.angularVelocity, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(WalkingControllerFailureStatusMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!this.falling_direction_.epsilonEquals(other.falling_direction_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(PlanOffsetStatus other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!this.offset_vector_.epsilonEquals(other.offset_vector_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(Vector3Stamped other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.header_.epsilonEquals(other.header_, epsilon)) return false; if (!this.vector_.epsilonEquals(other.vector_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(EuclideanWaypoint other, double epsilon) { if (!position.epsilonEquals(other.position, epsilon)) return false; if (!linearVelocity.epsilonEquals(other.linearVelocity, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(IMUPacket other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false; if (!this.angular_velocity_.epsilonEquals(other.angular_velocity_, epsilon)) return false; if (!this.linear_acceleration_.epsilonEquals(other.linear_acceleration_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(MagneticField other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.header_.epsilonEquals(other.header_, epsilon)) return false; if (!this.magnetic_field_.epsilonEquals(other.magnetic_field_, epsilon)) return false; for(int i3 = 0; i3 < magnetic_field_covariance_.length; ++i3) { if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.magnetic_field_covariance_[i3], other.magnetic_field_covariance_[i3], epsilon)) return false; } return true; }
@Override public boolean epsilonEquals(QuadrupedGroundPlaneMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!this.region_origin_.epsilonEquals(other.region_origin_, epsilon)) return false; if (!this.region_normal_.epsilonEquals(other.region_normal_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(SE3TrajectoryPointMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false; if (!this.position_.epsilonEquals(other.position_, epsilon)) return false; if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false; if (!this.linear_velocity_.epsilonEquals(other.linear_velocity_, epsilon)) return false; if (!this.angular_velocity_.epsilonEquals(other.angular_velocity_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(SO3TrajectoryPointMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false; if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false; if (!this.angular_velocity_.epsilonEquals(other.angular_velocity_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(EuclideanTrajectoryPointMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false; if (!this.position_.epsilonEquals(other.position_, epsilon)) return false; if (!this.linear_velocity_.epsilonEquals(other.linear_velocity_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(LoadBearingMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.load_, other.load_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.coefficient_of_friction_, other.coefficient_of_friction_, epsilon)) return false; if (!this.body_frame_to_contact_frame_.epsilonEquals(other.body_frame_to_contact_frame_, epsilon)) return false; if (!this.contact_normal_in_world_frame_.epsilonEquals(other.contact_normal_in_world_frame_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(KinematicsToolboxOutputStatus other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.joint_name_hash_, other.joint_name_hash_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.desired_joint_angles_, other.desired_joint_angles_, epsilon)) return false; if (!this.desired_root_translation_.epsilonEquals(other.desired_root_translation_, epsilon)) return false; if (!this.desired_root_orientation_.epsilonEquals(other.desired_root_orientation_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.solution_quality_, other.solution_quality_, epsilon)) return false; return true; }
@Test public void testInverseTransformTuple() throws Exception { Random random = new Random(24534L); RotationScaleMatrix rotationScaleMatrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0); Vector3D originalVector = EuclidCoreRandomTools.nextVector3D(random); Vector3D actualVector = new Vector3D(); Vector3D expectedVector = new Vector3D(originalVector); rotationScaleMatrix.transform(originalVector, actualVector); assertFalse(expectedVector.epsilonEquals(actualVector, EPS)); rotationScaleMatrix.inverseTransform(actualVector, actualVector); EuclidCoreTestTools.assertTuple3DEquals(expectedVector, actualVector, EPS); }