public void interpolate(FramePose framePose1, FramePose framePose2, double alpha) { checkReferenceFrameMatch(framePose1); framePose1.checkReferenceFrameMatch(framePose2); this.pose.interpolate(framePose1.pose, framePose2.pose, alpha); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { originPose.checkReferenceFrameMatch(parentFrame); originPose.getPose(transformToParent); }
public void setOrientation(FrameOrientation frameOrientation) { checkReferenceFrameMatch(frameOrientation); pose.setOrientation(frameOrientation.getQuaternion()); }
public void setPosition(FramePoint framePoint) { checkReferenceFrameMatch(framePoint); pose.setPosition(framePoint.getPoint()); }
@Override public final void setInitialStanceFoot(FramePose stanceFootPose, RobotSide initialSide) { stanceFootPose.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); this.initialSide = initialSide; stanceFootPose.getPose(initialFootPose); initialStanceFootWasSet = true; }
public double getPositionDistance(FramePose framePose) { checkReferenceFrameMatch(framePose); return this.pose.getPoint().distance(framePose.pose.getPoint()); }
public void setControlFrameFixedInEndEffector(FramePose pose) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); pose.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); pose.getPose(controlFrameOriginInEndEffectorFrame, controlFrameOrientationInEndEffectorFrame); }
public FrameVector getTranslationNOTDueToRotationAboutFrame(FramePose otherPose) { checkReferenceFrameMatch(otherPose); RigidBodyTransform transformToOtherPose = getTransformFromThisToThat(otherPose); FrameVector ret = new FrameVector(referenceFrame); transformToOtherPose.getTranslation(ret.tuple); return ret; }
public FrameVector getTranslationDueToRotationAboutFrame(FramePose otherPose) { checkReferenceFrameMatch(otherPose); FrameVector ret = getTranslationToOtherPoseTotal(otherPose); ret.sub(getTranslationNOTDueToRotationAboutFrame(otherPose)); return ret; }
protected void changeControlFrame(FramePose controlFramePoseInEndEffector) { controlFramePoseInEndEffector.checkReferenceFrameMatch(contactableFoot.getRigidBody().getBodyFixedFrame()); spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(controlFramePoseInEndEffector); controlFrame.setPoseAndUpdate(controlFramePoseInEndEffector); }
public void setSolePose(FramePose newSolePoseInWorldFrame) { newSolePoseInWorldFrame.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); if (transformFromSoleToWorld == null) transformFromSoleToWorld = new RigidBodyTransform(); newSolePoseInWorldFrame.getPose(transformFromSoleToWorld); setSolePose(transformFromSoleToWorld); }
public FrameVector getTranslationToOtherPoseTotal(FramePose otherPose) { checkReferenceFrameMatch(otherPose); FrameVector ret = new FrameVector(referenceFrame); ret.sub(otherPose.pose.getPoint(), this.pose.getPoint()); return ret; }
public RigidBodyTransform getTransformFromThisToThat(FramePose thatPose) { checkReferenceFrameMatch(thatPose); RigidBodyTransform transformToThis = new RigidBodyTransform(); this.getPose(transformToThis); RigidBodyTransform transformToThat = new RigidBodyTransform(); thatPose.getPose(transformToThat); return TransformTools.getTransformFromA2toA1(transformToThat, transformToThis); }
public void set(FramePose framePose, boolean notifyListeners) { framePose.checkReferenceFrameMatch(getReferenceFrame()); framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
public void set(FramePose framePose, boolean notifyListeners) { framePose.checkReferenceFrameMatch(getReferenceFrame()); framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
public FootTrajectoryTask(E stateEnum,RobotSide robotSide, FramePose pose, FootTrajectoryBehavior behavior, double trajectoryTime) { super(stateEnum,behavior); this.footPoseBehavior = behavior; pose.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); Point3d position = new Point3d(); Quat4d orientation = new Quat4d(); pose.getPose(position, orientation); footTrajectoryMessage = new FootTrajectoryMessage(robotSide, trajectoryTime, position, orientation); }
private ContactableSelectableBoxRobot createDebrisRobot(FramePose debrisPose) { debrisPose.checkReferenceFrameMatch(constructionWorldFrame); ContactableSelectableBoxRobot debris = ContactableSelectableBoxRobot.createContactable2By4Robot(debrisName + String.valueOf(id++), debrisDepth, debrisWidth, debrisLength, debrisMass); debris.setPosition(debrisPose.getX(), debrisPose.getY(), debrisPose.getZ()); debris.setYawPitchRoll(debrisPose.getYaw(), debrisPose.getPitch(), debrisPose.getRoll()); return debris; }
public double getEffectiveDistanceToFramePose(FramePose framePose, double radiusOfRotation) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); transformThis.invert(); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); transformThat.invert(); return TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformThis, transformThat, radiusOfRotation); }
public void setSoleFrame(FramePose newSolePoseInWorldFrame) { newSolePoseInWorldFrame.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); RigidBodyTransform transformFromSoleToWorld = new RigidBodyTransform(); newSolePoseInWorldFrame.getPose(transformFromSoleToWorld); RigidBodyTransform transformFromShinToWorld = new RigidBodyTransform(); transformFromShinToWorld.multiply(transformFromSoleToWorld, shinFrame.getTransformToDesiredFrame(soleFrame)); shinFrame.setPoseAndUpdate(transformFromShinToWorld); }
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); } }