Skip to content

chore: 731 skip return to near target state #733

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
kjy5 merged 5 commits into from
Sep 8, 2024
Merged

chore: 731 skip return to near target state #733

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
c8a94b6
Skip return to near target state, binary addressables
kennethjyang Sep 7, 2024
0d53921
Update BrainAtlas
kennethjyang Sep 7, 2024
f05ad55
New addressables 2.0 build
kennethjyang Sep 7, 2024
9e9dd42
Exits without near target state
kennethjyang Sep 7, 2024
aea37a6
Fixed Depth movement
kennethjyang Sep 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 3 additions & 4 deletions Assets/AddressableAssetsData/AddressableAssetSettings.asset
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,9 @@ MonoBehaviour:
m_DefaultGroup: eb9eb3c822ec72b468349d4e785d6617
m_currentHash:
serializedVersion: 2
Hash: 00000000000000000000000000000000
Hash: b3af3a77ece41351bcf4459c8498ca0f
m_OptimizeCatalogSize: 0
m_BuildRemoteCatalog: 1
m_BundleLocalCatalog: 0
m_CatalogRequestsTimeout: 0
m_DisableCatalogUpdateOnStart: 0
m_InternalIdNamingMode: 0
Expand All @@ -32,7 +31,7 @@ MonoBehaviour:
m_ClassName: UnityEngine.ResourceManagement.ResourceProviders.AssetBundleProvider
m_IgnoreUnsupportedFilesInBuild: 0
m_UniqueBundleIds: 0
m_EnableJsonCatalog: 1
m_EnableJsonCatalog: 0
m_NonRecursiveBuilding: 0
m_CCDEnabled: 0
m_maxConcurrentWebRequests: 500
Expand Down Expand Up @@ -81,7 +80,7 @@ MonoBehaviour:
- m_Id: 6e9217891ed892f40b1933afda7ddded
m_Value: '{UnityEngine.AddressableAssets.Addressables.RuntimePath}/[BuildTarget]'
- m_Id: 8b0e868bf8565ef4ba5a95e7d57f666c
m_Value: 'https://data.virtualbrainlab.org/PinpointData/1.0.3/[BuildTarget]'
m_Value: 'https://data.virtualbrainlab.org/PinpointData/2.0.0/[BuildTarget]'
- m_Id: b28712faecae3244489993d74caf0ba7
m_Value: '[UnityEditor.EditorUserBuildSettings.activeBuildTarget]'
m_ProfileEntryNames:
Expand Down
Binary file modified Assets/AddressableAssetsData/WebGL/addressables_content_state.bin
View file
Open in desktop
Binary file not shown.
Binary file modified Assets/AddressableAssetsData/Windows/addressables_content_state.bin
View file
Open in desktop
Binary file not shown.
4 changes: 4 additions & 0 deletions Assets/Scenes/TrajectoryPlanner.unity
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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: []
Expand Down
182 changes: 92 additions & 90 deletions ...ts/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
/// <summary>
/// Update the probe's position based on the manipulator's position.
/// </summary>
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
Expand Down
10 changes: 5 additions & 5 deletions ...ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,12 @@ public partial class ManipulatorBehaviorController
#region Properties

/// <summary>
/// Record of the manipulator's depth coordinate at the Dura.
/// Record of the manipulator's depth at the Dura.
/// </summary>
private float _duraDepth;

/// <summary>
/// Record of the probe's coordinate at the Dura.
/// AP, ML, DV coordinate of the Dura.
/// </summary>
private Vector3 _duraCoordinate;

Expand Down Expand Up @@ -71,10 +71,10 @@ public async Awaitable<bool> 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;

Expand Down
46 changes: 16 additions & 30 deletions ...robes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -119,13 +122,18 @@ await CommunicationManager.Instance.SetDepth(
)
);

print($"At {driveToNearTargetResponse.Depth}");

// Shortcut exit if there was an error.
if (CommunicationManager.HasError(driveToNearTargetResponse.Error))
return;
}

break;
case ProbeAutomationState.DrivingToPastTarget:
print(
$"{ProbeAutomationStateManager.ProbeAutomationState}: Going to {targetDepth + drivePastDistance}"
);
// Drive to past target.
var driveToPastTargetResponse =
await CommunicationManager.Instance.SetDepth(
Expand All @@ -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(
Expand All @@ -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;
Expand All @@ -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}"
);
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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}"
);
Expand Down Expand Up @@ -460,7 +446,7 @@ private float GetTargetDistanceToDura(ProbeManager targetInsertionProbeManager)
/// Compute the current distance to the target insertion.
/// </summary>
/// <param name="targetInsertionProbeManager"></param>
/// <returns>Distance in mm to the target from the probe.</returns>
/// <returns>Distance in mm to the target from the probe. NaN on error.</returns>
private float GetCurrentDistanceToTarget(ProbeManager targetInsertionProbeManager)
{
return Vector3.Distance(
Expand Down
Loading