public void setPosition(FramePoint position) { checkReferenceFrameMatch(position); geometryObject.setPosition(position.getPoint()); }
public double distance(FramePoint point) { checkReferenceFrameMatch(point); return plane3d.distance(point.getPoint()); }
public void setPosition(FramePoint position) { checkReferenceFrameMatch(position); geometryObject.setPosition(position.getPoint()); }
public void orthogonalProjection(FramePoint point) { checkReferenceFrameMatch(point); plane3d.orthogonalProjection(point.getPoint()); }
public void getPosition(FramePoint positionToPack) { checkReferenceFrameMatch(positionToPack); geometryObject.getPosition(positionToPack.getPoint()); }
public void setPosition(FramePoint position) { checkReferenceFrameMatch(position); geometryObject.setPosition(position.getPoint()); }
public void pointBetweenEndPointsGivenPercantage(double percentage, FramePoint pointToPack) { checkReferenceFrameMatch(pointToPack); lineSegment3d.pointBetweenEndPointsGivenPercentage(percentage, pointToPack.getPoint()); }
public void getPosition(FramePoint positionToPack) { checkReferenceFrameMatch(positionToPack); geometryObject.getPosition(positionToPack.getPoint()); }
public void getPosition(FramePoint positionToPack) { checkReferenceFrameMatch(positionToPack); geometryObject.getPosition(positionToPack.getPoint()); }
public void setPoseIncludingFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(orientation); referenceFrame = position.referenceFrame; setPose(position.getPoint(), orientation.getQuaternion()); }
private Point3d getPointInWorld(ReferenceFrame frame) { tmpFramePoint.setToZero(frame); tmpFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); return tmpFramePoint.getPoint(); }
public void getPositionIncludingFrame(FramePoint positionToPack) { positionToPack.setToZero(getReferenceFrame()); geometryObject.getPosition(positionToPack.getPoint()); }
public void set(FramePoint position, FrameVector linearVelocity) { checkReferenceFrameMatch(position); checkReferenceFrameMatch(linearVelocity); geometryObject.set(position.getPoint(), linearVelocity.getVector()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMass); centerOfMassVector3d.set(centerOfMass.getPoint()); transformToParent.setIdentity(); transformToParent.setTranslation(centerOfMassVector3d); }
public void setIncludingFrame(double time, FramePoint position, FrameVector linearVelocity) { position.checkReferenceFrameMatch(linearVelocity); setToZero(position.getReferenceFrame()); geometryObject.set(time, position.getPoint(), linearVelocity.getVector()); }
private FootstepDataMessage createRelativeFootStep(PoseReferenceFrame frame, RobotSide side, Point3d location, Quat4d orientation) { FramePose pose = offsetPointFromFrameInWorldFrame(frame, location, orientation); FootstepDataMessage message = new FootstepDataMessage(side, pose.getFramePointCopy().getPoint(), pose.getFrameOrientationCopy().getQuaternion()); return message; }
public void setSoleFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose(ReferenceFrame.getWorldFrame(), position.getPoint(), orientation.getQuaternionCopy())); }
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()); }
@Override public void doTransitionOutOfAction() { FramePose goalPose = new FramePose(); locateGoalBehavior.getReportedGoalPoseWorldFrame(goalPose); setGoalPose(goalPose); super.doTransitionOutOfAction(); sendPacketToUI(new UIPositionCheckerPacket(goalPose.getFramePointCopy().getPoint(), goalPose.getFrameOrientationCopy().getQuaternion())); PrintTools.info("got the goal: \n" + goalPose + " \n requesting planar regions..."); } };