public void rotate(Matrix3d rotationMatrix) { tempTransform.setRotationAndZeroTranslation(rotationMatrix); transform.multiply(transform, tempTransform); }
public void translateThenRotate(RigidBodyTransform translateThenRotateTransform) { tempTransform.set(translateThenRotateTransform); transform.multiply(transform, tempTransform); }
private void updateRootTransform() { rootTransform.multiply(simulatedRootToWorldTransform, controllerWorldToRootTransform); for(int i = 0; i < yoGraphicsLists.size(); i++) { yoGraphicsLists.get(i).setRootTransform(rootTransform); } }
public RigidBodyTransform generateTransform(RigidBodyTransform init) { RigidBodyTransform rotated = new RigidBodyTransform(); rotated.multiply(init, incrementalRotation); return rotated; } }
public void updateRobotLocationBasedOnMultisensePose(RigidBodyTransform headPose) { RigidBodyTransform pelvisPose = new RigidBodyTransform(); RigidBodyTransform transformFromHeadToPelvis = pelvisFrame.getTransformToDesiredFrame(headFrame); pelvisPose.multiply(headPose, transformFromHeadToPelvis); atomicPelvisPose.set(pelvisPose); }
public static RigidBodyTransform transformFromJMECoordinatesToZup(Transform transform) { RigidBodyTransform modifiedTransform = new RigidBodyTransform(yUpToZupTransform); RigidBodyTransform temp = jmeTransformToTransform3D(transform); modifiedTransform.multiply(temp); return modifiedTransform; }
/** * Add a rotation to the current transform. */ public final void applyRotationY(double angle) { RigidBodyTransform temp = new RigidBodyTransform(); temp.setRotationPitchAndZeroTranslation(angle); multiply(temp); }
/** * Add a rotation to the current transform. */ public final void applyRotationZ(double angle) { RigidBodyTransform temp = new RigidBodyTransform(); temp.setRotationYawAndZeroTranslation(angle); multiply(temp); }
public void setSolePose(RigidBodyTransform transform) { if (transformFromAnkleToWorld == null) transformFromAnkleToWorld = new RigidBodyTransform(); transformFromAnkleToWorld.multiply(transform, transformFromAnkleToSoleFrame); this.ankleReferenceFrame.setPoseAndUpdate(transformFromAnkleToWorld); }
public static RigidBodyTransform yawPitchDegreesTransform(Vector3d center, double yawCCWDegrees, double pitchDownDegrees) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawCCWDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(Math.toRadians(pitchDownDegrees)); location.multiply(tilt); location.setTranslation(center); return location; }
public static RigidBodyTransform getTransformFromA2toA1(RigidBodyTransform transformFromBtoA1, RigidBodyTransform transformFromBtoA2) { RigidBodyTransform temp = new RigidBodyTransform(transformFromBtoA2); temp.invert(); RigidBodyTransform ret = new RigidBodyTransform(transformFromBtoA1); ret.multiply(temp); return ret; }
public void getTransformToParent(RigidBodyTransform transformToParentToPack) { if (parentNode == null) { transformToParentToPack.setIdentity(); } parentNode.getSoleTransform(transformToParentToPack); transformToParentToPack.invert(); transformToParentToPack.multiply(transformToParentToPack, soleTransform); }
private void changeControlFrame(ReferenceFrame oldControlFrame, ReferenceFrame newControlFrame, FramePose framePoseToModify) { if (oldControlFrame == newControlFrame) return; framePoseToModify.getPose(oldTrackingFrameDesiredTransform); newControlFrame.getTransformToDesiredFrame(transformFromNewTrackingFrameToOldTrackingFrame, oldControlFrame); newTrackingFrameDesiredTransform.multiply(oldTrackingFrameDesiredTransform, transformFromNewTrackingFrameToOldTrackingFrame); framePoseToModify.setPose(newTrackingFrameDesiredTransform); }
private void changeDesiredPoseBodyFrame(ReferenceFrame oldBodyFrame, ReferenceFrame newBodyFrame, FramePose framePoseToModify) { if (oldBodyFrame == newBodyFrame) return; framePoseToModify.getPose(oldBodyFrameDesiredTransform); newBodyFrame.getTransformToDesiredFrame(transformFromNewBodyFrameToOldBodyFrame, oldBodyFrame); newBodyFrameDesiredTransform.multiply(oldBodyFrameDesiredTransform, transformFromNewBodyFrameToOldBodyFrame); framePoseToModify.setPose(newBodyFrameDesiredTransform); }
public static RigidBodyTransform transformLocalY(RigidBodyTransform originalTransform, double magnitude) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localTranslation = new Vector3d(0.0, magnitude, 0.0); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localTranslation); transform.multiply(postTranslation); return transform; }
public static RigidBodyTransform transformLocalZ(RigidBodyTransform originalTransform, double magnitude) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localZTranslation = new Vector3d(0.0, 0.0, magnitude); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localZTranslation); transform.multiply(postTranslation); return transform; }
public static RigidBodyTransform transformLocalX(RigidBodyTransform originalTransform, double magnitude) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localTranslation = new Vector3d(magnitude, 0.0, 0.0); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localTranslation); transform.multiply(postTranslation); return transform; }
public void shiftInSoleFrame(Vector2d shiftVector) { RigidBodyTransform shiftTransform = new RigidBodyTransform(); shiftTransform.setTranslation(new Vector3d(shiftVector.getX(), shiftVector.getY(), 0.0)); soleTransform.multiply(soleTransform, shiftTransform); }
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 static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3d(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3d(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }