public Quat4d getQuaternionCopy() { putYoValuesIntoFrameOrientation(); return frameOrientation.getQuaternionCopy(); }
public static Quat4d getTransformedQuat(Quat4d quat4d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FrameOrientation start = new FrameOrientation(starting, quat4d); start.changeFrame(ending); return start.getQuaternionCopy(); }
public void setSoleFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose(ReferenceFrame.getWorldFrame(), position.getPoint(), orientation.getQuaternionCopy())); }
public void variableChanged(YoVariable<?> v) { if (userDoChestOrientation.getBooleanValue()) { userDesiredChestOrientation.getFrameOrientationIncludingFrame(frameOrientation); ChestTrajectoryCommand chestTrajectoryControllerCommand = new ChestTrajectoryCommand(); chestTrajectoryControllerCommand.addTrajectoryPoint(userDesiredChestTrajectoryTime.getDoubleValue(), frameOrientation.getQuaternionCopy(), new Vector3d()); controllerCommandInputManager.submitCommand(chestTrajectoryControllerCommand); userDoChestOrientation.set(false); } } });
@Override public void variableChanged(YoVariable<?> v) { if (userDesiredSetHandPoseToActual.getBooleanValue()) { ReferenceFrame referenceFrame = getReferenceFrameToUse(); ReferenceFrame wristFrame = fullRobotModel.getEndEffectorFrame(userHandPoseSide.getEnumValue(), LimbName.ARM); FramePose currentPose = new FramePose(wristFrame); currentPose.changeFrame(referenceFrame); userDesiredHandPose.setPosition(currentPose.getFramePointCopy().getPointCopy()); userDesiredHandPose.setOrientation(currentPose.getFrameOrientationCopy().getQuaternionCopy()); userDesiredSetHandPoseToActual.set(false); } } });