EVJ2k0=b!VX9Ol5sZ?=rydJ%%v6v`eMV}}q<+2BnwJ)flbj_9B@`nW1u~2kV~8fy
z3w%OIf{|dEG+?AkCJkbUcV)D!9~Hq_w{@Us+;ho`T-c&jUxNQ`n;At3);pK9n`f>r~W3V
z%i({z31X@rGJ2!`{#!<^H7)DO0L_xoT2AA
NecAlepur1Qegm}va1Q_g
diff --git a/Assets/Scenes/TrajectoryPlanner.unity b/Assets/Scenes/TrajectoryPlanner.unity
index eb6f02e9..724fd7a6 100644
--- a/Assets/Scenes/TrajectoryPlanner.unity
+++ b/Assets/Scenes/TrajectoryPlanner.unity
@@ -783,6 +783,10 @@ PrefabInstance:
propertyPath: m_LocalEulerAnglesHint.z
value: 0
objectReference: {fileID: 0}
+ - target: {fileID: 5304039299580470460, guid: ee09154d7a7870a419882ff36080d765, type: 3}
+ propertyPath: _buildVersion
+ value: 1.5
+ objectReference: {fileID: 0}
m_RemovedComponents: []
m_RemovedGameObjects: []
m_AddedGameObjects: []
diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController.cs
index 3d205109..89e74f17 100644
--- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController.cs
+++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController.cs
@@ -345,104 +345,106 @@ private async void EchoPosition()
{
// Continue echoing position while enabled and there exists a probe controller.
while (enabled && _probeController)
- {
- // Get manipulator position.
- var positionResponse = await CommunicationManager.Instance.GetPosition(
- ManipulatorID
- );
+ await UpdateProbePositionFromManipulator();
+ }
- // Shortcut exit if there was an error.
- if (CommunicationManager.HasError(positionResponse.Error))
- return;
+ ///
+ /// Update the probe's position based on the manipulator's position.
+ ///
+ private async Awaitable UpdateProbePositionFromManipulator()
+ {
+ // Get manipulator position.
+ var positionResponse = await CommunicationManager.Instance.GetPosition(ManipulatorID);
- // Apply zero coordinate offset.
- var zeroCoordinateAdjustedManipulatorPosition =
- positionResponse.Position - ZeroCoordinateOffset;
+ // Shortcut exit if there was an error.
+ if (CommunicationManager.HasError(positionResponse.Error))
+ return;
- // Convert to coordinate space.
- var manipulatorSpacePosition = CoordinateTransform.T2U(
- zeroCoordinateAdjustedManipulatorPosition
- );
+ // Apply zero coordinate offset.
+ var zeroCoordinateAdjustedManipulatorPosition =
+ positionResponse.Position - ZeroCoordinateOffset;
- // Brain surface adjustment.
- var brainSurfaceAdjustment = float.IsNaN(BrainSurfaceOffset)
- ? 0
- : BrainSurfaceOffset;
- // Apply depth adjustment to manipulator position for non-3 axis manipulators.
- if (CoordinateTransform.Prefix != "3lhm")
- zeroCoordinateAdjustedManipulatorPosition.w += brainSurfaceAdjustment;
-
- // Convert to world space.
- var zeroCoordinateAdjustedWorldPosition = CoordinateSpace.Space2World(
- manipulatorSpacePosition
- );
+ // Convert to coordinate space.
+ var manipulatorSpacePosition = CoordinateTransform.T2U(
+ zeroCoordinateAdjustedManipulatorPosition
+ );
- // Set probe position (change axes to match probe).
- var transformedApmldv = BrainAtlasManager.World2T_Vector(
- zeroCoordinateAdjustedWorldPosition
- );
+ // Brain surface adjustment.
+ var brainSurfaceAdjustment = float.IsNaN(BrainSurfaceOffset) ? 0 : BrainSurfaceOffset;
+ // Apply depth adjustment to manipulator position for non-3 axis manipulators.
+ if (CoordinateTransform.Prefix != "3lhm")
+ zeroCoordinateAdjustedManipulatorPosition.w += brainSurfaceAdjustment;
- // Set probe position.
- // For 3-axis manipulators, use depth to adjust brain offset if applying offset on depth.
- if (CoordinateTransform.Prefix == "3lhm")
- _probeController.SetProbePosition(
- new Vector4(
- transformedApmldv.x,
- transformedApmldv.y,
- transformedApmldv.z,
- brainSurfaceAdjustment
- )
- );
- else
- _probeController.SetProbePosition(
- new Vector4(
- transformedApmldv.x,
- transformedApmldv.y,
- transformedApmldv.z,
- zeroCoordinateAdjustedManipulatorPosition.w
- )
- );
-
- // Don't log if the last position is the same.
- var positionDifference = _lastLoggedManipulatorPosition - positionResponse.Position;
- if (
- !(Mathf.Abs(positionDifference.x) > 0.0001)
- && !(Mathf.Abs(positionDifference.y) > 0.0001)
- && !(Mathf.Abs(positionDifference.z) > 0.0001)
- && !(Mathf.Abs(positionDifference.w) > 0.0001)
- )
- continue;
-
- // Log every 4 hz
- if (!(Time.time - _lastLoggedTime >= 0.25))
- continue;
-
- _lastLoggedTime = Time.time;
- var tipPos = _probeController.ProbeTipT.position;
-
- // ["ephys_link", Real time stamp, Manipulator ID, X, Y, Z, W, Phi, Theta, Spin, TipX, TipY, TipZ]
- OutputLog.Log(
- new[]
- {
- "ephys_link",
- DateTime.Now.ToString(CultureInfo.InvariantCulture),
- ManipulatorID,
- positionResponse.Position.x.ToString(CultureInfo.InvariantCulture),
- positionResponse.Position.y.ToString(CultureInfo.InvariantCulture),
- positionResponse.Position.z.ToString(CultureInfo.InvariantCulture),
- positionResponse.Position.w.ToString(CultureInfo.InvariantCulture),
- _probeController.Insertion.Yaw.ToString(CultureInfo.InvariantCulture),
- _probeController.Insertion.Pitch.ToString(CultureInfo.InvariantCulture),
- _probeController.Insertion.Roll.ToString(CultureInfo.InvariantCulture),
- tipPos.x.ToString(CultureInfo.InvariantCulture),
- tipPos.y.ToString(CultureInfo.InvariantCulture),
- tipPos.z.ToString(CultureInfo.InvariantCulture)
- }
+ // Convert to world space.
+ var zeroCoordinateAdjustedWorldPosition = CoordinateSpace.Space2World(
+ manipulatorSpacePosition
+ );
+
+ // Set probe position (change axes to match probe).
+ var transformedApmldv = BrainAtlasManager.World2T_Vector(
+ zeroCoordinateAdjustedWorldPosition
+ );
+
+ // Set probe position.
+ // For 3-axis manipulators, use depth to adjust brain offset if applying offset on depth.
+ if (CoordinateTransform.Prefix == "3lhm")
+ _probeController.SetProbePosition(
+ new Vector4(
+ transformedApmldv.x,
+ transformedApmldv.y,
+ transformedApmldv.z,
+ brainSurfaceAdjustment
+ )
+ );
+ else
+ _probeController.SetProbePosition(
+ new Vector4(
+ transformedApmldv.x,
+ transformedApmldv.y,
+ transformedApmldv.z,
+ zeroCoordinateAdjustedManipulatorPosition.w
+ )
);
- // Update last logged position
- _lastLoggedManipulatorPosition = positionResponse.Position;
- }
+ // Don't log if the last position is the same.
+ var positionDifference = _lastLoggedManipulatorPosition - positionResponse.Position;
+ if (
+ !(Mathf.Abs(positionDifference.x) > 0.0001)
+ && !(Mathf.Abs(positionDifference.y) > 0.0001)
+ && !(Mathf.Abs(positionDifference.z) > 0.0001)
+ && !(Mathf.Abs(positionDifference.w) > 0.0001)
+ )
+ return;
+
+ // Log every 4 hz
+ if (!(Time.time - _lastLoggedTime >= 0.25))
+ return;
+
+ _lastLoggedTime = Time.time;
+ var tipPos = _probeController.ProbeTipT.position;
+
+ // ["ephys_link", Real time stamp, Manipulator ID, X, Y, Z, W, Phi, Theta, Spin, TipX, TipY, TipZ]
+ OutputLog.Log(
+ new[]
+ {
+ "ephys_link",
+ DateTime.Now.ToString(CultureInfo.InvariantCulture),
+ ManipulatorID,
+ positionResponse.Position.x.ToString(CultureInfo.InvariantCulture),
+ positionResponse.Position.y.ToString(CultureInfo.InvariantCulture),
+ positionResponse.Position.z.ToString(CultureInfo.InvariantCulture),
+ positionResponse.Position.w.ToString(CultureInfo.InvariantCulture),
+ _probeController.Insertion.Yaw.ToString(CultureInfo.InvariantCulture),
+ _probeController.Insertion.Pitch.ToString(CultureInfo.InvariantCulture),
+ _probeController.Insertion.Roll.ToString(CultureInfo.InvariantCulture),
+ tipPos.x.ToString(CultureInfo.InvariantCulture),
+ tipPos.y.ToString(CultureInfo.InvariantCulture),
+ tipPos.z.ToString(CultureInfo.InvariantCulture)
+ }
+ );
+
+ // Update last logged position
+ _lastLoggedManipulatorPosition = positionResponse.Position;
}
#endregion
diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs
index acebe25d..0ded0617 100644
--- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs
+++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs
@@ -13,12 +13,12 @@ public partial class ManipulatorBehaviorController
#region Properties
///
- /// Record of the manipulator's depth coordinate at the Dura.
+ /// Record of the manipulator's depth at the Dura.
///
private float _duraDepth;
///
- /// Record of the probe's coordinate at the Dura.
+ /// AP, ML, DV coordinate of the Dura.
///
private Vector3 _duraCoordinate;
@@ -71,10 +71,10 @@ public async Awaitable ResetDuraOffset()
// Reset dura offset.
ComputeBrainSurfaceOffset();
- // Wait for computation to complete.
- await Awaitable.NextFrameAsync();
+ // Force update probe position.
+ await UpdateProbePositionFromManipulator();
- // Reset Dura offset.
+ // Save the Dura's position.
_duraDepth = positionResponse.Position.w;
_duraCoordinate = _probeController.Insertion.APMLDV;
diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs
index c7005280..21ab7cb9 100644
--- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs
+++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs
@@ -110,6 +110,9 @@ float drivePastDistance
> NEAR_TARGET_DISTANCE
)
{
+ print(
+ $"{ProbeAutomationStateManager.ProbeAutomationState}: Going to {targetDepth - NEAR_TARGET_DISTANCE}"
+ );
var driveToNearTargetResponse =
await CommunicationManager.Instance.SetDepth(
new SetDepthRequest(
@@ -119,6 +122,8 @@ await CommunicationManager.Instance.SetDepth(
)
);
+ print($"At {driveToNearTargetResponse.Depth}");
+
// Shortcut exit if there was an error.
if (CommunicationManager.HasError(driveToNearTargetResponse.Error))
return;
@@ -126,6 +131,9 @@ await CommunicationManager.Instance.SetDepth(
break;
case ProbeAutomationState.DrivingToPastTarget:
+ print(
+ $"{ProbeAutomationStateManager.ProbeAutomationState}: Going to {targetDepth + drivePastDistance}"
+ );
// Drive to past target.
var driveToPastTargetResponse =
await CommunicationManager.Instance.SetDepth(
@@ -136,11 +144,16 @@ await CommunicationManager.Instance.SetDepth(
)
);
+ print($"At {driveToPastTargetResponse.Depth}");
+
// Shortcut exit if there was an error.
if (CommunicationManager.HasError(driveToPastTargetResponse.Error))
return;
break;
case ProbeAutomationState.ReturningToTarget:
+ print(
+ $"{ProbeAutomationStateManager.ProbeAutomationState}: Going to {targetDepth}"
+ );
// Drive up to target.
var returnToTargetResponse = await CommunicationManager.Instance.SetDepth(
new SetDepthRequest(
@@ -150,6 +163,8 @@ await CommunicationManager.Instance.SetDepth(
)
);
+ print($"At {returnToTargetResponse.Depth}");
+
// Shortcut exit if there was an error.
if (CommunicationManager.HasError(returnToTargetResponse.Error))
return;
@@ -162,14 +177,11 @@ await CommunicationManager.Instance.SetDepth(
case ProbeAutomationState.AtNearTargetInsert:
case ProbeAutomationState.AtPastTarget:
case ProbeAutomationState.AtTarget:
- case ProbeAutomationState.ExitingToNearTarget:
- case ProbeAutomationState.AtNearTargetExit:
case ProbeAutomationState.ExitingToDura:
case ProbeAutomationState.AtDuraExit:
case ProbeAutomationState.ExitingToMargin:
case ProbeAutomationState.AtExitMargin:
case ProbeAutomationState.ExitingToTargetEntryCoordinate:
- case ProbeAutomationState.DrivingToBregma:
throw new InvalidOperationException(
$"Not a valid driving state: {ProbeAutomationStateManager.ProbeAutomationState}"
);
@@ -255,30 +267,6 @@ public async void Exit(ProbeManager targetInsertionProbeManager, float baseSpeed
// Handle exiting state.
switch (ProbeAutomationStateManager.ProbeAutomationState)
{
- case ProbeAutomationState.ExitingToNearTarget:
- // Exit to near target if not already there.
- if (
- GetCurrentDistanceToTarget(targetInsertionProbeManager)
- < NEAR_TARGET_DISTANCE
- )
- {
- var exitToNearTargetResponse =
- await CommunicationManager.Instance.SetDepth(
- new SetDepthRequest(
- ManipulatorID,
- targetDepth - NEAR_TARGET_DISTANCE,
- baseSpeed
- * EXIT_DRIVE_SPEED_MULTIPLIER
- * NEAR_TARGET_SPEED_MULTIPLIER
- )
- );
-
- // Shortcut exit if there was an error.
- if (CommunicationManager.HasError(exitToNearTargetResponse.Error))
- return;
- }
-
- break;
case ProbeAutomationState.ExitingToDura:
// Exit back up to the Dura.
var exitToDuraResponse = await CommunicationManager.Instance.SetDepth(
@@ -344,10 +332,8 @@ await CommunicationManager.Instance.SetPosition(
case ProbeAutomationState.AtPastTarget:
case ProbeAutomationState.ReturningToTarget:
case ProbeAutomationState.AtTarget:
- case ProbeAutomationState.AtNearTargetExit:
case ProbeAutomationState.AtDuraExit:
case ProbeAutomationState.AtExitMargin:
- case ProbeAutomationState.DrivingToBregma:
throw new InvalidOperationException(
$"Not a valid exit state: {ProbeAutomationStateManager.ProbeAutomationState}"
);
@@ -460,7 +446,7 @@ private float GetTargetDistanceToDura(ProbeManager targetInsertionProbeManager)
/// Compute the current distance to the target insertion.
///