368 bug new scale pitch (#393)

* Return to zero coordinate button

* Added stop button to return button

* New Scale transforms!!!

* Fixed manual control UI bugs, use real zero coordinate

Author: kjy5

Assets/Prefabs/UI/SettingsMenu/Menus/ManipulatorConnection.prefab

@@ -2,13 +2,41 @@
 
 namespace CoordinateTransforms
 {
-    public class NewScaleLeftTransform : AffineTransform
+    public class NewScaleLeftTransform : CoordinateTransform
     {
-        public NewScaleLeftTransform(float phi, float theta) : base(Vector3.one, new Vector3(theta, 0, phi))
+        private readonly Quaternion _inverseRotation;
+        private readonly Quaternion _rotation;
+
+        public NewScaleLeftTransform(float yaw, float pitch)
         {
+            var yawRotation = Quaternion.AngleAxis(yaw, Vector3.back);
+            var pitchRotation = Quaternion.AngleAxis(pitch, yawRotation * Vector3.right);
+
+            _rotation = yawRotation * pitchRotation;
+            _inverseRotation = Quaternion.Inverse(_rotation);
         }
 
         public override string Name => "New Scale Left";
         public override string Prefix => "ns-l";
+
+        public override Vector3 Transform2Space(Vector3 coordTransformed)
+        {
+            return _inverseRotation * coordTransformed;
+        }
+
+        public override Vector3 Space2Transform(Vector3 coordSpace)
+        {
+            return _rotation * coordSpace;
+        }
+
+        public override Vector3 Transform2SpaceAxisChange(Vector3 coordTransformed)
+        {
+            return coordTransformed;
+        }
+
+        public override Vector3 Space2TransformAxisChange(Vector3 coordSpace)
+        {
+            return coordSpace;
+        }
     }
 }

Assets/Scripts/Core/CoordinateSystems/NewScaleLeftTransform.cs

@@ -144,7 +144,7 @@ public bool IsSetToDropToSurfaceWithDepth
         }
 
         public CoordinateSpace CoordinateSpace { get; set; }
-        public AffineTransform Transform { get; set; }
+        public CoordinateTransform Transform { get; set; }
 
         public bool IsRightHanded
         {
@@ -351,6 +351,12 @@ public void MoveXYZByWorldSpaceDelta(Vector3 worldSpaceDelta, Action<Vector4> on
             }, Debug.LogError);
         }
 
+        /// <summary>
+        ///     Drive manipulator depth by a given delta in world space
+        /// </summary>
+        /// <param name="worldSpaceDelta">Distance to drive depth in world space coordinates</param>
+        /// <param name="onSuccessCallback">Action on success</param>
+        /// <param name="onErrorCallback">Action on error</param>
         public void MoveDepthByWorldSpaceDelta(float worldSpaceDelta, Action<bool> onSuccessCallback, Action<string>
             onErrorCallback = null)
         {
@@ -379,6 +385,18 @@ public void MoveDepthByWorldSpaceDelta(float worldSpaceDelta, Action<bool> onSuc
             }, Debug.LogError);
         }
 
+        /// <summary>
+        ///     Drive the manipulator back to the zero coordinate position
+        /// </summary>
+        /// <param name="onSuccessCallback">Action on success</param>
+        /// <param name="onErrorCallBack">Action on failure</param>
+        public void MoveBackToZeroCoordinate(Action<Vector4> onSuccessCallback, Action<string> onErrorCallBack)
+        {
+            // Send move command
+            CommunicationManager.Instance.GotoPos(ManipulatorID, ZeroCoordinateOffset, AUTOMATIC_MOVEMENT_SPEED,
+                onSuccessCallback, onErrorCallBack);
+        }
+
         #endregion
     }
 }

Assets/Scripts/TrajectoryPlanner/Probes/ManipulatorBehaviorController.cs

