public double getLinearPartY() { return linearPart.getY(); }
@Override public boolean containsNaN() { return Double.isNaN(radius.getX()) || Double.isNaN(radius.getY()) || Double.isNaN(radius.getZ()); }
@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; }
/** * Pack the standard deviation along each principal axis in the Vector3d. * @param principalStandardDeviationToPack The standard deviation is stored in the Vector3d as follows: x is the standard deviation on the principal axis, y on the secondary axis, and z on the third axis. */ public void getStandardDeviation(Vector3d principalStandardDeviationToPack) { principalStandardDeviationToPack.setX(Math.sqrt(variance.getX())); principalStandardDeviationToPack.setY(Math.sqrt(variance.getY())); principalStandardDeviationToPack.setZ(Math.sqrt(variance.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()); }
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()); }
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 public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart) { int index = rowStart; MatrixTools.insertQuat4dIntoEJMLVector(matrix, jointRotation, rowStart); // RotationFunctions.assertQuaternionNormalized(jointRotation, "SixDoFJoint "+name+":"); index += RotationTools.QUATERNION_SIZE; matrix.set(index++, 0, jointTranslation.getX()); matrix.set(index++, 0, jointTranslation.getY()); matrix.set(index++, 0, jointTranslation.getZ()); }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d point, double epsilon) { tempPoint3d.set(point); double scaledX = tempPoint3d.getX() / radius.getX(); double scaledY = tempPoint3d.getY() / radius.getY(); double scaledZ = tempPoint3d.getZ() / radius.getZ(); double sumOfSquares = scaledX * scaledX + scaledY * scaledY + scaledZ * scaledZ; return sumOfSquares <= 1.0 + epsilon; }
@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 projectOntoXYPlane(Line2d lineToPack) { lineToPack.set(line.getPoint().getX(), line.getPoint().getY(), line.getNormalizedVector().getX(), line.getNormalizedVector().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); }
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; }
private void setColumn(Twist twist, FramePoint comPositionScaledByMass, double subTreeMass, int column) { tempVector.set(comPositionScaledByMass.getPoint()); tempVector.cross(twist.getAngularPart(), tempVector); tempJacobianColumn.set(tempVector); tempVector.set(twist.getLinearPart()); tempVector.scale(subTreeMass); tempJacobianColumn.add(tempVector); jacobianMatrix.set(0, column, tempJacobianColumn.getX()); jacobianMatrix.set(1, column, tempJacobianColumn.getY()); jacobianMatrix.set(2, column, tempJacobianColumn.getZ()); } }