public PointXAxisAtPositionFrame(String name, ReferenceFrame originFrame) { super(name, originFrame); this.positionToPointAt = new FramePoint(originFrame); }
public ZUpPreserveYReferenceFrame(ReferenceFrame worldFrame, FramePoint origin, String name) { super(name, worldFrame, false, false, true); this.worldFrame = worldFrame; this.origin = new FramePoint(origin); this.update(); }
public FramePoint getCenter() { FramePoint ret = new FramePoint(referenceFrame); getCenter(ret); return ret; }
public CenterOfMassReferenceFrame(String frameName, ReferenceFrame parentFrame, RigidBody[] rigidBodies) { super(frameName, parentFrame); centerOfMassCalculator = new CenterOfMassCalculator(rigidBodies, parentFrame); centerOfMass = new FramePoint(parentFrame); }
public ZUpFrame(ReferenceFrame worldFrame, FramePoint origin, String name) { super(name, worldFrame, false, false, true); this.worldFrame = worldFrame; this.origin = new FramePoint(origin); this.update(); }
public FramePoint getPositionCopy() { FramePoint positionCopy = new FramePoint(getReferenceFrame()); getPosition(positionCopy); return positionCopy; }
public FramePoint getPointCopy() { FramePoint pointToReturn = new FramePoint(this.getReferenceFrame()); this.getPoint(pointToReturn); return pointToReturn; }
public FramePoint getPositionCopy() { FramePoint positionCopy = new FramePoint(getReferenceFrame()); getPosition(positionCopy); return positionCopy; }
public YoFramePointControlFlowOutputPort(ControlFlowElement controlFlowElement, String namePrefix, ReferenceFrame frame, YoVariableRegistry registry) { super(namePrefix, controlFlowElement); yoFramePoint = new YoFramePoint(namePrefix, frame, registry); super.setData(new FramePoint(frame)); }
public FramePoint getFramePointCopy() { FramePoint ret = new FramePoint(getReferenceFrame(), pose.getPoint()); return ret; }
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 FramePoint orthogonalProjectionCopy(FramePoint point) { checkReferenceFrameMatch(point); FramePoint returnPoint = new FramePoint(point); orthogonalProjection(returnPoint); return returnPoint; }
public ContactPoint(FramePoint2d point2d, PlaneContactState parentContactState) { position = new FramePoint(point2d.getReferenceFrame(), point2d.getX(), point2d.getY(), 0.0); this.parentContactState = parentContactState; }
public static FramePoint generateRandomFramePoint(Random random, ReferenceFrame frame, double xMaxAbsoluteX, double yMaxAbsoluteY, double zMaxAbsoluteZ) { FramePoint randomPoint = new FramePoint(frame, RandomTools.generateRandomDouble(random, xMaxAbsoluteX), RandomTools.generateRandomDouble(random, yMaxAbsoluteY), RandomTools.generateRandomDouble(random, zMaxAbsoluteZ)); return randomPoint; }
public static FramePoint generateRandomFramePoint(Random random, ReferenceFrame frame, double xMin, double xMax, double yMin, double yMax, double zMin, double zMax) { FramePoint randomPoint = new FramePoint(frame, RandomTools.generateRandomDouble(random, xMin, xMax), RandomTools.generateRandomDouble(random, yMin, yMax), RandomTools.generateRandomDouble(random, zMin, zMax)); return randomPoint; }
public static Point3d getTransformedPoint(Point3d point3d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FramePoint framePoint = new FramePoint(starting, point3d); framePoint.changeFrame(ending); return framePoint.getPoint(); }
public void initialize() { time.set(0.0); FramePoint positionToPack = new FramePoint(); positionProvider.getPosition(positionToPack); positionToPack.changeFrame(position.getReferenceFrame()); position.set(positionToPack); }
@Override protected void setBehaviorInput() { FramePoint point = new FramePoint(ReferenceFrame.getWorldFrame(), grabLocation.getX(), grabLocation.getY(), grabLocation.getZ() + objectRadius); atlasPrimitiveActions.wholeBodyBehavior.setSolutionQualityThreshold(2.01); atlasPrimitiveActions.wholeBodyBehavior.setTrajectoryTime(3); FrameOrientation tmpOr = new FrameOrientation(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); atlasPrimitiveActions.wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
private FramePoint offsetPointFromDoor(Vector3f point) { PoseReferenceFrame doorPose = new PoseReferenceFrame("doorFrame", ReferenceFrame.getWorldFrame()); doorPose.setPoseAndUpdate(new RigidBodyTransform(searchForDoorBehavior.getLocation())); FramePoint point1 = new FramePoint(doorPose, point.x, point.y, point.z); return point1; }
protected RobotSide determineClosestFootSideToEnd(SideDependentList<Footstep> lastFootsteps) { FramePose2d endPose = getPath().getPoseAtS(1); Point3d endPoint = new Point3d(endPose.getX(), endPose.getY(), 0.0); RobotSide closestSideToEnd = FootstepUtils.getFrontFootRobotSideFromFootsteps(lastFootsteps, new FramePoint(ReferenceFrame.getWorldFrame(), endPoint)); return closestSideToEnd; }