public void packOrientation(Quaternion orientation) { // FIXME used to be that before switching to EuclidCore // orientation.setW((this.orientation[0])); // orientation.setW((this.orientation[1])); // orientation.setW((this.orientation[2])); // orientation.setW((this.orientation[3])); orientation.set(this.orientation); }
@Override protected void copy(Quaternion src, Quaternion dest) { dest.set(src); }
@Override public void update() { if (unfilteredQuaternion == null) { throw new NullPointerException("AlphaFilteredYoFrameQuaternion must be constructed with a non null " + "quaternion variable to call update(), otherwise use update(Quat4d)"); } qMeasured.set(unfilteredQuaternion); update(qMeasured); }
@Override public void set(SO3Waypoint other) { orientation.set(other.orientation); angularVelocity.set(other.angularVelocity); }
public void queueAxisAngle(AxisAngle axisAngle) { tempQuaternion.set(axisAngle); queueQuaternion(tempQuaternion); }
public void queueMatrix(RotationMatrix rotationMatrix) { tempQuaternion.set(rotationMatrix); queueQuaternion(tempQuaternion); }
public static Quaternion nextQuaternion(Random random, double minMaxAngleRange) { AxisAngle orientation = nextAxisAngle(random, minMaxAngleRange); Quaternion quat = new Quaternion(); quat.set(orientation); return quat; }
private PelvisOrientationTrajectoryMessage createPelvisOrientationTrajectoryMessage(Vector3D rotationAxis, double rotationAngle) { AxisAngle desiredPelvisAxisAngle = new AxisAngle(rotationAxis, rotationAngle); Quaternion desiredPelvisQuat = new Quaternion(); desiredPelvisQuat.set(desiredPelvisAxisAngle); PelvisOrientationTrajectoryMessage pelvisOrientationTrajectoryMessage = HumanoidMessageTools.createPelvisOrientationTrajectoryMessage(3.0, desiredPelvisQuat); return pelvisOrientationTrajectoryMessage; }
public static VehiclePosePacket createVehiclePosePacket(Point3D position, Quaternion orientation) { VehiclePosePacket message = new VehiclePosePacket(); message.getPosition().set(position); message.getOrientation().set(orientation); return message; }
public double getBodyEstimateYaw() { bodyOrientation.set(bodyCurrentOrientationQx.getDoubleValue(), bodyCurrentOrientationQy.getDoubleValue(), bodyCurrentOrientationQz.getDoubleValue(), bodyCurrentOrientationQs.getDoubleValue()); return bodyOrientation.getYaw(); }
public static WallPosePacket createWallPosePacket(double cuttingRadius, Tuple3DReadOnly centerPosition, QuaternionReadOnly centerOrientation) { WallPosePacket message = new WallPosePacket(); message.setCuttingRadius(cuttingRadius); message.getCenterPosition().set(centerPosition); message.getCenterOrientation().set(centerOrientation); return message; }
public static WallPosePacket createWallPosePacket(double cuttingRadius, Tuple3DReadOnly centerPosition, RotationMatrixReadOnly rotationMatrix) { WallPosePacket message = new WallPosePacket(); message.setCuttingRadius(cuttingRadius); message.getCenterPosition().set(centerPosition); message.getCenterOrientation().set(new Quaternion(rotationMatrix)); return message; }
private void lookDown() { AxisAngle orientationAxisAngle = new AxisAngle(0.0, 1.0, 0.0, Math.PI / 2.0); Quaternion headOrientation = new Quaternion(); headOrientation.set(orientationAxisAngle); HeadTrajectoryMessage headTrajectoryMessage = HumanoidMessageTools.createHeadTrajectoryMessage(1.0, headOrientation, ReferenceFrame.getWorldFrame(), referenceFrames.getChestFrame()); headTrajectoryMessage.setDestination(PacketDestination.CONTROLLER.ordinal()); headTrajectoryPublisher.publish(headTrajectoryMessage); }
private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message) { FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.getLocation().set(stepLocation); footstepData.getOrientation().set(orient); footstepData.setRobotSide(robotSide.toByte()); message.getFootstepDataList().add().set(footstepData); }
private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message) { FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.getLocation().set(stepLocation); footstepData.getOrientation().set(orient); footstepData.setRobotSide(robotSide.toByte()); message.getFootstepDataList().add().set(footstepData); }
public void getPose(RigidBodyTransform pose) { quaternion.set(qx.getDoubleValue(), qy.getDoubleValue(), qz.getDoubleValue(), qw.getDoubleValue()); pose.setRotation(quaternion); pose.setTranslation(xPos.getDoubleValue(), yPos.getDoubleValue(), zPos.getDoubleValue()); } }
public static IMUPacket nextIMUPacket(Random random) { IMUPacket next = new IMUPacket(); next.getLinearAcceleration().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getOrientation().set(EuclidCoreRandomTools.nextQuaternion32(random)); next.getAngularVelocity().set(EuclidCoreRandomTools.nextVector3D32(random)); return next; }
public void computeChestTrajectoryMessage() { checkIfDataHasBeenSet(); ReferenceFrame chestFrame = fullRobotModelToUseForConversion.getChest().getBodyFixedFrame(); Quaternion desiredQuaternion = new Quaternion(); FrameQuaternion desiredOrientation = new FrameQuaternion(chestFrame); desiredOrientation.changeFrame(worldFrame); desiredQuaternion.set(desiredOrientation); ReferenceFrame pelvisZUpFrame = referenceFrames.getPelvisZUpFrame(); ChestTrajectoryMessage chestTrajectoryMessage = HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredQuaternion, worldFrame, pelvisZUpFrame); output.getChestTrajectoryMessage().set(chestTrajectoryMessage); }
private void addFootstep(Point3D stepLocation, RobotSide robotSide, FootstepDataListMessage message) { FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.getLocation().set(stepLocation); footstepData.getOrientation().set(new Quaternion(0.0, 0.0, 0.0, 1.0)); footstepData.setRobotSide(robotSide.toByte()); message.getFootstepDataList().add().set(footstepData); }
public static VideoPacket createVideoPacket(VideoSource videoSource, long timeStamp, byte[] data, Point3DReadOnly position, QuaternionReadOnly orientation, IntrinsicParameters intrinsicParameters) { VideoPacket message = new VideoPacket(); message.setVideoSource(videoSource.toByte()); message.setTimestamp(timeStamp); message.getData().add(data); message.getPosition().set(position); message.getOrientation().set(orientation); message.getIntrinsicParameters().set(toIntrinsicParametersMessage(intrinsicParameters)); return message; }