private void submitFootPosition(boolean parallelize, RobotSide robotSide, FramePoint desiredFootPosition) { FrameOrientation desiredFootOrientation = new FrameOrientation(desiredFootPosition.getReferenceFrame()); FramePose desiredFootPose = new FramePose(desiredFootPosition, desiredFootOrientation); submitFootPose(parallelize, robotSide, desiredFootPose); }
/** * Creates a new FramePoint2d based on the x and y components of this FramePoint */ public FramePoint2d toFramePoint2d() { return new FramePoint2d(this.getReferenceFrame(), this.getX(), this.getY()); }
public void set(RigidBody rigidBody, FramePoint measurementPointInBodyFrame, FrameVector velocityOfMeasurementPointInWorldFrame, boolean isPointVelocityValid) { this.rigidBodyName = rigidBody.getName(); this.bodyFixedReferenceFrameName = measurementPointInBodyFrame.getReferenceFrame().getName(); this.isPointVelocityValid = isPointVelocityValid; measurementPointInBodyFrame.get(this.measurementPointInBodyFrame); velocityOfMeasurementPointInWorldFrame.get(this.velocityOfMeasurementPointInWorldFrame); }
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 getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void setIncludingFrame(FramePoint position, FrameVector linearVelocity) { position.checkReferenceFrameMatch(linearVelocity); setToZero(position.getReferenceFrame()); geometryObject.set(position.getPoint(), linearVelocity.getVector()); }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
@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); } };
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 getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
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); }
@Override protected void setBehaviorInput() { FramePoint point = new FramePoint(ReferenceFrame.getWorldFrame(), initialSphereDetectionBehavior.getBallLocation().getX(), initialSphereDetectionBehavior.getBallLocation().getY(), initialSphereDetectionBehavior.getBallLocation().getZ() + initialSphereDetectionBehavior.getSpehereRadius()); wholeBodyBehavior.setSolutionQualityThreshold(2.01); wholeBodyBehavior.setTrajectoryTime(3); FrameOrientation tmpOr = new FrameOrientation(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
public void getIntersectionWithLine(FramePoint pointToPack, FrameLine line) { checkReferenceFrameMatch(line.getReferenceFrame()); checkReferenceFrameMatch(pointToPack.getReferenceFrame()); Point3d intersectionToPack = new Point3d(); plane3d.getIntersectionWithLine(intersectionToPack, line.getPoint(), line.getNormalizedVector()); pointToPack.set(intersectionToPack); }
private void updateSensorPosition() { sensorFrame.update(); sensorPose.setPose(sensorFrame.getTransformToDesiredFrame(world)); sensorPose.getPositionIncludingFrame(temp); yoSensorPositionInWorld.set(temp.getReferenceFrame(), temp.getX(), temp.getY(), temp.getZ()); }
public static void computePseudoCMP3d(FramePoint pseudoCMP3dToPack, FramePoint centerOfMass, FramePoint2d cmp, double fZ, double totalMass, double omega0) { double zCMP = centerOfMass.getZ() - fZ / (totalMass * MathTools.square(omega0)); pseudoCMP3dToPack.setIncludingFrame(cmp.getReferenceFrame(), cmp.getX(), cmp.getY(), 0.0); pseudoCMP3dToPack.changeFrame(centerOfMass.getReferenceFrame()); pseudoCMP3dToPack.setZ(zCMP); }
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 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; } }