public double distanceSquared(FramePoint framePoint) { checkReferenceFrameMatch(framePoint); return this.tuple.distanceSquared(framePoint.tuple); }
public double distance(FramePoint framePoint) { checkReferenceFrameMatch(framePoint); return this.tuple.distance(framePoint.tuple); }
public void setPoints(FramePoint pointA, FramePoint pointB, FramePoint pointC) { pointA.checkReferenceFrameMatch(referenceFrame); pointB.checkReferenceFrameMatch(referenceFrame); pointC.checkReferenceFrameMatch(referenceFrame); pointA.get(temporaryPointA); pointB.get(temporaryPointB); pointC.get(temporaryPointC); plane3d.setPoints(temporaryPointA, temporaryPointB, temporaryPointC); }
public void setBodyFixedPointToControl(FramePoint bodyFixedPointInEndEffectorFrame) { bodyFixedPointInEndEffectorFrame.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); bodyFixedPointToControl.setIncludingFrame(bodyFixedPointInEndEffectorFrame); }
public static FramePoint getMidPoint(FramePoint point1, FramePoint point2) { point1.checkReferenceFrameMatch(point2); FramePoint midPoint = new FramePoint(point1.referenceFrame); midPoint.interpolate(point1, point2, 0.5); return midPoint; }
public void setIncludingFrame(FramePoint position, FrameOrientation orientation, FrameVector linearVelocity, FrameVector angularVelocity) { position.checkReferenceFrameMatch(orientation); position.checkReferenceFrameMatch(linearVelocity); position.checkReferenceFrameMatch(angularVelocity); setToZero(position.getReferenceFrame()); geometryObject.set(position.getPoint(), orientation.getQuaternion(), linearVelocity.getVector(), angularVelocity.getVector()); }
public void setIncludingFrame(double time, FramePoint position, FrameOrientation orientation, FrameVector linearVelocity, FrameVector angularVelocity) { position.checkReferenceFrameMatch(orientation); position.checkReferenceFrameMatch(linearVelocity); position.checkReferenceFrameMatch(angularVelocity); setToZero(position.getReferenceFrame()); geometryObject.set(time, position.getPoint(), orientation.getQuaternion(), linearVelocity.getVector(), angularVelocity.getVector()); }
public double getXYPlaneDistance(FramePoint framePoint) { checkReferenceFrameMatch(framePoint); double dx, dy; dx = this.getX() - framePoint.getX(); dy = this.getY() - framePoint.getY(); return Math.sqrt(dx * dx + dy * dy); }
public double getXYPlaneDistance(FramePoint2d framePoint2d) { checkReferenceFrameMatch(framePoint2d); double dx, dy; dx = this.getX() - framePoint2d.getX(); dy = this.getY() - framePoint2d.getY(); return Math.sqrt(dx * dx + dy * dy); }
public void setIncludingFrame(FramePoint firstEndpoint, FramePoint secondEndpoint) { firstEndpoint.checkReferenceFrameMatch(secondEndpoint); setToZero(firstEndpoint.getReferenceFrame()); set(firstEndpoint, secondEndpoint); }
public void setIncludingFrame(FramePoint firstEndpoint, FrameVector fromFirstToSecondEndpoint) { firstEndpoint.checkReferenceFrameMatch(fromFirstToSecondEndpoint); setToZero(firstEndpoint.getReferenceFrame()); set(firstEndpoint, fromFirstToSecondEndpoint); }
public void setPoseIncludingFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(orientation); referenceFrame = position.referenceFrame; setPose(position.getPoint(), orientation.getQuaternion()); }
public static ReferenceFrame constructReferenceFrameFromPointAndZAxis(String frameName, FramePoint point, FrameVector zAxis) { point.checkReferenceFrameMatch(zAxis.getReferenceFrame()); RigidBodyTransform transformToParent = createTransformFromPointAndZAxis(point, zAxis); return constructFrameWithUnchangingTransformToParent(frameName, point.getReferenceFrame(), transformToParent); }
public FrameLine(FramePoint point, FrameVector vector) { this(point.getReferenceFrame(), new Line3d(point.getGeometryObject(), vector.getGeometryObject())); point.checkReferenceFrameMatch(vector); }
public void set(FramePoint measurementPointInBodyFrame, FramePoint positionOfMeasurementPointInWorldFrame, boolean isPointPositionValid) { bodyFixedReferenceFrameName = measurementPointInBodyFrame.getReferenceFrame().getName(); this.isPointPositionValid = isPointPositionValid; positionOfMeasurementPointInWorldFrame.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); measurementPointInBodyFrame.get(this.measurementPointInBodyFrame); positionOfMeasurementPointInWorldFrame.get(this.positionOfMeasurementPointInWorldFrame); }
public void setIncludingFrame(FramePoint position, FrameVector linearVelocity) { position.checkReferenceFrameMatch(linearVelocity); setToZero(position.getReferenceFrame()); geometryObject.set(position.getPoint(), linearVelocity.getVector()); }
public void setIncludingFrame(double time, FramePoint position, FrameVector linearVelocity) { position.checkReferenceFrameMatch(linearVelocity); setToZero(position.getReferenceFrame()); geometryObject.set(time, position.getPoint(), linearVelocity.getVector()); }
public void set(FramePoint desiredPosition, FrameVector desiredLinearVelocity, FrameVector feedForwardLinearAcceleration) { desiredPosition.checkReferenceFrameMatch(worldFrame); desiredLinearVelocity.checkReferenceFrameMatch(worldFrame); feedForwardLinearAcceleration.checkReferenceFrameMatch(worldFrame); desiredPosition.get(desiredPositionInWorld); desiredLinearVelocity.get(desiredLinearVelocityInWorld); feedForwardLinearAcceleration.get(feedForwardLinearAccelerationInWorld); }
public static double getAngularVelocity(FramePoint position, FrameVector velocity) { position.checkReferenceFrameMatch(velocity); double x = position.getX(); double y = position.getY(); double xd = velocity.getX(); double yd = velocity.getY(); double radiusSquared = MathTools.square(x) + MathTools.square(y); return (x * yd - y * xd) / radiusSquared; } }
public void setControlFrameFixedInEndEffector(FramePoint position, FrameOrientation orientation) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); position.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); orientation.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); position.get(controlFrameOriginInEndEffectorFrame); orientation.getQuaternion(controlFrameOrientationInEndEffectorFrame); }