public void setOrientation(Quaternion orientation) { isEmpty = false; this.orientation = new Quaternion32(orientation); }
public static float getAngle(Quaternion32 q) { return 2.0f * (float) Math.acos(q.getS()); }
newHashCode = q.hashCode(); assertEquals(newHashCode, q.hashCode()); previousHashCode = q.hashCode(); float qx = q.getX32(); float qy = q.getY32(); float qz = q.getZ32(); float qs = q.getS32(); switch (random.nextInt(4)) break; q.setUnsafe(qx, qy, qz, qs); newHashCode = q.hashCode(); assertNotEquals(newHashCode, previousHashCode); previousHashCode = newHashCode;
@Override public Quaternion32 createTuple(double x, double y, double z, double s) { Quaternion32 quaternion = new Quaternion32(); quaternion.setUnsafe(x, y, z, s); return quaternion; }
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; }
@Override public void receivedPacket(RobotConfigurationData packet) { latestRobotConfigurationData = packet; FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); TFloatArrayList newJointAngles = packet.getJointAngles(); TFloatArrayList newJointVelocities = packet.getJointAngles(); TFloatArrayList newJointTorques = packet.getJointTorques(); OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (int i = 0; i < newJointAngles.size(); i++) { oneDoFJoints[i].setQ(newJointAngles.get(i)); oneDoFJoints[i].setQd(newJointVelocities.get(i)); oneDoFJoints[i].setTau(newJointTorques.get(i)); } pelvisTranslationFromRobotConfigurationData.set(packet.getRootTranslation()); pelvisOrientationFromRobotConfigurationData.set(packet.getRootOrientation()); rootJoint.getJointPose().setPosition(pelvisTranslationFromRobotConfigurationData.getX(), pelvisTranslationFromRobotConfigurationData.getY(), pelvisTranslationFromRobotConfigurationData.getZ()); rootJoint.getJointPose().getOrientation().setQuaternion(pelvisOrientationFromRobotConfigurationData.getX(), pelvisOrientationFromRobotConfigurationData.getY(), pelvisOrientationFromRobotConfigurationData.getZ(), pelvisOrientationFromRobotConfigurationData.getS()); computeDriftTransform(); rootJoint.getPredecessor().updateFramesRecursively(); yoVariableServer.update(System.currentTimeMillis()); }
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; }
Quaternion32 quaternion = new Quaternion32(); Quaternion32 quaternionCopy; Quaternion32 expected = new Quaternion32(); expected.setToZero(); EuclidCoreTestTools.assertQuaternionEquals(quaternion, expected, EPS); 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()); quaternion = new Quaternion32(yaw, pitch, roll); QuaternionConversion.convertYawPitchRollToQuaternion(yaw, pitch, roll, expected);
@Override public Quaternion32 createEmptyTuple() { return new Quaternion32(); }
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); }
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); }
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();