@Override public boolean epsilonEquals(QuaternionStamped other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!this.header_.epsilonEquals(other.header_, epsilon)) return false; if (!this.quaternion_.epsilonEquals(other.quaternion_, 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; }
public static boolean quaternionEpsilonEquals(QuaternionReadOnly original, QuaternionReadOnly result, double epsilon) { if (original.epsilonEquals(result, epsilon)) { return true; } else { Quaternion originalNegated = quaternionForEpsilonEquals.get(); originalNegated.setAndNegate(original); return originalNegated.epsilonEquals(result, epsilon); } }
@Override public boolean epsilonEquals(WalkOverTerrainGoalPacket 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.position_.epsilonEquals(other.position_, epsilon)) return false; if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(UIPositionCheckerPacket 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.position_.epsilonEquals(other.position_, epsilon)) return false; if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(VehiclePosePacket 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.position_.epsilonEquals(other.position_, epsilon)) return false; if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.index_, other.index_, 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(WallPosePacket 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.cutting_radius_, other.cutting_radius_, epsilon)) return false; if (!this.center_position_.epsilonEquals(other.center_position_, epsilon)) return false; if (!this.center_orientation_.epsilonEquals(other.center_orientation_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(FootstepStatusMessage 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.footstep_status_, other.footstep_status_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.footstep_index_, other.footstep_index_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; if (!this.desired_foot_position_in_world_.epsilonEquals(other.desired_foot_position_in_world_, epsilon)) return false; if (!this.desired_foot_orientation_in_world_.epsilonEquals(other.desired_foot_orientation_in_world_, epsilon)) return false; if (!this.actual_foot_position_in_world_.epsilonEquals(other.actual_foot_position_in_world_, epsilon)) return false; if (!this.actual_foot_orientation_in_world_.epsilonEquals(other.actual_foot_orientation_in_world_, 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(LidarScanMessage 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.robot_timestamp_, other.robot_timestamp_, epsilon)) return false; if (!this.lidar_position_.epsilonEquals(other.lidar_position_, epsilon)) return false; if (!this.lidar_orientation_.epsilonEquals(other.lidar_orientation_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.scan_, other.scan_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(KinematicsToolboxConfigurationMessage 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.privileged_root_joint_position_.epsilonEquals(other.privileged_root_joint_position_, epsilon)) return false; if (!this.privileged_root_joint_orientation_.epsilonEquals(other.privileged_root_joint_orientation_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsIntegerSequence(this.privileged_joint_hash_codes_, other.privileged_joint_hash_codes_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.privileged_joint_angles_, other.privileged_joint_angles_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(KinematicsToolboxRigidBodyMessage 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.end_effector_hash_code_, other.end_effector_hash_code_, epsilon)) return false; if (!this.desired_position_in_world_.epsilonEquals(other.desired_position_in_world_, epsilon)) return false; if (!this.desired_orientation_in_world_.epsilonEquals(other.desired_orientation_in_world_, epsilon)) return false; if (!this.control_frame_position_in_end_effector_.epsilonEquals(other.control_frame_position_in_end_effector_, epsilon)) return false; if (!this.control_frame_orientation_in_end_effector_.epsilonEquals(other.control_frame_orientation_in_end_effector_, epsilon)) return false; if (!this.angular_selection_matrix_.epsilonEquals(other.angular_selection_matrix_, epsilon)) return false; if (!this.linear_selection_matrix_.epsilonEquals(other.linear_selection_matrix_, epsilon)) return false; if (!this.angular_weight_matrix_.epsilonEquals(other.angular_weight_matrix_, epsilon)) return false; if (!this.linear_weight_matrix_.epsilonEquals(other.linear_weight_matrix_, 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; }
@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(VideoPacket 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.video_source_, other.video_source_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.timestamp_, other.timestamp_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsByteSequence(this.data_, other.data_, epsilon)) return false; if (!this.position_.epsilonEquals(other.position_, epsilon)) return false; if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false; if (!this.intrinsic_parameters_.epsilonEquals(other.intrinsic_parameters_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(ReachingManifoldMessage 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.end_effector_hash_code_, other.end_effector_hash_code_, epsilon)) return false; if (!this.manifold_origin_position_.epsilonEquals(other.manifold_origin_position_, epsilon)) return false; if (!this.manifold_origin_orientation_.epsilonEquals(other.manifold_origin_orientation_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsByteSequence(this.manifold_configuration_space_names_, other.manifold_configuration_space_names_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.manifold_lower_limits_, other.manifold_lower_limits_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.manifold_upper_limits_, other.manifold_upper_limits_, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(FootstepNodeDataMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.x_index_, other.x_index_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.y_index_, other.y_index_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.yaw_index_, other.yaw_index_, epsilon)) return false; if (!this.snap_translation_.epsilonEquals(other.snap_translation_, epsilon)) return false; if (!this.snap_rotation_.epsilonEquals(other.snap_rotation_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.parent_node_id_, other.parent_node_id_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.bipedal_footstep_planner_node_rejection_reason_, other.bipedal_footstep_planner_node_rejection_reason_, epsilon)) return false; return true; }
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testInterpolateAgainstQuat4d() throws Exception { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Random random = new Random(6546545L); Quaternion q0 = RandomGeometry.nextQuaternion(random); Quaternion q1 = RandomGeometry.nextQuaternion(random); Quaternion expectedQInterpolated = new Quaternion(); Quaternion actualQInterpolated = new Quaternion(); for (double alpha = 0.0; alpha <= 1.0; alpha += 1.0e-6) { expectedQInterpolated.interpolate(q0, q1, alpha); quaternionCalculus.interpolate(alpha, q0, q1, actualQInterpolated); assertTrue(expectedQInterpolated.epsilonEquals(actualQInterpolated, EPSILON)); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testSetQuaternionBasedOnMatrix3d() { Random random = new Random(1776L); Quaternion unitQuaternion = new Quaternion(0.0, 0.0, 0.0, 1.0); int numberOfTests = 100000; for (int i = 0; i < numberOfTests; i++) { Quaternion randomQuaternion = RandomGeometry.nextQuaternion(random); RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.set(randomQuaternion); Quaternion quaternionToPack = new Quaternion(rotationMatrix); quaternionToPack.multiplyConjugateOther(randomQuaternion); if (quaternionToPack.getS() < 0.0) quaternionToPack.negate(); boolean quaternionsAreEpsilonEquals = unitQuaternion.epsilonEquals(quaternionToPack, 1e-7); assertTrue(quaternionsAreEpsilonEquals); } }