public void rotateEuler(double rotateX, double rotateY, double rotateZ) { tempTransform.setRotationEulerAndZeroTranslation(rotateX, rotateY, rotateZ); transform.set(transform); transform.multiply(tempTransform); }
public static Transform transformFromZupToJMECoordinates(RigidBodyTransform transform) { RigidBodyTransform modifiedTransform = new RigidBodyTransform(zUpToYupTransform); modifiedTransform.multiply(transform); return j3dTransform3DToJMETransform(modifiedTransform); }
public void translate(double x, double y, double z) { tempTransform.setTranslationAndIdentityRotation(x, y, z); transform.set(transform); transform.multiply(tempTransform); }
public static RigidBodyTransform transformFromJMECoordinatesToZup(Transform transform) { RigidBodyTransform modifiedTransform = new RigidBodyTransform(yUpToZupTransform); RigidBodyTransform temp = jmeTransformToTransform3D(transform); modifiedTransform.multiply(temp); return modifiedTransform; }
public void rotateEuler(Vector3DReadOnly eulerAngles) { tempTransform.setRotationEulerAndZeroTranslation(eulerAngles); transform.set(transform); transform.multiply(tempTransform); }
public void rotate(RotationMatrixReadOnly rotationMatrix) { tempTransform.setRotationAndZeroTranslation(rotationMatrix); transform.set(transform); transform.multiply(tempTransform); }
public void rotate(QuaternionReadOnly rotationQuaternion) { tempTransform.setRotationAndZeroTranslation(rotationQuaternion); transform.set(transform); transform.multiply(tempTransform); }
public void translateThenRotate(RigidBodyTransform translateThenRotateTransform) { tempTransform.set(translateThenRotateTransform); transform.set(transform); transform.multiply(tempTransform); }
public void initialize(ReferenceFrame pelvisFrame, MocapRigidBody markerRigidBody) { RigidBodyTransform marker2ToMocapTransform = new RigidBodyTransform(markerRigidBody.getOrientation(), markerRigidBody.getPosition()); RigidBodyTransform worldToPelvisTransform = pelvisFrame.getTransformToWorldFrame(); worldToPelvisTransform.invert(); RigidBodyTransform worldToMocapTransform = new RigidBodyTransform(); worldToMocapTransform.multiply(marker2ToMocapTransform); worldToMocapTransform.multiply(pelvisToMarker2Transform); worldToMocapTransform.multiply(worldToPelvisTransform); mocapFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("mocapFrame", ReferenceFrame.getWorldFrame(), worldToMocapTransform); initialized = true; }
public void translateThenRotateEuler(Vector3D translationVector, Vector3D eulerAngles) { tempTransform.setRotationEulerAndZeroTranslation(eulerAngles); tempTransform.setTranslation(translationVector); transform.set(transform); transform.multiply(tempTransform); }
public static RigidBodyTransform shiftInSoleFrame(Vector2D shiftVector, RigidBodyTransform soleTransform) { RigidBodyTransform shiftTransform = new RigidBodyTransform(); shiftTransform.setTranslation(new Vector3D(shiftVector.getX(), shiftVector.getY(), 0.0)); soleTransform.multiply(shiftTransform); return soleTransform; }
protected Point3D getPoint(int index, float range) { Point3D p = new Point3D(range, 0.0, 0.0); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.multiply(getSweepTransform(index)); transform.transform(p); return p; }
protected Point3D32 getPoint3f(int index, float range) { Point3D32 p = new Point3D32(range, 0.0f, 0.0f); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.multiply(getSweepTransform(index)); transform.transform(p); return p; }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.setAndInvert(transformShoulderToEndEffector); transformEndEffectorToDesired.set(transformEndEffectorToShoulder); transformEndEffectorToDesired.multiply(transformShoulderToDesired); }
public void getAnklePose(FramePose3D poseToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); poseToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform); }
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); }
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); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newPose = new RigidBodyTransform(); pinJointTransform.setRotationYawAndZeroTranslation(steeringWheelPinJoint.getQYoVariable().getDoubleValue()); newPose.set(originalSteeringWheelPose); newPose.multiply(pinJointTransform); steeringWheelFrame.setPoseAndUpdate(newPose); super.updateAllGroundContactPointVelocities(); }
public void setSoleFrame(FramePose3D newSolePoseInWorldFrame) { newSolePoseInWorldFrame.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); RigidBodyTransform transformFromSoleToWorld = new RigidBodyTransform(); newSolePoseInWorldFrame.get(transformFromSoleToWorld); RigidBodyTransform transformFromShinToWorld = new RigidBodyTransform(); transformFromShinToWorld.set(transformFromSoleToWorld); transformFromShinToWorld.multiply(shinFrame.getTransformToDesiredFrame(soleFrame)); shinFrame.setPoseAndUpdate(transformFromShinToWorld); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform sliderJointTransform = new RigidBodyTransform(); RigidBodyTransform newButtonPose = new RigidBodyTransform(); buttonPushVector.scale(buttonSliderJoint.getQYoVariable().getDoubleValue()); sliderJointTransform.setTranslationAndIdentityRotation(buttonPushVector); buttonPushVector.normalize(); newButtonPose.set(originalButtonTransform); newButtonPose.multiply(sliderJointTransform); buttonFrame.setPoseAndUpdate(newButtonPose); super.updateAllGroundContactPointVelocities(); }