public void setPoints(FramePoint p1, FramePoint p2, FramePoint p3) { this.p1.setIncludingFrame(p1); this.p2.setIncludingFrame(p2); this.p3.setIncludingFrame(p3); this.p1.changeFrame(parentFrame); this.p2.changeFrame(parentFrame); this.p3.changeFrame(parentFrame); }
public String toStringForASingleReferenceFrame(ReferenceFrame referenceFrame) { getFrameTupleIncludingFrame(framePoint); framePoint.changeFrame(referenceFrame); return framePoint.toString(); }
protected void setWaypointPositions() { List<FramePoint> waypoints = getWaypointsFromTrajectoryParameters(trajectoryParameters); for (int i = 0; i < 2; i++) { waypoints.get(i).changeFrame(referenceFrame); allPositions[waypointIndices[i]].set(waypoints.get(i)); } checkForCloseWaypoints(); }
private List<FramePoint> getWaypointsAtGroundClearance(double groundClearance, double[] proportionsThroughTrajectoryForGroundClearance) { FramePoint initialPosition = allPositions[0].getFramePointCopy(); FramePoint finalPosition = allPositions[3].getFramePointCopy(); positionSources[0].getPosition(initialPosition); positionSources[1].getPosition(finalPosition); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); List<FramePoint> waypoints = getWaypointsAtSpecifiedGroundClearance(initialPosition, finalPosition, groundClearance, proportionsThroughTrajectoryForGroundClearance); return waypoints; }
public ReferenceFrame copyAndAimAxisAtPoint(Axis axisToAlign, FramePoint targetToAimAt) { ReferenceFrame initialFrame = targetToAimAt.getReferenceFrame(); targetToAimAt.changeFrame(this); FrameVector targetRelativeToCurrentFrame = new FrameVector(this, targetToAimAt.getX(), targetToAimAt.getY(), targetToAimAt.getZ()); targetToAimAt.changeFrame(initialFrame); return copyAndAlignAxisWithVector(axisToAlign, targetRelativeToCurrentFrame); }
private Point3d getPointInWorld(ReferenceFrame frame) { tmpFramePoint.setToZero(frame); tmpFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); return tmpFramePoint.getPoint(); }
private void updateFinalPosition() { finalPositionProvider.getPosition(tempFinalPosition); tempFinalPosition.changeFrame(finalPosition.getReferenceFrame()); finalPosition.set(tempFinalPosition); }
public void setFinalConditions(YoFramePoint finalPosition, YoFrameVector finalVelocity, ReferenceFrame attachedFrame) { finalPosition.getFrameTupleIncludingFrame(finalPositionInSpecificFrame); finalVelocity.getFrameTupleIncludingFrame(finalVelocityInSpecificFrame); finalPositionInSpecificFrame.changeFrame(attachedFrame); finalVelocityInSpecificFrame.changeFrame(attachedFrame); }
public void convertToFramePose(FramePoint endEffectorPositionIn, FrameOrientation endEffectorOrientationIn, FramePose poseToPack) { endEffectorPosition.setIncludingFrame(endEffectorPositionIn); endEffectorPosition.changeFrame(endEffectorFrame); endEffectorOrientation.setIncludingFrame(endEffectorOrientationIn); endEffectorOrientation.changeFrame(endEffectorFrame); poseToPack.setPoseIncludingFrame(endEffectorPosition, endEffectorOrientation); }
private FramePoint getDesiredFootstepPositionCopy(ReferenceFrame supportAnkleZUpFrame, ReferenceFrame supportAnkleFrame, RobotSide swingLegSide) { FrameVector2d desiredOffsetFromAnkle = computeDesiredOffsetFromSupportAnkle(supportAnkleZUpFrame, swingLegSide, angleToDestination.getDoubleValue(), distanceToDestination.getDoubleValue()); FramePoint footstepPosition = computeDesiredFootPosition(supportAnkleFrame, desiredOffsetFromAnkle); footstepPosition.changeFrame(worldFrame); return footstepPosition; }
public void update() { toolBody.getInertia().setMass(objectMass.getDoubleValue()); temporaryPoint.setIncludingFrame(objectCenterOfMass.getFrameTuple()); temporaryPoint.changeFrame(elevatorFrame); toolFrame.setPositionAndUpdate(temporaryPoint); // Visualization toolFramePoint.setToZero(toolFrame); toolFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); objectCenterOfMassInWorld.set(toolFramePoint); }
public void initialize() { time.set(0.0); FramePoint positionToPack = new FramePoint(); positionProvider.getPosition(positionToPack); positionToPack.changeFrame(position.getReferenceFrame()); position.set(positionToPack); }
private void visualizeTrajectory() { for (int i = 0; i < numberOfBalls; i++) { double t = i / ((double) numberOfBalls - 1) * trajectoryTime.getDoubleValue(); compute(t); currentPosition.getFrameTupleIncludingFrame(ballPosition); ballPosition.changeFrame(ReferenceFrame.getWorldFrame()); bagOfBalls.setBallLoop(ballPosition); } }
private void visualizeTrajectory() { for (int i = 0; i < numberOfBalls; i++) { double t = (double) i / ((double) numberOfBalls - 1) * trajectoryTime.getDoubleValue(); compute(t); currentPosition.getFrameTupleIncludingFrame(ballPosition); ballPosition.changeFrame(ReferenceFrame.getWorldFrame()); bagOfBalls.setBallLoop(ballPosition); } }
public void setInitialPoseToMatchReferenceFrame(ReferenceFrame referenceFrame) { initialPosition.setToZero(referenceFrame); initialOrientation.setToZero(referenceFrame); initialPosition.changeFrame(trajectoryFrame); initialOrientation.changeFrame(trajectoryFrame); yoInitialPosition.set(initialPosition); yoInitialOrientation.set(initialOrientation); }
public void requestICPPlannerToHoldCurrentCoM() { centerOfMassPosition.setToZero(centerOfMassFrame); FrameConvexPolygon2d supportPolygonInMidFeetZUp = bipedSupportPolygons.getSupportPolygonInMidFeetZUp(); convexPolygonShrinker.shrinkConstantDistanceInto(supportPolygonInMidFeetZUp, distanceToShrinkSupportPolygonWhenHoldingCurrent.getDoubleValue(), shrunkSupportPolygon); centerOfMassPosition.changeFrame(shrunkSupportPolygon.getReferenceFrame()); centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition); shrunkSupportPolygon.orthogonalProjection(centerOfMassPosition2d); centerOfMassPosition.setXY(centerOfMassPosition2d); centerOfMassPosition.changeFrame(worldFrame); icpPlanner.holdCurrentICP(yoTime.getDoubleValue(), centerOfMassPosition); }
public Graphics3DObject createVisualization(FramePoint voxelLocation, double scale, double reachabilityValue) { ReferenceFrame originalFrame = voxelLocation.getReferenceFrame(); voxelLocation.changeFrame(ReferenceFrame.getWorldFrame()); Graphics3DObject voxelViz = new Graphics3DObject(); AppearanceDefinition appearance = YoAppearance.RGBColorFromHex(Color.HSBtoRGB((float) (0.7 * reachabilityValue), 1.0f, 1.0f)); voxelViz.translate(voxelLocation.getX(), voxelLocation.getY(), voxelLocation.getZ()); voxelViz.addSphere(scale * voxelSize / 2.0, appearance); voxelLocation.changeFrame(originalFrame); return voxelViz; } }
private FramePose offsetPointFromChestInWorldFrame(double x, double y, double z, double yaw, double pitch, double roll) { FramePoint point1 = new FramePoint(referenceFrames.getChestFrame(), x, y, z); point1.changeFrame(ReferenceFrame.getWorldFrame()); FrameOrientation orient = new FrameOrientation(referenceFrames.getChestFrame(), yaw, pitch, roll); orient.changeFrame(ReferenceFrame.getWorldFrame()); FramePose pose = new FramePose(point1, orient); return pose; }
public void setInitialTimePositionsAndVelocities() { t0 = startTimeProvider.getValue(); startTime.set(startTimeProvider.getValue()); timeIntoTouchdown.set(0.0); initialPositionSource.getPosition(p0); p0.changeFrame(referenceFrame); velocitySource.get(pd0); pd0.changeFrame(referenceFrame); accelerationSource.get(pdd0); pdd0.changeFrame(referenceFrame); }
public DenseMatrix64F computeResidual() { PointPositionDataObject data = pointPositionMeasurementInputPort.getData(); ReferenceFrame referenceFrame = referenceFrameMap.getFrameByName(pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, data.getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); residualVector.set(data.getMeasurementPointInWorldFrame()); residualVector.sub(tempFramePoint.getPoint()); MatrixTools.insertTuple3dIntoEJMLVector(residualVector, residual, 0); return residual; }