public ReferenceFrame getReferenceFrame() { return desiredPose.getReferenceFrame(); }
public PoseReferenceFrame(String frameName, FramePose pose) { this(frameName, pose.getReferenceFrame()); setPoseAndUpdate(pose); }
public FramePoint getFramePointCopy() { FramePoint ret = new FramePoint(getReferenceFrame(), pose.getPoint()); return ret; }
public FrameOrientation getFrameOrientationCopy() { FrameOrientation ret = new FrameOrientation(getReferenceFrame(), pose.getOrientation()); return ret; }
public FramePose(FramePose framePose) { this(framePose.getReferenceFrame(), new Pose(framePose.getGeometryObject())); }
public void setPose(FramePose framePose) { yoFramePoint.checkReferenceFrameMatch(framePose.getReferenceFrame()); framePose.getPosition(tempPoint); yoFramePoint.setPoint(tempPoint); framePose.getOrientation(tempQuaternion); yoFrameOrientation.set(tempQuaternion); }
public static FramePose2d pose2dFormPose(FramePose pose) { FramePose2d pose2d = new FramePose2d(pose.getReferenceFrame()); pose2d.setYaw(pose.getYaw()); pose2d.setX(pose.getX()); pose2d.setY(pose.getY()); return pose2d; }
private void updateControlFrameAndDesireds(ReferenceFrame newControlFrame, boolean initializeToCurrent, FrameSE3TrajectoryPoint trajectoryPointToPack) { trajectoryPointToPack.setToZero(newControlFrame); if (!initializeToCurrent) { positionTrajectoryGenerator.getPosition(tempFramePoint); orientationTrajectoryGenerator.getOrientation(tempFrameOrientation); desiredPose.setPoseIncludingFrame(tempFramePoint, tempFrameOrientation); changeControlFrame(controlFrame, newControlFrame, desiredPose); desiredPose.getPoseIncludingFrame(tempFramePoint, tempFrameOrientation); trajectoryPointToPack.setToZero(desiredPose.getReferenceFrame()); trajectoryPointToPack.setPosition(tempFramePoint); trajectoryPointToPack.setOrientation(tempFrameOrientation); } setControlFrameFixedInEndEffector(newControlFrame); }
private void setXYVectorFromPoseToPoseNormalize(YoFrameVector2d vectorToPack, FramePose fromPose, FramePose toPose) { if (fromPose.epsilonEquals(toPose, 1e-7, Double.MAX_VALUE)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2d frameTuple2d = vectorToPack.getFrameTuple2d(); frameTuple2d.setByProjectionOntoXYPlane(toPose.getFramePointCopy()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.setWithoutChecks(frameTuple2d); } }
FramePoint midpoint = new FramePoint(startPose.getReferenceFrame()); midpoint.set(endPoint); midpoint.add(startPoint);
ReferenceFrame originalControlledWithRespectToFrame = desiredControlFramePose.getReferenceFrame(); ReferenceFrame localControlledWithRespectToFrame = originalToLocalFramesMap.get(originalControlledWithRespectToFrame); if (localControlledWithRespectToFrame != null)
desiredCoP3d.changeFrame(bodyFixedControlledPose.getReferenceFrame()); bodyFixedControlledPose.setPosition(desiredCoP3d);