@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { translationToParent.get(tempVector); transformToParent.setTranslationAndIdentityRotation(tempVector); } }
public void translate(double x, double y, double z) { tempTransform.setTranslationAndIdentityRotation(x, y, z); transform.multiply(transform, tempTransform); }
public void translate(Vector3d translationVector) { tempTransform.setTranslationAndIdentityRotation(translationVector); transform.multiply(transform, tempTransform); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempTranslation.set(axis); tempTranslation.scale(position); transformToParent.setTranslationAndIdentityRotation(tempTranslation); } }
private static ReferenceFrame createOffsetFrame(ReferenceFrame parentFrame, Vector3d offset, String frameName) { RigidBodyTransform transformToParent = new RigidBodyTransform(); transformToParent.setTranslationAndIdentityRotation(offset); return createOffsetFrame(parentFrame, transformToParent, frameName); }
public static RevoluteJoint addRevoluteJoint(String jointName, RigidBody parentBody, Vector3d jointOffset, Vector3d jointAxis) { RigidBodyTransform transformToParent = new RigidBodyTransform(); transformToParent.setTranslationAndIdentityRotation(jointOffset); return addRevoluteJoint(jointName, parentBody, transformToParent, jointAxis); }
public void translate(double x, double y, double z) { RigidBodyTransform translator = new RigidBodyTransform(); translator.setTranslationAndIdentityRotation(new Vector3d(x, y, z)); transform.multiply(translator); }
public void translate(double x, double y, double z) { RigidBodyTransform translator = new RigidBodyTransform(); translator.setTranslationAndIdentityRotation(new Vector3d(x, y, z)); transform.multiply(translator); }
public static void main(String[] args) { RigidBodyTransform t = new RigidBodyTransform(); t.setTranslationAndIdentityRotation(new Vector3d(4.5, 6.6, 22)); System.out.println(getTransformedPoint(new Point3d(0.0, 1.0, 4.6), t)); }
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 static RigidBodyTransform transformLocal(RigidBodyTransform originalTransform, double localX, double localY, double localZ) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localTranslation = new Vector3d(localX, localY, localZ); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localTranslation); transform.multiply(postTranslation); return transform; }
private void updateStartAndGoalOffsetErrorTranslation() { //Translation stateEstimatorReferenceFrame.getTransformToDesiredFrame(stateEstimatorTransform_Translation, worldFrame); stateEstimatorTransform_Translation.setRotationToIdentity(); stateEstimatorPose_Translation.setPose(stateEstimatorTransform_Translation); stateEstimatorReferenceFrame_Translation.setPoseAndUpdate(stateEstimatorPose_Translation); startOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Translation); startOffsetErrorPose.getPosition(updatedStartOffset_Translation); goalOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Translation); goalOffsetErrorPose.getPosition(updatedGoalOffset_Translation); startOffsetTransform_Translation.setTranslationAndIdentityRotation(updatedStartOffset_Translation); offsetBetweenStartAndGoalVector_Translation.sub(updatedGoalOffset_Translation, updatedStartOffset_Translation); goalTranslationRawX.set(offsetBetweenStartAndGoalVector_Translation.getX()); goalTranslationRawY.set(offsetBetweenStartAndGoalVector_Translation.getY()); goalTranslationRawZ.set(offsetBetweenStartAndGoalVector_Translation.getZ()); goalTranslationWithDeadzoneX.update(); goalTranslationWithDeadzoneY.update(); goalTranslationWithDeadzoneZ.update(); updatedGoalOffsetWithDeadzone_Translation.setX(updatedStartOffset_Translation.getX() + goalTranslationWithDeadzoneX.getDoubleValue()); updatedGoalOffsetWithDeadzone_Translation.setY(updatedStartOffset_Translation.getY() + goalTranslationWithDeadzoneY.getDoubleValue()); updatedGoalOffsetWithDeadzone_Translation.setZ(updatedStartOffset_Translation.getZ() + goalTranslationWithDeadzoneZ.getDoubleValue()); goalOffsetTransform_Translation.setTranslationAndIdentityRotation(updatedGoalOffsetWithDeadzone_Translation); }
/** * Compare the localizationTimeStampedTransform with the stateEstimatorTimeStampedTransform having the same timeStamp * @param localizationTimeStampedTransformInWorld */ public void updateLocalizationTransform(TimeStampedTransform3D localizationTimeStampedTransformInWorld) { //update the stateEstimator reference frame in the past stateEstimatorTimeStampedTransformBuffer.findTransform(localizationTimeStampedTransformInWorld.getTimeStamp(), stateEstimatorTimeStampedTransformInPast); stateEstimatorPoseInThePast.setPoseIncludingFrame(worldFrame, stateEstimatorTimeStampedTransformInPast.getTransform3D()); stateEstimatorReferenceFrameInThePast.setPoseAndUpdate(stateEstimatorPoseInThePast); //update the localization Pose localizationPoseInThePast.setPoseIncludingFrame(worldFrame, localizationTimeStampedTransformInWorld.getTransform3D()); localizationReferenceFrameInThePast.setPoseAndUpdate(localizationPoseInThePast); stateEstimatorPositionInThePastInWorldFrame.setToZero(stateEstimatorReferenceFrameInThePast); stateEstimatorPositionInThePastInWorldFrame.changeFrame(worldFrame); localizationPositionInThePastInWorldFrame.setToZero(localizationReferenceFrameInThePast); localizationPositionInThePastInWorldFrame.changeFrame(worldFrame); translationOffsetFrameVector.sub(localizationPositionInThePastInWorldFrame, stateEstimatorPositionInThePastInWorldFrame); translationOffsetFrameVector.get(translationOffsetVector); localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Translation.setTranslationAndIdentityRotation(translationOffsetVector); localizationPoseInThePast.changeFrame(stateEstimatorReferenceFrameInThePast); localizationPoseInThePast.getPose(localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Rotation); localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Rotation.zeroTranslation(); totalErrorTransform.set(localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Rotation); totalErrorTransform.setTranslation(translationOffsetVector); localizationReferenceFrameInPresent_Rotation.update(); }
transform3D.setTranslationAndIdentityRotation(translation);
transform3D.setTranslationAndIdentityRotation(translation);
toOriginTransform.setTranslationAndIdentityRotation(offset); rotationMatrix.transform(translation); RigidBodyTransform translationTransform = new RigidBodyTransform(); translationTransform.setTranslationAndIdentityRotation(translation); fullTransform.multiply(translationTransform);