@@ -30,6 +30,18 @@ public void Initialize(EphysLinkSettings settingsMenu, string manipulatorID, str
             _manipulatorIdText.text = manipulatorID;
             UpdateLinkableProbeOptions();
 
+            // FIXME: Dependent on Manipulator Type. Should be standardized by Ephys Link.
+            // Show or hide handedness dropdown depending on manipulator type
+            if (type == "new_scale")
+            {
+                _handednessDropdown.value = 0;
+                _handednessGroup.SetActive(false);
+            }
+            else
+            {
+                _handednessGroup.SetActive(true);
+            }
+
             // FIXME: Dependent on Manipulator Type. Should be standardized by Ephys Link.
             // Apply handedness from memory or default to right handed, also pass along manipulator type
             if (_attachedProbe)
@@ -81,6 +93,12 @@ public void OnLinkedProbeSelectionChanged(int value)
                 // With values != 0, there definitely was an attached probe before
                 _attachedProbe.SetIsEphysLinkControlled(false, _manipulatorId, onSuccess: () =>
                 {
+                    // Disable keyboard control
+                    _attachedProbe.ProbeController.ManipulatorKeyboardControl = false;
+                    _enableManualControlToggle.SetIsOnWithoutNotify(false);
+
+
+                    // Remove probe from linked probes list
                     _ephysLinkSettings.LinkedProbes.Remove(_attachedProbe);
                     _attachedProbe = null;
 
@@ -93,7 +111,15 @@ public void OnLinkedProbeSelectionChanged(int value)
                 // Disconnect currently attached probe
                 if (_attachedProbe)
                     _attachedProbe.SetIsEphysLinkControlled(false,
-                        onSuccess: () => { _ephysLinkSettings.LinkedProbes.Remove(_attachedProbe); });
+                        onSuccess: () =>
+                        {
+                            // Disable keyboard control
+                            _attachedProbe.ProbeController.ManipulatorKeyboardControl = false;
+                            _enableManualControlToggle.SetIsOnWithoutNotify(false);
+
+                            // Remove probe from linked probes list
+                            _ephysLinkSettings.LinkedProbes.Remove(_attachedProbe);
+                        });
 
                 // Find the new probe and attach it
                 var selectedProbeUUID = _linkedProbeDropdown.options[value].text;
@@ -182,6 +208,54 @@ public void UpdateKeyboardControlState(bool state)
                     _attachedProbe.ProbeController.ManipulatorKeyboardControl = false;
                     Debug.LogError(err);
                 });
+
+            // Enable/disable return to zero coordinate button
+            _returnToZeroCoordinateButton.interactable = state;
+        }
+
+        /// <summary>
+        ///     Return manipulator back to zero coordinate
+        /// </summary>
+        public void ReturnToZeroCoordinate()
+        {
+            if (_returningToZeroCoordinate)
+            {
+                // Return in progress, should stop
+                _returnToZeroCoordinateButtonText.text = "Stopping...";
+
+                CommunicationManager.Instance.Stop(stopState =>
+                {
+                    // Update text
+                    _returnToZeroCoordinateButtonText.text =
+                        stopState ? "Return to Zero Coordinate" : "Failed to Stop, Try Again";
+
+                    // Re-enable keyboard control if stop was successful
+                    _attachedProbe.ProbeController.ManipulatorKeyboardControl = stopState;
+
+                    // Update flag
+                    _returningToZeroCoordinate = !stopState;
+                });
+            }
+            else
+            {
+                // Disable keyboard control
+                _attachedProbe.ProbeController.ManipulatorKeyboardControl = false;
+
+                // Set button text and update flag
+                _returnToZeroCoordinateButtonText.text = "Stop";
+                _returningToZeroCoordinate = true;
+
+                // Move manipulator back to zero coordinate
+                _attachedProbe.ManipulatorBehaviorController.MoveBackToZeroCoordinate(_ => PostMoveAction(),
+                    _ => PostMoveAction());
+
+                void PostMoveAction()
+                {
+                    _returnToZeroCoordinateButtonText.text = "Return to Zero Coordinate";
+                    _attachedProbe.ProbeController.ManipulatorKeyboardControl = true;
+                    _returningToZeroCoordinate = false;
+                }
+            }
         }
 
         #endregion
@@ -269,6 +343,7 @@ private void UpdateBrainSurfaceOffsetInputField(float brainSurfaceOffset)
         #region Components
 
         [SerializeField] private TMP_Text _manipulatorIdText;
+        [SerializeField] private GameObject _handednessGroup;
         [SerializeField] private Dropdown _handednessDropdown;
         [SerializeField] private Dropdown _linkedProbeDropdown;
 
@@ -278,7 +353,9 @@ private void UpdateBrainSurfaceOffsetInputField(float brainSurfaceOffset)
         [SerializeField] private InputField _zeroCoordinateZInputField;
         [SerializeField] private InputField _zeroCoordinateDInputField;
         [SerializeField] private InputField _brainSurfaceOffsetInputField;
-        [SerializeField] private Toggle _keyboardControlToggle;
+        [SerializeField] private Button _returnToZeroCoordinateButton;
+        [SerializeField] private Text _returnToZeroCoordinateButtonText;
+        [SerializeField] private Toggle _enableManualControlToggle;
 
         private ProbeManager _attachedProbe;
 
@@ -290,6 +367,8 @@ private void UpdateBrainSurfaceOffsetInputField(float brainSurfaceOffset)
         private string _manipulatorId;
         private string _type;
 
+        private bool _returningToZeroCoordinate;
+
         #endregion
     }
 }