@Override public boolean containsNaN() { if (Double.isNaN(time)) return true; if (Double.isNaN(position.getX()) || Double.isNaN(position.getY()) || Double.isNaN(position.getZ())) return true; if (Double.isNaN(linearVelocity.getX()) || Double.isNaN(linearVelocity.getY()) || Double.isNaN(linearVelocity.getZ())) return true; return false; }
@Override public boolean containsNaN() { if (Double.isNaN(orientation.getX()) || Double.isNaN(orientation.getY()) || Double.isNaN(orientation.getZ()) || Double.isNaN(orientation.getW())) return true; if (Double.isNaN(angularVelocity.getX()) || Double.isNaN(angularVelocity.getY()) || Double.isNaN(angularVelocity.getZ())) return true; return false; }
@Override public void applyTransform(RigidBodyTransform transform) { checkIsTransformationInPlane(transform); Vector3d xVector = new Vector3d(1.0, 0.0, 0.0); transform.transform(xVector); double deltaYaw = Math.atan2(xVector.getY(), xVector.getX()); if (Double.isNaN(deltaYaw) || Double.isInfinite(deltaYaw)) deltaYaw = 0.0; this.yaw = AngleTools.trimAngleMinusPiToPi(this.yaw + deltaYaw); }
public void getTranslation(Tuple3d translationToPack) { translationToPack.setX(jointTranslation.getX()); translationToPack.setZ(jointTranslation.getZ()); }
public void setWeights(Vector3d angular, Vector3d linear) { weightVector.set(0, 0, angular.getX()); weightVector.set(1, 0, angular.getY()); weightVector.set(2, 0, angular.getZ()); weightVector.set(3, 0, linear.getX()); weightVector.set(4, 0, linear.getY()); weightVector.set(5, 0, linear.getZ()); hasWeight = angular.getX() != HARD_CONSTRAINT && angular.getY() != HARD_CONSTRAINT && angular.getZ() != HARD_CONSTRAINT; hasWeight = linear.getX() != HARD_CONSTRAINT && linear.getY() != HARD_CONSTRAINT && linear.getZ() != HARD_CONSTRAINT && hasWeight; }
public void update(Vector3d vectorUnfiltered) { x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
@Override public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); int index = rowStart; matrix.set(index++, 0, yawPitchRoll[1]); matrix.set(index++, 0, jointTranslation.getX()); matrix.set(index++, 0, jointTranslation.getZ()); }
public void update(Vector3d vectorSource) { x.update(vectorSource.getX()); y.update(vectorSource.getY()); z.update(vectorSource.getZ()); }
public void update(Vector3d vectorUnfiltered) { x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
/** * Pack the variance along each principal axis in the given Vector3d. * @param principalVarianceToPack The variance is stored in the Vector3d as follows: x is the variance on the principal axis, y on the secondary axis, and z on the third axis. */ public void getVariance(Vector3d principalVarianceToPack) { principalVarianceToPack.setX(variance.getX()); principalVarianceToPack.setY(variance.getY()); principalVarianceToPack.setZ(variance.getZ()); }
public static void packVector3dToGeometry_msgsVector3(Vector3d vector, Vector3 rosVectorToPack) { rosVectorToPack.setX(vector.getX()); rosVectorToPack.setY(vector.getY()); rosVectorToPack.setZ(vector.getZ()); }
private CombinedTerrainObject3D setUpGround(String name, Vector3d tableCenter, double tableLength) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); YoAppearanceTexture texture = new YoAppearanceTexture("Textures/gridGroundProfile.png"); combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, texture); combinedTerrainObject.addBox(tableCenter.getX()-tableLength , tableCenter.getY()-tableLength, tableCenter.getX()+tableLength, tableCenter.getY()+tableLength , tableCenter.getZ()); combinedTerrainObject.addBox(wallPosition.getX() - 1.0, wallPosition.getY() - 0.05, wallPosition.getX() + 1.0, wallPosition.getY() + 0.05, 2.0); return combinedTerrainObject; }
@Override protected void doScaleInstruction(Graphics3DScaleInstruction graphics3dObjectScale) { Node scale = new Node(); Vector3d scaleFactor = graphics3dObjectScale.getScaleFactor(); scale.setLocalScale((float) scaleFactor.getX(), (float) scaleFactor.getY(), (float) scaleFactor.getZ()); currentNode.attachChild(scale); currentNode = scale; addChangedScaleListener(scale, graphics3dObjectScale); }
public void get(double[] array) { array[0] = rotation.getW(); array[1] = rotation.getX(); array[2] = rotation.getY(); array[3] = rotation.getZ(); array[4] = translation.getX(); array[5] = translation.getY(); array[6] = translation.getZ(); twist.getArray(array, 7); }
public void applyTransform(RigidBodyTransform transform, boolean requireTransformInPlane) { temporaryTransformedVector.set(this.getX(), this.getY(), 0.0); transform.transform(temporaryTransformedVector); if (requireTransformInPlane) checkIsTransformationInPlane(temporaryTransformedVector); this.set(temporaryTransformedVector.getX(), temporaryTransformedVector.getY()); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { transformToWorld.getTranslation(translationToWorld); x.set(translationToWorld.getX()); y.set(translationToWorld.getY()); z.set(translationToWorld.getZ()); orientation.setIncludingFrame(ReferenceFrame.getWorldFrame(), transformToWorld); setOrientation(orientation); }
public void createInertiaEllipsoid(Matrix3d momentOfInertia, Vector3d comOffset, double mass, AppearanceDefinition appearance) { Vector3d principalMomentsOfInertia = new Vector3d(momentOfInertia.getM00(), momentOfInertia.getM11(), momentOfInertia.getM22()); Vector3d ellipsoidRadii = InertiaTools.getInertiaEllipsoidRadii(principalMomentsOfInertia, mass); this.translate(comOffset); this.addEllipsoid(ellipsoidRadii.getX(), ellipsoidRadii.getY(), ellipsoidRadii.getZ(), appearance); this.identity(); }
protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side) { ReferenceFrame soleFrame = soleFrames.get(side); Vector3d translation = new Vector3d(); Quat4d rotation = new Quat4d(); soleFrame.getTransformToWorldFrame().getTranslation(translation); soleFrame.getTransformToWorldFrame().getRotation(rotation); FramePose2d solePose2d = new FramePose2d(soleFrame, new Point2d(translation.getX(), translation.getY()), RotationTools.computeYaw(rotation)); Footstep foot = createFootstep(side, solePose2d); return foot; }