/** * Set to identity */ public RigidBodyTransform() { setIdentity(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); } };
public void identity() { transform.setIdentity(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); } };
private void setRotationWithAxisAngle(double axisAngleX, double axisAngleY, double axisAngleZ, double axisAngleTheta) { double mag = Math.sqrt(axisAngleX * axisAngleX + axisAngleY * axisAngleY + axisAngleZ * axisAngleZ); if (almostZero(mag)) { setIdentity(); } else { mag = 1.0 / mag; double ax = axisAngleX * mag; double ay = axisAngleY * mag; double az = axisAngleZ * mag; double sinTheta = Math.sin(axisAngleTheta); double cosTheta = Math.cos(axisAngleTheta); double t = 1.0 - cosTheta; double xz = ax * az; double xy = ax * ay; double yz = ay * az; mat00 = (t * ax * ax + cosTheta); mat01 = (t * xy - sinTheta * az); mat02 = (t * xz + sinTheta * ay); mat10 = (t * xy + sinTheta * az); mat11 = (t * ay * ay + cosTheta); mat12 = (t * yz - sinTheta * ax); mat20 = (t * xz - sinTheta * ay); mat21 = (t * yz + sinTheta * ax); mat22 = (t * az * az + cosTheta); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); transformToParent.setTranslation(originVector.getVector()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); rotationPlane.changeFrame(parentFrame); rotationPlane.getTransform3D(transformToParent); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); rotationPlane.changeFrame(parentFrame); rotationPlane.getTransform3D(transformToParent); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); copFilteredInFootFrame.get(copOffset); transformToParent.setTranslation(copOffset); } };
public void getTransformToParent(RigidBodyTransform transformToParentToPack) { if (parentNode == null) { transformToParentToPack.setIdentity(); } parentNode.getSoleTransform(transformToParentToPack); transformToParentToPack.invert(); transformToParentToPack.multiply(transformToParentToPack, soleTransform); }
private void setupRotationTransforms(int i) { tempTransformFromCurrentToRotary.setIdentity(); tempTransformFromCurrentToRotary.setRotationRollAndZeroTranslation(indexToAngle(i)); tempInverseTransformFromCurrentToRotary.invert(tempTransformFromCurrentToRotary); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMass); centerOfMassVector3d.set(centerOfMass.getPoint()); transformToParent.setIdentity(); transformToParent.setTranslation(centerOfMassVector3d); }
public void getTransformToDesiredFrame(RigidBodyTransform transformToPack, ReferenceFrame desiredFrame) { verifySameRoots(desiredFrame); this.efficientComputeTransform(); desiredFrame.efficientComputeTransform(); if (desiredFrame.inverseTransformToRoot != null) { if (this.transformToRoot != null) { transformToPack.multiply(desiredFrame.inverseTransformToRoot, this.transformToRoot); } else { transformToPack.set(desiredFrame.inverseTransformToRoot); } } else { if (this.transformToRoot != null) { transformToPack.set(this.transformToRoot); } else { transformToPack.setIdentity(); } } }
/** * yawAboutPoint * * @param pointToYawAbout FramePoint2d * @param yaw double * @return CartesianPositionFootstep */ public void yawAboutPoint(FramePoint2d pointToYawAbout, FramePoint2d pointToPack, double yaw) { if (temporaryPointForYawing == null) temporaryPointForYawing = new FrameVector2d(this); else temporaryPointForYawing.setIncludingFrame(this); temporaryPointForYawing.sub(pointToYawAbout); temporaryTransformToDesiredFrame.setIdentity(); temporaryTransformToDesiredFrame.setRotationYawAndZeroTranslation(yaw); temporaryPointForYawing.applyTransform(temporaryTransformToDesiredFrame); pointToPack.setIncludingFrame(pointToYawAbout); pointToPack.add(temporaryPointForYawing); }
referenceFrame.transformToRoot.setIdentity();
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { screenPosition.changeFrame(pixelsFrame); transformToParent.setIdentity(); tempTranslation.set(screenPosition.getX() + getPlotterWidthPixels() / 2.0, screenPosition.getY() - getPlotterHeightPixels() / 2.0, 0.0); transformToParent.applyTranslation(tempTranslation); transformToParent.applyRotationZ(screenRotation); tempTranslation.set(-getPlotterWidthPixels() / 2.0, getPlotterHeightPixels() / 2.0, 0.0); transformToParent.applyTranslation(tempTranslation); transformToParent.applyRotationY(Math.PI); transformToParent.applyRotationZ(Math.PI); } };
/** * Updates max velocity clipping, interpolates from where we were * at the last correction tick to the goal and updates the pelvis * @param pelvisPose - non corrected pelvis position */ private void correctPelvisPose(RigidBodyTransform pelvisPose) { updateTranslationalMaxVelocityClip(); updateRotationalMaxVelocityClip(); interpolatedRotationCorrectionFrame.interpolate(interpolationRotationStartFrame, totalRotationErrorFrame, rotationClippedAlphaValue.getDoubleValue()); interpolatedTranslationCorrectionFrame.interpolate(interpolationTranslationStartFrame, totalTranslationErrorFrame, translationClippedAlphaValue.getDoubleValue()); if (USE_ROTATION_CORRECTION) interpolatedRotationCorrectionFrame.getTransformToParent(interpolatedRotationError); else interpolatedRotationError.setIdentity(); orientationCorrection.setAndUpdate(interpolatedRotationError); interpolatedTranslationCorrectionFrame.getTransformToParent(interpolatedTranslationError); translationCorrection.setAndUpdate(interpolatedTranslationError); orientationCorrection.getTransformToDesiredFrame(pelvisPose, worldFrame); }
transform3D.setIdentity();
RigidBodyTransform pelvisTransformWithOffset = new RigidBodyTransform(); pelvisTransformWithOffset.setIdentity(); pelvisTransformWithOffset.multiply(pelvisTransformInPast_Translation); pelvisTransformWithOffset.multiply(offsetTranslationTransform);
if (tempVector.dot(expectedArmZeroConfiguration) > 1.0 - 1e-5) armZeroJointAngleConfigurationOffset.setIdentity();