public void getPosition(FramePoint framePointToPack) { framePointToPack.setToZero(referenceFrame); } }
protected FrameVector computeGroundReactionForce(FramePoint2d cmp2d, double fZ) { centerOfMass.setToZero(centerOfMassFrame); WrenchDistributorTools.computePseudoCMP3d(cmp3d, centerOfMass, cmp2d, fZ, totalMass, omega0); centerOfMass.setToZero(centerOfMassFrame); WrenchDistributorTools.computeForce(groundReactionForce, centerOfMass, cmp3d, fZ); groundReactionForce.changeFrame(centerOfMassFrame); return groundReactionForce; }
public void getCenter(FramePoint pointToPack) { pointToPack.setToZero(referenceFrame); box3d.getCenter(pointToPack.getPoint()); }
public void getBodyFixedPoint(FramePoint bodyFixedPointToPack) { bodyFixedPointToPack.setToZero(frameAtBodyFixedPoint); bodyFixedPointToPack.changeFrame(endEffector.getBodyFixedFrame()); }
public void getPositionIncludingFrame(FramePoint positionToPack) { positionToPack.setToZero(getReferenceFrame()); geometryObject.getPosition(positionToPack.getPoint()); }
public void getPositionIncludingFrame(FramePoint positionToPack) { positionToPack.setToZero(getReferenceFrame()); geometryObject.getPosition(positionToPack.getPoint()); }
public void getPositionIncludingFrame(FramePoint positionToPack) { positionToPack.setToZero(getReferenceFrame()); geometryObject.getPosition(positionToPack.getPoint()); }
public void getCenter(FramePoint centerToPack) { centerToPack.setToZero(getReferenceFrame()); sphere.getCenter(centerToPack.getPoint()); }
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 setReachabilityVertex(int vertexIndex, FramePoint2d vertexLocation, ReferenceFrame frame) { tmpPoint.setToZero(frame); tmpPoint.setXY(vertexLocation); tmpPoint.changeFrame(worldFrame); reachabilityVertexLocations.get(vertexIndex).set(0, 0, tmpPoint.getX()); reachabilityVertexLocations.get(vertexIndex).set(1, 0, tmpPoint.getY()); }
public void getScanPoint(int index, FramePoint scanPointToPack) { index *= 3; scanPointToPack.setToZero(ReferenceFrame.getWorldFrame()); scanPointToPack.setX(scan[index++]); scanPointToPack.setY(scan[index++]); scanPointToPack.setZ(scan[index++]); }
private void computeUnbiasedEstimatedMeasurement(SpatialAccelerationCalculator spatialAccelerationCalculator, FrameVector estimatedMeasurement) { tempFramePoint.setToZero(measurementFrame); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(estimatedMeasurement, measurementLink, tempFramePoint); estimatedMeasurement.changeFrame(gravitationalAcceleration.getReferenceFrame()); estimatedMeasurement.sub(gravitationalAcceleration); estimatedMeasurement.changeFrame(measurementFrame); } }
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 getVoxel(FramePoint voxelLocationToPack, int xIndex, int yIndex, int zIndex) { voxelLocationToPack.setToZero(referenceFrame); voxelLocationToPack.setX(getCoordinateFromIndex(xIndex)); voxelLocationToPack.setY(getCoordinateFromIndex(yIndex)); voxelLocationToPack.setZ(getCoordinateFromIndex(zIndex)); }
public void update() { virtualLegTangentialFrameHipCentered.update(); virtualLegTangentialFrameAnkleCentered.update(); anklePosition.setToZero(endEffectorFrame); anklePosition.changeFrame(worldFrame); yoCurrentFootPosition.set(anklePosition); anklePosition.changeFrame(virtualLegTangentialFrameHipCentered); currentLegLength.set(-anklePosition.getZ()); }
private void getCorrectionVelocityForMeasurementFrameOffset(FrameVector correctionTermToPack) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); measurementOffset.setToZero(measurementFrame); measurementOffset.changeFrame(rootJoint.getFrameAfterJoint()); correctionTermToPack.setToZero(tempRootJointAngularVelocity.getReferenceFrame()); correctionTermToPack.cross(tempRootJointAngularVelocity, measurementOffset); } }
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 getClosestVoxel(FramePoint voxelLocationToPack, FramePoint inputPoint) { checkReferenceFrameMatch(inputPoint); if (!boundingBox.isInside(inputPoint.getPoint())) throw new RuntimeException("The given point is outside the grid"); voxelLocationToPack.setToZero(getReferenceFrame()); voxelLocationToPack.setX(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getX()))); voxelLocationToPack.setY(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getY()))); voxelLocationToPack.setZ(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getZ()))); }
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); }