@Override public Quaternion32 createEmptyTuple() { return new Quaternion32(); }
public void setOrientation(Quaternion orientation) { isEmpty = false; this.orientation = new Quaternion32(orientation); }
@Override public Quaternion32 createTuple(double x, double y, double z, double s) { Quaternion32 quaternion = new Quaternion32(); quaternion.setUnsafe(x, y, z, s); return quaternion; }
public static Quaternion32 nextQuaternion32(Random random) { return new Quaternion32(nextQuaternion(random, Math.PI)); }
@Override public void set(BoxMessage other) { setPacketInformation(other); isEmpty = other.isEmpty; size = new Vector3D32(other.size); center = new Point3D32(center); orientation = new Quaternion32(other.orientation); }
Quaternion32 quaternion = new Quaternion32(); Quaternion32 quaternionCopy; Quaternion32 expected = new Quaternion32(); Quaternion32 quaternion2 = new Quaternion32(quaternion); quaternionArray = new float[] {expected.getX32(), expected.getY32(), expected.getZ32(), expected.getS32()}; quaternion = new Quaternion32(quaternionArray); rotationMatrix = rotationMatrixCopy = EuclidCoreRandomTools.nextRotationMatrix(random); quaternion = new Quaternion32(rotationMatrix); QuaternionConversion.convertMatrixToQuaternion(rotationMatrix, expected); rotationVector = rotationVectorCopy = EuclidCoreRandomTools.nextRotationVector(random); quaternion = new Quaternion32(rotationVector); QuaternionConversion.convertRotationVectorToQuaternion(rotationVector, expected); quaternion = new Quaternion32(expected.getX32(), expected.getY32(), expected.getZ32(), expected.getS32()); double roll = EuclidCoreRandomTools.nextDouble(random, Math.PI); quaternion = new Quaternion32(yaw, pitch, roll); QuaternionConversion.convertYawPitchRollToQuaternion(yaw, pitch, roll, expected);
public static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { Quaternion32 quat = new Quaternion32(); us.ihmc.euclid.tuple3D.Vector3D32 vector = new us.ihmc.euclid.tuple3D.Vector3D32(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX32(), vector.getY32(), vector.getZ32()); Quaternion jmeQuat = new Quaternion(quat.getX32(), quat.getY32(), quat.getZ32(), quat.getS32()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
public void setInitialConfiguration(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus) { RobotConfigurationData configurationData = new RobotConfigurationData(); configurationData.setJointNameHash(kinematicsToolboxOutputStatus.getJointNameHash()); MessageTools.copyData(kinematicsToolboxOutputStatus.getDesiredJointAngles(), configurationData.getJointAngles()); configurationData.getRootTranslation().set(new Vector3D32(kinematicsToolboxOutputStatus.getDesiredRootTranslation())); configurationData.getRootOrientation().set(new Quaternion32(kinematicsToolboxOutputStatus.getDesiredRootOrientation())); setInitialConfiguration(configurationData); }
private static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { us.ihmc.euclid.tuple4D.Quaternion32 quat = new us.ihmc.euclid.tuple4D.Quaternion32(); us.ihmc.euclid.tuple3D.Vector3D32 vector = new us.ihmc.euclid.tuple3D.Vector3D32(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX32(), vector.getY32(), vector.getZ32()); Quaternion jmeQuat = new Quaternion(quat.getX32(), quat.getY32(), quat.getZ32(), quat.getS32()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
lidarOrientation = new Quaternion32(); lidarSensorFrame.getTransformToDesiredFrame(transformToWorld, worldFrame); transformToWorld.get(lidarOrientation, lidarPosition); lidarOrientation = lidarOrientation == null ? null : new Quaternion32(lidarOrientation);
public static FootstepPlanningRequestPacket createFootstepPlanningRequestPacket(FramePose3D initialStanceFootPose, RobotSide initialStanceSide, FramePose3D goalPose, FootstepPlannerType requestedPlannerType) { FootstepPlanningRequestPacket message = new FootstepPlanningRequestPacket(); message.setInitialStanceRobotSide(initialStanceSide.toByte()); FramePoint3D initialFramePoint = new FramePoint3D(initialStanceFootPose.getPosition()); initialFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); message.getStanceFootPositionInWorld().set(new Point3D32(initialFramePoint)); FrameQuaternion initialFrameOrientation = new FrameQuaternion(initialStanceFootPose.getOrientation()); initialFrameOrientation.changeFrame(ReferenceFrame.getWorldFrame()); message.getStanceFootOrientationInWorld().set(new Quaternion32(initialFrameOrientation)); FramePoint3D goalFramePoint = new FramePoint3D(goalPose.getPosition()); goalFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); message.getGoalPositionInWorld().set(new Point3D32(goalFramePoint)); FrameQuaternion goalFrameOrientation = new FrameQuaternion(goalPose.getOrientation()); goalFrameOrientation.changeFrame(ReferenceFrame.getWorldFrame()); message.getGoalOrientationInWorld().set(new Quaternion32(goalFrameOrientation)); message.setRequestedFootstepPlannerType(requestedPlannerType.toByte()); return message; }
Quaternion32 lidarOrientation = new Quaternion32();