public void getOrientation(FrameOrientation orientationToPack) { orientationToPack.setIncludingFrame(orientation); } }
public void getFrameOrientationIncludingFrame(FrameOrientation frameOrientationToPack) { putYoValuesIntoFrameOrientation(); frameOrientationToPack.setIncludingFrame(frameOrientation); }
public void getDesireds(FrameOrientation desiredOrientationToPack, FrameVector desiredAngularVelocityToPack) { desiredOrientationToPack.setIncludingFrame(desiredOrientation); desiredAngularVelocityToPack.setIncludingFrame(desiredAngularVelocity); }
public void setInitialOrientation(FrameOrientation initialOrientation) { tempOrientation.setIncludingFrame(initialOrientation); tempOrientation.changeFrame(trajectoryFrame); this.initialOrientation.set(tempOrientation); }
public void setFinalOrientation(FrameOrientation finalOrientation) { tempOrientation.setIncludingFrame(finalOrientation); tempOrientation.changeFrame(trajectoryFrame); this.finalOrientation.set(tempOrientation); }
public void set(FrameOrientation frameOrientation, boolean notifyListeners) { checkReferenceFrameMatch(frameOrientation); this.frameOrientation.setIncludingFrame(frameOrientation); getYoValuesFromFrameOrientation(notifyListeners); }
public void set(RigidBodyTransform transform3D) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), transform3D); set(tempFrameOrientation); }
public void getIncludingFrame(FrameOrientation desiredOrientationToPack, FrameVector desiredAngularVelocityToPack, FrameVector feedForwardAngularAccelerationToPack) { desiredOrientationToPack.setIncludingFrame(worldFrame, desiredOrientationInWorld); desiredAngularVelocityToPack.setIncludingFrame(worldFrame, desiredAngularVelocityInWorld); feedForwardAngularAccelerationToPack.setIncludingFrame(worldFrame, feedForwardAngularAccelerationInWorld); }
public void getIncludingFrame(FrameOrientation desiredOrientationToPack, FrameVector desiredAngularVelocityToPack, FrameVector feedForwardAngularAccelerationToPack) { desiredOrientationToPack.setIncludingFrame(worldFrame, desiredOrientationInWorld); desiredAngularVelocityToPack.setIncludingFrame(worldFrame, desiredAngularVelocityInWorld); feedForwardAngularAccelerationToPack.setIncludingFrame(worldFrame, feedForwardAngularAccelerationInWorld); }
public void set(Quat4d quaternion, boolean notifyListeners) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), quaternion); set(tempFrameOrientation, notifyListeners); }
public void setAndMatchFrame(FrameOrientation frameOrientation, boolean notifyListeners) { this.frameOrientation.setIncludingFrame(frameOrientation); this.frameOrientation.changeFrame(getReferenceFrame()); getYoValuesFromFrameOrientation(notifyListeners); }
public void convertToFramePose(FramePoint endEffectorPositionIn, FrameOrientation endEffectorOrientationIn, FramePose poseToPack) { endEffectorPosition.setIncludingFrame(endEffectorPositionIn); endEffectorPosition.changeFrame(endEffectorFrame); endEffectorOrientation.setIncludingFrame(endEffectorOrientationIn); endEffectorOrientation.changeFrame(endEffectorFrame); poseToPack.setPoseIncludingFrame(endEffectorPosition, endEffectorOrientation); }
public void getPose(FrameVector translationFromVoxelOrigin, FrameOrientation orientation, int rayIndex, int rotationAroundRayIndex) { MathTools.checkIfInRange(rayIndex, 0, numberOfRays - 1); MathTools.checkIfInRange(rotationAroundRayIndex, 0, numberOfRotationsAroundRay - 1); if (type == SphereVoxelType.graspAroundSphere) translationFromVoxelOrigin.setIncludingFrame(parentFrame, pointsOnSphere[rayIndex]); else translationFromVoxelOrigin.setToZero(parentFrame); orientation.setIncludingFrame(parentFrame, rotations[rayIndex][rotationAroundRayIndex]); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { getQuaternion(quaternion); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameOrientation.setIncludingFrame(currentReferenceFrame, quaternion); frameOrientation.changeFrame(desiredFrame); frameOrientation.getQuaternion(quaternion); set(quaternion); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(quaternion); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameOrientation.setIncludingFrame(currentReferenceFrame, quaternion); frameOrientation.changeFrame(desiredFrame); frameOrientation.getQuaternion(quaternion); set(quaternion); }
private void updateRootJointRotation(FloatingInverseDynamicsJoint rootJoint, FrameOrientation estimationLinkOrientation, ReferenceFrame estimationFrame) { tempOrientationEstimatinLink.setIncludingFrame(estimationLinkOrientation); computeEstimationLinkToWorldTransform(tempEstimationLinkToWorld, tempOrientationEstimatinLink); computeRootJointToWorldTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); Matrix3d rootJointRotation = new Matrix3d(); tempRootJointToWorld.getRotation(rootJointRotation); rootJoint.setRotation(rootJointRotation); }
private void measureOrientationInFusedFrame(FrameOrientation orientationToPack, IMUSensorReadOnly imu) { // R_{IMU}^{world} imu.getOrientationMeasurement(rotationFromIMUToWorld); transformFromIMUToWorld.setRotationAndZeroTranslation(rotationFromIMUToWorld); // R_{Fused IMU}^{IMU} fusedMeasurementFrame.getTransformToDesiredFrame(transformFromFusedIMUToIMU, imu.getMeasurementFrame()); // R_{Fused IMU}^{world} = R_{IMU}^{world} * R_{Fused IMU}^{IMU} transformFromFusedIMUToWorld.multiply(transformFromIMUToWorld, transformFromFusedIMUToIMU); transformFromFusedIMUToWorld.getRotation(rotationFromFusedIMUToWorld); orientationToPack.setIncludingFrame(fusedMeasurementFrame, rotationFromFusedIMUToWorld); }
private void updateRootJointConfiguration(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame) { tempCenterOfMassPositionState.setIncludingFrame(centerOfMassPositionPort.getData()); tempOrientationState.setIncludingFrame(orientationPort.getData()); computeEstimationLinkTransform(estimationFrame, tempEstimationLinkToWorld, tempCenterOfMassPositionState, tempOrientationState); computeRootJointTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); rootJoint.setPositionAndRotation(tempRootJointToWorld); }
public void setInitialClearance(FrameVector initialDirection, double leaveDistance) { this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld); rotationPlane.setIncludingFrame(this.initialDirection.getReferenceFrame(), axisAngleToWorld); this.leaveDistance.set(leaveDistance); }
public void setFinalApproach(FrameVector finalDirection, double approachDistance) { this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld); rotationPlane.setIncludingFrame(this.finalDirection.getReferenceFrame(), axisAngleToWorld); this.approachDistance.set(approachDistance); }